Improvement on rigid body rotation

src/main/java/electrosphere/engine/LoadingThread.java

@@ -5,6 +5,8 @@ import electrosphere.controls.ControlHandler;
 import electrosphere.entity.Entity;
 import electrosphere.entity.EntityDataStrings;
 import electrosphere.entity.EntityUtils;
+import electrosphere.entity.state.BehaviorTree;
+import electrosphere.entity.state.collidable.Impulse;
 import electrosphere.entity.state.movement.ApplyRotationTree;
 import electrosphere.game.collision.CollisionEngine;
 import electrosphere.entity.types.creature.CreatureUtils;
@@ -679,6 +681,15 @@ public class LoadingThread extends Thread {
         Entity sword = ItemUtils.spawnBasicItem("Katana");
         EntityUtils.getPosition(sword).set(new Vector3f(1,0.4f,2));
         EntityUtils.getRotation(sword).set(new Quaternionf().rotationY(0.6f));
+        // Globals.entityManager.registerBehaviorTree(new BehaviorTree() {
+        //     int i = 0;
+        //     public void simulate(){
+        //         if(i < 100){
+        //             i++;
+        //             CollisionObjUtils.getCollidable(sword).addImpulse(new Impulse(new Vector3d(0,0,1), new Vector3d(-1,0,0), 0.001, Collidable.TYPE_CREATURE));
+        //             EntityUtils.getPosition(sword).set(1,0.2f,2);
+        //         }
+        // }});
 
         // Entity shorts = ItemUtils.spawnBasicItem("boots1");
         // EntityUtils.getPosition(shorts).set(new Vector3f(2,0.1f,2));

src/main/java/electrosphere/entity/state/collidable/CollidableTree.java

@@ -29,7 +29,7 @@ public class CollidableTree {
     Entity parent;
     CollisionObject body;
     Collidable collidable;
-    // Quaternionf angularVelocity = new Quaternionf(0,0,0,1);
+    Quaternionf angularVelocity = new Quaternionf(0,0,0,0);
     Vector4d cumulativeTorque = new Vector4d(0,0,0,0);
     
     
@@ -77,13 +77,14 @@ public class CollidableTree {
                 !Double.isNaN(impulse.getCollisionPoint().z) && 
                 !Double.isNaN(impulse.getDirection().x) &&
                 !Double.isNaN(impulse.getDirection().y) &&
-                !Double.isNaN(impulse.getDirection().z)
+                !Double.isNaN(impulse.getDirection().z) &&
+                ItemUtils.isItem(parent)
             ){
                 Vector3d collisionPoint = new Vector3d(impulse.getCollisionPoint()).normalize();
-                Vector3d forceDir = new Vector3d(impulse.getDirection()).normalize();
+                Vector3d forceDir = new Vector3d(impulse.getDirection().mul(-1.0f)).normalize();
                 Vector3d torqueVec = new Vector3d(collisionPoint).cross(forceDir).normalize();
-                double torqueMag = Math.abs(impulse.force);
+                // double torqueMag = Math.abs(impulse.force);
-                // double torqueMag = impulse.getCollisionPoint().length() * impulse.getDirection().length();
+                double torqueMag = impulse.getCollisionPoint().length() * impulse.getDirection().length() * impulse.getForce();
                 // if(impulse.type.matches(Collidable.TYPE_ITEM) && ItemUtils.isItem(parent)){
                 //     // System.out.println(rotation);
                 //     if(impulse.collisionPoint.x < 0){
@@ -97,7 +98,17 @@ public class CollidableTree {
                 // if(impulse.type.matches(Collidable.TYPE_ITEM)){
                 //     System.out.println("Impulse: " + torqueVec + " " + torqueMag);
                 // }
-                cumulativeTorque.add(new Vector4d(torqueVec.x,torqueVec.y,torqueVec.z,torqueMag));
+                if(CreatureUtils.isCreature(parent) && forceDir.y < 0.5){
+                    System.out.println(collisionPoint + " x " + forceDir + " => " + torqueVec + " " + torqueMag);
+                }
+                Quaternionf impulseRotation = new Quaternionf().rotationAxis((float)torqueMag,new Vector3f((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z));
+                if(angularVelocity.lengthSquared() > 0.001){
+                    angularVelocity.slerp(impulseRotation, (float)(torqueMag / Math.abs(angularVelocity.w)));
+                } else {
+                    angularVelocity.set(impulseRotation);
+                }
+                // angularVelocity.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
+                // cumulativeTorque.add(new Vector4d((float)torqueVec.x,(float)torqueVec.y,(float)torqueVec.z,(float)torqueMag));
             }
             offsetVector.add(impulseForce);
         }
@@ -105,25 +116,46 @@ public class CollidableTree {
         //     System.out.println(cumulativeTorque);
         // }
         //friction
-        // if(cumulativeTorque.w > 0.001){
+        if(angularVelocity.lengthSquared() > 0.001){
-        //     Vector3d normalizedTorqueDir = new Vector3d(cumulativeTorque.x,cumulativeTorque.y,cumulativeTorque.z).normalize();
+            angularVelocity.scale((float)(Math.sqrt(angularVelocity.lengthSquared()) * 0.9));
-        //     double newMag = cumulativeTorque.w * 0.9;
+            System.out.println(angularVelocity);
-        //     cumulativeTorque.set(normalizedTorqueDir.x,normalizedTorqueDir.y,normalizedTorqueDir.z,newMag);
-        // }
-        if(inverseInertiaTensor != null && cumulativeTorque.w > 0.01){
-            Vector4f angularMomentum = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.w));
-            // Quaternionf nextRotation = new Quaternionf(rotation).mul(new Quaternionf(angularMomentum.x,angularMomentum.y,angularMomentum.z,angularMomentum.w).scale(0.001f)).normalize();
-            // rotation = nextRotation;
-            // rotation.mul(new Quaternionf((float)angularMomentum.x,(float)angularMomentum.y,(float)angularMomentum.z,(float)angularMomentum.w / 0.01f));
-            if(ItemUtils.isItem(parent)){
-                System.out.println("cumulative quat: " + cumulativeTorque);
-            }
-            rotation.x = rotation.x + rotation.x * (float)cumulativeTorque.x * 0.001f;
-            rotation.y = rotation.y + rotation.y * (float)cumulativeTorque.y * 0.001f;
-            rotation.z = rotation.z + rotation.z * (float)cumulativeTorque.z * 0.001f;
-            rotation.w = rotation.w + rotation.w * (float)cumulativeTorque.w * 0.001f;
-            rotation.normalize();
         }
+        // if(cumulativeTorque.w > 0.001){
+        //     Vector4f holder = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.y,(float)cumulativeTorque.z,(float)cumulativeTorque.w));
+        //     cumulativeTorque = new Vector4d(holder.x,holder.y,holder.z,holder.w);
+        //     angularVelocity = angularVelocity.add(cumulativeTorque).normalize();
+        //     cumulativeTorque.set(0,0,0,0);
+        // //     Vector3d normalizedTorqueDir = new Vector3d(cumulativeTorque.x,cumulativeTorque.y,cumulativeTorque.z).normalize();
+        // //     double newMag = cumulativeTorque.w * 0.9;
+        // //     cumulativeTorque.set(normalizedTorqueDir.x,normalizedTorqueDir.y,normalizedTorqueDir.z,newMag);
+        // }
+        if(angularVelocity.lengthSquared() > 0.001){
+            // System.out.println("-" + rotation);
+            Quaternionf newRotation = new Quaternionf(rotation).mul(angularVelocity).normalize();
+            // if(new Quaternionf(newRotation).add(new Quaternionf(rotation).conjugate()).lengthSquared() > 0.2){
+            //     newRotation.w = Math.copySign(newRotation.w, rotation.w);
+            //     newRotation.x = Math.copySign(newRotation.x, rotation.x);
+            //     newRotation.y = Math.copySign(newRotation.y, rotation.y);
+            //     newRotation.z = Math.copySign(newRotation.z, rotation.z);
+            // }
+            rotation.set(newRotation);
+            // System.out.println("=" + rotation);
+        }
+        // if(inverseInertiaTensor != null && angularVelocity.w > 0.01){
+        //     // Vector4f angularMomentum = inverseInertiaTensor.transform(new Vector4f((float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.x,(float)cumulativeTorque.w));
+        //     // Quaternionf nextRotation = new Quaternionf(rotation).mul(new Quaternionf(angularMomentum.x,angularMomentum.y,angularMomentum.z,angularMomentum.w).scale(0.001f)).normalize();
+        //     // rotation = nextRotation;
+        //     // rotation.mul(new Quaternionf((float)angularMomentum.x,(float)angularMomentum.y,(float)angularMomentum.z,(float)angularMomentum.w / 0.01f));
+        //     // if(ItemUtils.isItem(parent)){
+        //     //     System.out.println("cumulative quat: " + cumulativeTorque);
+        //     // }
+        //     rotation.x = rotation.x + rotation.x * (float)(angularVelocity.x * angularVelocity.w) * 0.01f;
+        //     rotation.y = rotation.y + rotation.y * (float)(angularVelocity.y * angularVelocity.w) * 0.01f;
+        //     rotation.z = rotation.z + rotation.z * (float)(angularVelocity.z * angularVelocity.w) * 0.01f;
+        //     rotation.w = 1;
+        //     // rotation.w = rotation.w + rotation.w * (float)cumulativeTorque.w * 0.001f;
+        //     rotation.normalize();
+        // }
         
         //the reasoning here is that if we have something above something else and it's pushing it into the terrain,
         //we should instead just not push into terrain and push along terrain

src/main/java/electrosphere/game/collision/CollisionEngine.java

@@ -106,8 +106,8 @@ public class CollisionEngine {
                         }
                     }
                     if (hit) {
-                        resolveCollision(physicsObject1,physicsObject2, normal, localPosition1, magnitude);
+                        resolveCollision(physicsObject1,physicsObject2, normal, localPosition2, magnitude);
-                        resolveCollision(physicsObject2,physicsObject1, new Vector3d(normal).mul(-1.0), localPosition2, magnitude);
+                        resolveCollision(physicsObject2,physicsObject1, new Vector3d(normal).mul(-1.0), localPosition1, magnitude);
 //                        System.out.println("HIT + " + normal);
                         // Collision happened between physicsObject1 and physicsObject2. Collision normal is in variable 'normal'.
                     }
@@ -146,7 +146,7 @@ public class CollisionEngine {
 //                        System.out.println("Structure-creature collision");
                         break;
                     case Collidable.TYPE_ITEM:
-                        receiver.addImpulse(new Impulse(normal, localPosition, magnitude * 0.5, Collidable.TYPE_ITEM));
+                        receiver.addImpulse(new Impulse(normal, localPosition, magnitude, Collidable.TYPE_ITEM));
                         break;
                 }
                 break;
@@ -172,7 +172,7 @@ public class CollisionEngine {
 //                        System.out.println("Structure-creature collision");
                         break;
                     case Collidable.TYPE_ITEM:
-                        receiver.addImpulse(new Impulse(normal, localPosition, magnitude * 0.5, Collidable.TYPE_ITEM));
+                        receiver.addImpulse(new Impulse(normal, localPosition, magnitude, Collidable.TYPE_ITEM));
                         break;
                 }
                 break;

src/main/java/electrosphere/net/server/ServerConnectionHandler.java

@@ -105,6 +105,7 @@ public class ServerConnectionHandler implements Runnable {
         newPlayerObject.setPlayerEntity(newPlayerCharacter);
         newPlayerObject.setWorldX(Globals.serverWorldData.convertRealToChunkSpace(Globals.spawnPoint.x));
         newPlayerObject.setWorldY(Globals.serverWorldData.convertRealToChunkSpace(Globals.spawnPoint.z));
+        System.out.println(EntityUtils.getRotation(newPlayerCharacter).set(0,1,0,1).normalize());
         Globals.dataCellManager.addPlayer(newPlayerObject);
         Globals.dataCellManager.movePlayer(newPlayerObject, Globals.serverWorldData.convertRealToChunkSpace(Globals.spawnPoint.x), Globals.serverWorldData.convertRealToChunkSpace(Globals.spawnPoint.z));
         IronSightTree.attachIronSightTree(newPlayerCharacter);

src/main/java/electrosphere/util/Utilities.java

@@ -25,6 +25,7 @@ import java.util.concurrent.TimeUnit;
 import java.util.logging.Level;
 import java.util.logging.Logger;
 import org.joml.Matrix4f;
+import org.joml.Quaternionf;
 import org.joml.Vector3f;
 import org.lwjgl.BufferUtils;
 import org.lwjgl.assimp.AIMatrix4x4;