floating origin for CollisionEngine.java

docs/src/progress/renderertodo.md

@@ -1301,6 +1301,8 @@ Save assembly on compile
 Delete old server physics cell on terrain edit
 Fix server terrain physics entity positioning
 Disable client fluid draw cell loading gate
+Properly differentiate local/world bone attach point calculation
+Floating origin implementation for collision engine
 
 
 

src/main/java/electrosphere/collision/CollisionBodyCreation.java

@@ -212,7 +212,7 @@ public class CollisionBodyCreation {
     }
 
     /**
-     * Sets the offset position of the first geometry in a given body
+     * Sets the offset position of the first geometry in a given body (local to the parent)
      * @param collisionEngine The collision engine
      * @param body The body
      * @param offsetPosition The position to offset the first geometry by

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

@@ -116,6 +116,13 @@ public class CollisionEngine {
     //The list of all collidables the engine is currently tracking
     List<Collidable> collidableList = new ArrayList<Collidable>();
 
+    /**
+     * Dynamic spatial offset applied to all operations on the space.
+     * This is used for world origin rebasing.
+     * This makes physics behave more consistently by moving far out objects to center around 0,0,0.
+     */
+    Vector3d floatingOrigin = new Vector3d(0,0,0);
+
     //callbacks for collision check
     RayCastCallback rayCastCallback = new RayCastCallback();
 
@@ -530,7 +537,7 @@ public class CollisionEngine {
                     Entity physicsEntity = collidable.getParent();
                     DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                     Matrix4d inverseTransform = new Matrix4d();
-                    Vector4d rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.getRigidBodyPosition(rigidBody),1));
+                    Vector4d rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin),1));
                     Vector3d calculatedPosition = new Vector3d(rawPos.x,rawPos.y,rawPos.z);
                     Vector3d suggestedPosition = this.suggestMovementPosition(collisionWorldData, calculatedPosition);
                     if(calculatedPosition.distance(suggestedPosition) > 0){
@@ -545,6 +552,48 @@ public class CollisionEngine {
         spaceLock.unlock();
         Globals.profiler.endCpuSample();
     }
+
+    /**
+     * Rebases the world origin
+     */
+    public void rebaseWorldOrigin(){
+        Globals.profiler.beginCpuSample("rebaseWorldOrigin");
+        spaceLock.lock();
+        if(this.collisionWorldData != null && this.floatingOrigin.x == 0 && this.floatingOrigin.z == 0){
+            int collected = 0;
+            Vector3d newOrigin = new Vector3d();
+            //calculate new origin
+            for(Collidable collidable : collidableList){
+                Entity physicsEntity = collidable.getParent();
+                DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
+                if(rigidBody != null){
+                    Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
+                    if(collected == 0){
+                        newOrigin.set(currentBodyOffset);
+                    } else {
+                        float percentExisting = collected / (float)(collected + 1);
+                        float percentNew = 1.0f - percentExisting;
+                        newOrigin = newOrigin.mul(percentExisting).add(currentBodyOffset.mul(percentNew));
+                    }
+                    collected++;
+                }
+            }
+            Vector3d delta = new Vector3d(this.floatingOrigin).sub(newOrigin);
+            this.floatingOrigin = this.floatingOrigin.set(newOrigin);
+            //apply new origin to all geoms
+            //calculate new origin
+            for(Collidable collidable : collidableList){
+                Entity physicsEntity = collidable.getParent();
+                DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
+                if(rigidBody != null){
+                    Vector3d existingPosition = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
+                    rigidBody.setPosition(PhysicsUtils.jomlVecToOdeVec(existingPosition.add(delta)));
+                }
+            }
+        }
+        spaceLock.unlock();
+        Globals.profiler.endCpuSample();
+    }
     
     /**
      * Registers a collision object with the server
@@ -573,7 +622,7 @@ public class CollisionEngine {
     public void listBodyPositions(){
         for(DBody body : bodies){
             LoggerInterface.loggerEngine.INFO("" + body);
-            LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.getRigidBodyPosition(body));
+            LoggerInterface.loggerEngine.INFO("" + PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin));
         }
     }
     
@@ -914,7 +963,7 @@ public class CollisionEngine {
      */
     protected void setBodyTransform(DBody body, Vector3d position, Quaterniond rotation){
         spaceLock.lock();
-        body.setPosition(position.x, position.y, position.z);
+        body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
         body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
         spaceLock.unlock();
     }
@@ -932,7 +981,7 @@ public class CollisionEngine {
     protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
         if(body != null){
             spaceLock.lock();
-            body.setPosition(position.x, position.y, position.z);
+            body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
             body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
             body.setLinearVel(PhysicsUtils.jomlVecToOdeVec(linearVel));
             body.setAngularVel(PhysicsUtils.jomlVecToOdeVec(angularVel));
@@ -951,7 +1000,7 @@ public class CollisionEngine {
      */
     protected void setBodyTransform(DBody body, CollidableTemplate template, Vector3d position, Quaterniond rotation, Vector3d scale){
         spaceLock.lock();
-        body.setPosition(position.x, position.y, position.z);
+        body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
         body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
         DGeom firstGeom = body.getFirstGeom();
         if(firstGeom instanceof DCylinder){
@@ -965,14 +1014,14 @@ public class CollisionEngine {
     }
 
     /**
-     * Sets the transform of a geometry
+     * Sets the transform of a geometry (local to the parent)
      * @param geom The geometry
      * @param position The position
      * @param rotation The rotation
      */
     protected void setGeomTransform(DGeom geom, Vector3d position, Quaterniond rotation){
         spaceLock.lock();
-        geom.setOffsetWorldPosition(position.x, position.y, position.z);
+        geom.setOffsetPosition(position.x, position.y, position.z);
         geom.setOffsetQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
         spaceLock.unlock();
     }
@@ -995,7 +1044,7 @@ public class CollisionEngine {
     protected Vector3d getBodyPosition(DBody body){
         Vector3d rVal = null;
         spaceLock.lock();
-        rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition());
+        rVal = PhysicsUtils.odeVecToJomlVec(body.getPosition()).add(this.floatingOrigin);
         spaceLock.unlock();
         return rVal;
     }

src/main/java/electrosphere/collision/PhysicsUtils.java

@@ -15,15 +15,6 @@ import electrosphere.game.data.collidable.CollidableTemplate;
  */
 public class PhysicsUtils {
     
-    /**
-     * Gets the current position of a rigid body as a joml vector
-     * @param body The dbody
-     * @return The position
-     */
-    public static Vector3d getRigidBodyPosition(DBody body){
-        return odeVecToJomlVec(body.getPosition());
-    }
-    
     /**
      * Gets the rotation of an ode body as a joml quaternion
      * @param body

src/main/java/electrosphere/entity/state/attach/AttachUtils.java

@@ -294,7 +294,7 @@ public class AttachUtils {
         Vector3d parentScale
     ){
         //transform bone space
-        Vector3d position = AttachUtils.calculateBoneAttachmentPosition(
+        Vector3d position = AttachUtils.calculateBoneAttachmentWorldPosition(
             offsetVector,
             offsetRotation,
             bonePosition,
@@ -331,7 +331,7 @@ public class AttachUtils {
      * @param parentScale The parent's scale
      * @return The position of the attached/child entity
      */
-    public static Vector3d calculateBoneAttachmentPosition(
+    public static Vector3d calculateBoneAttachmentLocalPosition(
         //optional offsets
         Vector3d offsetVector,
         Quaterniond offsetRotation,
@@ -352,6 +352,39 @@ public class AttachUtils {
         position = position.mul(parentScale);
         position = position.rotate(new Quaterniond(parentRotation));
         //transform worldspace
+        // position.add(parentPosition);
+
+        return position;
+    }
+
+    /**
+     * Calculates the position of an entity attached to a bone
+     * @param offsetVector The offset position
+     * @param offsetRotation The offset rotation
+     * @param bonePosition The bone's position
+     * @param boneRotation The bone's rotation
+     * @param parentPosition The parent's position
+     * @param parentRotation The parent's rotation
+     * @param parentScale The parent's scale
+     * @return The position of the attached/child entity
+     */
+    public static Vector3d calculateBoneAttachmentWorldPosition(
+        //optional offsets
+        Vector3d offsetVector,
+        Quaterniond offsetRotation,
+
+        //current bone transform
+        Vector3d bonePosition,
+        Quaterniond boneRotation,
+
+        //parent transforms
+        Vector3d parentPosition,
+        Quaterniond parentRotation,
+        Vector3d parentScale
+    ){
+        //transform bone space
+        Vector3d position = calculateBoneAttachmentLocalPosition(offsetVector, offsetRotation, bonePosition, boneRotation, parentPosition, parentRotation, parentScale);
+        //transform worldspace
         position.add(parentPosition);
 
         return position;

src/main/java/electrosphere/entity/state/hitbox/HitboxCollectionState.java

@@ -253,7 +253,7 @@ public class HitboxCollectionState {
         if(parent != null && !isServer && EntityUtils.getActor(parent) != null){
             if(!this.geoms.isEmpty()){
                 Vector3d entityPosition = EntityUtils.getPosition(parent);
-                this.body.setPosition(PhysicsUtils.jomlVecToOdeVec(entityPosition));
+                PhysicsUtils.setRigidBodyTransform(collisionEngine, entityPosition, new Quaterniond(), body);
                 for(String boneName : this.boneHitboxMap.keySet()){
                     if(EntityUtils.getActor(parent).containsBone(boneName)){
                         Vector3d bonePosition = EntityUtils.getActor(parent).getBonePosition(boneName);
@@ -293,7 +293,7 @@ public class HitboxCollectionState {
         } else if(parent != null && isServer && EntityUtils.getPoseActor(parent) != null){
             if(!this.geoms.isEmpty()){
                 Vector3d entityPosition = EntityUtils.getPosition(parent);
-                this.body.setPosition(PhysicsUtils.jomlVecToOdeVec(entityPosition));
+                PhysicsUtils.setRigidBodyTransform(collisionEngine, entityPosition, new Quaterniond(), body);
                 //
                 for(String boneName : this.boneHitboxMap.keySet()){
                     if(EntityUtils.getPoseActor(parent).containsBone(boneName)){
@@ -358,8 +358,7 @@ public class HitboxCollectionState {
      * @param bonePosition The position of the bone
      */
     private void updateStaticCapsulePosition(CollisionEngine collisionEngine, DGeom geom, HitboxState shapeStatus){
-        Vector3d parentPos = EntityUtils.getPosition(parent);
+        PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, new Vector3d(), new Quaterniond(0.707,0,0,0.707));
-        PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, parentPos, new Quaterniond(0.707,0,0,0.707));
     }
 
     /**
@@ -388,7 +387,7 @@ public class HitboxCollectionState {
         Vector3d parentScale = new Vector3d(EntityUtils.getScale(parent));
 
         //calculate
-        Vector3d hitboxPos = AttachUtils.calculateBoneAttachmentPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale);
+        Vector3d hitboxPos = AttachUtils.calculateBoneAttachmentLocalPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale);
 
 
         PhysicsEntityUtils.setGeometryPosition(collisionEngine, geom, hitboxPos, new Quaterniond());
@@ -425,7 +424,7 @@ public class HitboxCollectionState {
         Vector3d parentScale = new Vector3d(EntityUtils.getScale(parent));
 
         //calculate
-        Vector3d worldPosition = AttachUtils.calculateBoneAttachmentPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale);
+        Vector3d worldPosition = AttachUtils.calculateBoneAttachmentLocalPosition(offsetPosition, offsetRotation, bonePositionD, boneRotation, parentPosition, parentRotation, parentScale);
         Quaterniond worldRotation = new Quaterniond();
 
         if(previousWorldPos != null){

src/main/java/electrosphere/server/datacell/GriddedDataCellManager.java

@@ -537,10 +537,12 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
             cellPlayerlessFrameMap.put(groundDataCells.get(this.getServerDataCellKey(worldPos)),0);
             loadedCellsLock.unlock();
         } else if(groundDataCells.get(this.getServerDataCellKey(worldPos)) == null) {
-            LoggerInterface.loggerEngine.WARNING(
+            LoggerInterface.loggerEngine.ERROR(
-                "Trying to create data cell outside world bounds!\n" +
+                new Error(
-                worldPos + "\n" +
+                    "Trying to create data cell outside world bounds!\n" +
-                this.serverWorldData.getWorldSizeDiscrete()
+                    worldPos + "\n" +
+                    this.serverWorldData.getWorldSizeDiscrete()
+                )
             );
         }
         return groundDataCells.get(this.getServerDataCellKey(worldPos));

src/main/java/electrosphere/server/datacell/Realm.java

@@ -218,6 +218,10 @@ public class Realm {
         collisionEngine.clearCollidableImpulseLists();
         chemistryEngine.clearCollidableImpulseLists();
         
+        //
+        //rebase physics origin
+        this.collisionEngine.rebaseWorldOrigin();
+        
         Globals.profiler.endCpuSample();
     }