client collidable LOD work

docs/src/progress/renderertodo.md

@@ -2100,6 +2100,17 @@ Actually fix pathing spinlock
 Support for observing ai entities
 ServerGroundMovementTree supports collidable entities
 ServerGroundMovementTree geom work
+ServerLODComponent replaces bodies with geometries instead of just destroying the geometries
+
+
+(06/01/2025 - 06/04/2025)
+Fix rebase world origin routine
+Non-rigid-body collidables behave as expected
+Fix physics performance issues
+
+(06/04/2025)
+ServerGroundMovementTree actually moves collidable-based entities
+Client uses non-rigid-body collidables for farther away entities (via client LOD tree)
 
 
 

src/main/java/electrosphere/client/ClientState.java

@@ -16,6 +16,7 @@ import electrosphere.client.terrain.cells.ClientDrawCellManager;
 import electrosphere.client.terrain.foliage.FoliageCellManager;
 import electrosphere.client.terrain.manager.ClientTerrainManager;
 import electrosphere.collision.CollisionEngine;
+import electrosphere.collision.PhysicsCallback;
 import electrosphere.data.entity.common.CommonEntityType;
 import electrosphere.data.voxel.VoxelType;
 import electrosphere.engine.Globals;
@@ -189,7 +190,7 @@ public class ClientState {
      * Constructor
      */
     public ClientState(){
-        this.clientSceneWrapper = new ClientSceneWrapper(this.clientScene, new CollisionEngine(), CollisionEngine.create(new ClientChemistryCollisionCallback()), new CollisionEngine());
+        this.clientSceneWrapper = new ClientSceneWrapper(this.clientScene, CollisionEngine.create("clientPhysics", new PhysicsCallback()), CollisionEngine.create("clientChem", new ClientChemistryCollisionCallback()), new CollisionEngine("clientInteraction"));
         this.clientTemporalService = (ClientTemporalService)Globals.engineState.serviceManager.registerService(new ClientTemporalService());
     }
 

src/main/java/electrosphere/client/interact/ClientInteractionEngine.java

@@ -7,13 +7,9 @@ import java.util.concurrent.locks.ReentrantLock;
 import org.joml.Matrix4d;
 import org.joml.Quaterniond;
 import org.joml.Vector3d;
-import org.joml.Vector3i;
 import org.ode4j.ode.DBody;
 
-import electrosphere.client.block.BlockChunkData;
 import electrosphere.client.entity.camera.CameraEntityUtils;
-import electrosphere.client.scene.ClientWorldData;
-import electrosphere.client.terrain.cache.ChunkData;
 import electrosphere.client.ui.menu.ingame.InteractionTargetMenu;
 import electrosphere.collision.CollisionBodyCreation;
 import electrosphere.collision.CollisionEngine;
@@ -24,9 +20,7 @@ import electrosphere.engine.Globals;
 import electrosphere.entity.Entity;
 import electrosphere.entity.EntityDataStrings;
 import electrosphere.entity.EntityUtils;
-import electrosphere.entity.types.EntityTypes.EntityType;
 import electrosphere.entity.types.common.CommonEntityUtils;
-import electrosphere.server.physics.terrain.manager.ServerTerrainChunk;
 
 /**
  * Manages the interaction state 
@@ -90,7 +84,7 @@ public class ClientInteractionEngine {
                 rVal.putData(EntityDataStrings.INTERACTION_COLLIDABLE, collidable);
                 rVal.putData(EntityDataStrings.INTERACTION_BODY, rigidBody);
 
-                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable);
+                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
             } break;
             case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
                 //
@@ -125,7 +119,7 @@ public class ClientInteractionEngine {
                 rVal.putData(EntityDataStrings.INTERACTION_COLLIDABLE, collidable);
                 rVal.putData(EntityDataStrings.INTERACTION_BODY, rigidBody);
 
-                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable);
+                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
             } break;
             case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
                 //
@@ -161,7 +155,7 @@ public class ClientInteractionEngine {
                 rVal.putData(EntityDataStrings.INTERACTION_COLLIDABLE, collidable);
                 rVal.putData(EntityDataStrings.INTERACTION_BODY, rigidBody);
 
-                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable);
+                Globals.clientState.clientSceneWrapper.getInteractionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
             } break;
             default: {
                 throw new Error("Unsupported shape type! " + physicsTemplate.getType());

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

@@ -264,7 +264,7 @@ public class CollisionBodyCreation {
      * @param geom the geometry
      */
     public static void destroyShape(CollisionEngine collisionEngine, DGeom geom){
-        collisionEngine.destroyGeom(geom);
+        collisionEngine.destroyDGeom(geom);
     }
 
     /**

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

@@ -1,6 +1,7 @@
 package electrosphere.collision;
 
 import java.util.ArrayList;
+import java.util.Collections;
 import java.util.HashMap;
 import java.util.Iterator;
 import java.util.List;
@@ -10,7 +11,6 @@ import java.util.concurrent.locks.ReentrantLock;
 import org.joml.Matrix4d;
 import org.joml.Quaterniond;
 import org.joml.Vector3d;
-import org.joml.Vector4d;
 import org.ode4j.math.DVector3;
 import org.ode4j.ode.DBhvSpace;
 import org.ode4j.ode.DBody;
@@ -26,7 +26,6 @@ import org.ode4j.ode.DGeom.DNearCallback;
 import org.ode4j.ode.DJoint;
 import org.ode4j.ode.DJointGroup;
 import org.ode4j.ode.DMass;
-import org.ode4j.ode.DPlane;
 import org.ode4j.ode.DRay;
 import org.ode4j.ode.DSpace;
 import org.ode4j.ode.DSphere;
@@ -109,12 +108,12 @@ public class CollisionEngine {
     /**
      * The world object
      */
-    private DWorld world;
+    protected DWorld world;
 
     /**
      * The main space in the world
      */
-    private DBhvSpace space;
+    protected DBhvSpace space;
 
     /**
      * Lock for thread-safeing all ODE calls
@@ -124,7 +123,7 @@ public class CollisionEngine {
     /**
      * The contact group for caching collisions between collision and physics calls
      */
-    private DJointGroup contactgroup;
+    protected DJointGroup contactgroup;
 
     /**
      * <p> Maximum number of contact points per body </p>
@@ -134,27 +133,27 @@ public class CollisionEngine {
      * I used a value of 10 for a long time and found that cubes were sinking into TriMeshes.
      * </p>
      */
-    private static final int MAX_CONTACTS = 64;
+    protected static final int MAX_CONTACTS = 64;
 
     /**
      * The list of dbodies ode should be tracking
      */
-    private List<DBody> bodies = new ArrayList<DBody>();
+    protected List<DBody> bodies = new ArrayList<DBody>();
 
     /**
      * This is used to relate DBody's back to their collidables so that when the library detects a collision, the callback can know which collidables are involved.
      */
-    private Map<DBody,Collidable> bodyPointerMap = new HashMap<DBody,Collidable>();
+    protected Map<DBody,Collidable> bodyPointerMap = new HashMap<DBody,Collidable>();
 
     /**
      * This is used to relate DGeom's back to their collidables so that when the library detects a collision, the callback can know which collidables are involved.
      */
-    private Map<DGeom,Collidable> geomPointerMap = new HashMap<DGeom,Collidable>();
+    protected Map<DGeom,Collidable> geomPointerMap = new HashMap<DGeom,Collidable>();
 
     /**
      * The list of all collidables the engine is currently tracking
      */
-    private List<Collidable> collidableList = new ArrayList<Collidable>();
+    protected List<Collidable> collidableList = new ArrayList<Collidable>();
 
     /**
      * Dynamic spatial offset applied to all operations on the space.
@@ -178,26 +177,42 @@ public class CollisionEngine {
      */
     private DNearCallback nearCallback;
 
-    /**
-     * The base plane of the physics engine
-     */
-    private DPlane basePlane = null;
-
     /**
      * Number of geometries
      */
     private int geomCount = 0;
 
+    /**
+     * The near collision count
+     */
+    protected int nearCollisionCount = 0;
+
+    /**
+     * The number of collisions that make it all the way to creating impulses
+     */
+    protected int finalCollisionCount = 0;
+
+    /**
+     * Tracks whether the engine has rebased or not
+     */
+    private boolean hasRebased = false;
+
 
     /**
      * buffer for storing potential collisions
      */
-    private DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);
+    protected DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);
+
+    /**
+     * The name of the collision engine
+     */
+    protected final String name;
     
     /**
      * Constructor
      */
-    public CollisionEngine(){
+    public CollisionEngine(String name){
+        this.name = name;
         world = OdeHelper.createWorld();
         world.setGravity(0,-GRAVITY_MAGNITUDE,0);
         world.setQuickStepNumIterations(QUICKSTEP_ITERATION_COUNT);
@@ -206,9 +221,6 @@ public class CollisionEngine {
         // world.setContactSurfaceLayer(0.001);
         // world.setCFM(1e-10);
 
-        //base plane
-        basePlane = OdeHelper.createPlane(space, 0, 1, 0, 0);
-
         contactgroup = OdeHelper.createJointGroup();
         this.nearCallback = new DNearCallback() {
             @Override
@@ -221,12 +233,22 @@ public class CollisionEngine {
     /**
      * Creates a collision engine with a specified callback
      */
-    public static CollisionEngine create(CollisionResolutionCallback callback){
+    public static CollisionEngine create(String name, CollisionResolutionCallback callback){
-        CollisionEngine rVal = new CollisionEngine();
+        CollisionEngine rVal = new CollisionEngine(name);
         rVal.setCollisionResolutionCallback(callback);
         return rVal;
     }
 
+    /**
+     * Creates a collision engine with a specified callback
+     */
+    public static CollisionEngine create(String name, PhysicsCallback callback){
+        CollisionEngine rVal = new CollisionEngine(name);
+        rVal.nearCallback = callback;
+        callback.engine = rVal;
+        return rVal;
+    }
+
 
     /**
      * Resolves collisions in the engine
@@ -240,26 +262,19 @@ public class CollisionEngine {
      */
     public static void resolveCollision(DContactGeom contactGeom, Collidable impactor, Collidable receiver, Vector3d normal, Vector3d localPosition, Vector3d worldPos, float magnitude){
         switch(receiver.getType()){
-            case Collidable.TYPE_CREATURE:
+            case Collidable.TYPE_CREATURE: {
                 switch(impactor.getType()){
-                    case Collidable.TYPE_STATIC:
+                    case Collidable.TYPE_STATIC: {
-                    //    System.out.println(EntityUtils.getPosition(impactor.getParent()) + " " + EntityUtils.getPosition(receiver.getParent()));
-//                        System.out.println();
-                    //    System.out.println("Terrain-creature collision: " + normal + " mag:" + magnitude);
-                        // if(normal.y > normal.x + normal.z){
-                        //     normal.x = 0;
-                        //     normal.z = 0;
-                        // }
                         receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude * 2, Collidable.TYPE_STATIC));
-                        break;
+                    } break;
-                    case Collidable.TYPE_CREATURE:
+                    case Collidable.TYPE_CREATURE: {
                         receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_CREATURE));
-                        break;
+                    } break;
-                    case Collidable.TYPE_OBJECT:
+                    case Collidable.TYPE_OBJECT: {
                         receiver.addImpulse(new Impulse(normal, localPosition, worldPos, magnitude, Collidable.TYPE_OBJECT));
-                        break;
+                    } break;
                 }
-                break;
+            } break;
         }
     }
     
@@ -276,8 +291,15 @@ public class CollisionEngine {
         spaceLock.unlock();
     }
 
+    /**
+     * Gets the list of collidables
+     * @return The list of collidables
+     */
     public List<Collidable> getCollidables(){
-        return collidableList;
+        spaceLock.lock();
+        List<Collidable> rVal = Collections.unmodifiableList(this.collidableList);
+        spaceLock.unlock();
+        return rVal;
     }
     
     
@@ -302,12 +324,6 @@ public class CollisionEngine {
                 ){
             return false;
         }
-//        //
-//        // are we below the terrain?
-//        //
-//        if(w.getElevationAtPoint(positionToCheck) > positionToCheck.y){
-//            return false;
-//        }
         return rVal;
     }
     
@@ -317,6 +333,9 @@ public class CollisionEngine {
     public void simulatePhysics(){
         Globals.profiler.beginCpuSample("physics");
         spaceLock.lock();
+        //reset tracking
+        this.nearCollisionCount = 0;
+        this.finalCollisionCount = 0;
         // remove all contact joints
         contactgroup.empty();
         //main simulation
@@ -324,7 +343,6 @@ public class CollisionEngine {
             Globals.profiler.beginCpuSample("collide");
             OdeHelper.spaceCollide(space, 0, nearCallback);
             Globals.profiler.endCpuSample();
-            // space.collide2(space, collisionWorldData, nearCallback);
 
             //simulate physics
             Globals.profiler.beginCpuSample("step physics");
@@ -362,8 +380,16 @@ public class CollisionEngine {
      * @param o2 the second collision body
      */
 	private void nearCallback(Object data, DGeom o1, DGeom o2){
+        if(this.name.equals("serverPhysics")){
+            this.nearCollisionCount++;
+        }
 		// if (o1->body && o2->body) return;
 
+        //ie if both are in static space
+        if(o1 == o2){
+            return;
+        }
+
         //if the collision is static-on-static, skip
         if(o1.getCategoryBits() == Collidable.TYPE_STATIC_BIT && o2.getCategoryBits() == Collidable.TYPE_STATIC_BIT){
             return;
@@ -384,11 +410,6 @@ public class CollisionEngine {
             return;
         }
 
-        //check if we're colliding against the base plane
-        if(o1 == this.basePlane || o2 == this.basePlane){
-            return;
-        }
-
         //get the collidables for each geom
         Collidable c1 = null;
         if(b1 != null){
@@ -428,59 +449,66 @@ public class CollisionEngine {
 
         Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - Full collision phase");
         try {
+            Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - setup");
             //null out the contact buffer
             contacts.nullify();
             SurfaceParams surfaceParams1 = c1.getSurfaceParams();
-            SurfaceParams surfaceParams2 = c2.getSurfaceParams();
+            SurfaceParams surfaceParams2 = null;
+            if(c2 != null){
+                surfaceParams2 = c2.getSurfaceParams();
+            }
             for (int i=0; i<MAX_CONTACTS; i++) {
                 DContact contact = contacts.get(i);
                 contact.surface.mode = surfaceParams1.getMode();
                 contact.surface.mu = surfaceParams1.getMu();
                 if(surfaceParams1.getRho() != null){
                     contact.surface.rho = surfaceParams1.getRho();
-                } else if(surfaceParams2.getRho() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getRho() != null){
                     contact.surface.rho = surfaceParams2.getRho();
                 }
                 if(surfaceParams1.getRho2() != null){
                     contact.surface.rho2 = surfaceParams1.getRho2();
-                } else if(surfaceParams2.getRho2() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getRho2() != null){
                     contact.surface.rho = surfaceParams2.getRho2();
                 }
                 if(surfaceParams1.getRhoN() != null){
                     contact.surface.rhoN = surfaceParams1.getRhoN();
-                } else if(surfaceParams2.getRhoN() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getRhoN() != null){
                     contact.surface.rho = surfaceParams2.getRhoN();
                 }
                 if(surfaceParams1.getBounce() != null){
                     contact.surface.bounce = surfaceParams1.getBounce();
-                } else if(surfaceParams2.getBounce() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getBounce() != null){
                     contact.surface.rho = surfaceParams2.getBounce();
                 }
                 if(surfaceParams1.getBounceVel() != null){
                     contact.surface.bounce_vel = surfaceParams1.getBounceVel();
-                } else if(surfaceParams2.getBounceVel() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getBounceVel() != null){
                     contact.surface.rho = surfaceParams2.getBounceVel();
                 }
                 if(surfaceParams1.getSoftErp() != null){
                     contact.surface.soft_erp = surfaceParams1.getSoftErp();
-                } else if(surfaceParams2.getSoftErp() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getSoftErp() != null){
                     contact.surface.rho = surfaceParams2.getSoftErp();
                 }
                 if(surfaceParams1.getSoftCfm() != null){
                     contact.surface.soft_cfm = surfaceParams1.getSoftCfm();
-                } else if(surfaceParams2.getSoftCfm() != null){
+                } else if(surfaceParams2 != null && surfaceParams2.getSoftCfm() != null){
                     contact.surface.rho = surfaceParams2.getSoftCfm();
                 }
             }
+            Globals.profiler.endCpuSample();
             //calculate collisions
             Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - OdeHelper.collide");
             int numc = OdeHelper.collide(o1,o2,MAX_CONTACTS,contacts.getGeomBuffer());
             Globals.profiler.endCpuSample();
             //create DContacts based on each collision that occurs
+            Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - contact iterations");
             if(numc != 0){
                 for(int i=0; i<numc; i++){
                     DContact contact = contacts.get(i);
 
+
                     //special code for ray casting
                     if (o1 instanceof DRay || o2 instanceof DRay){
                         DVector3 end = new DVector3();
@@ -549,8 +577,12 @@ public class CollisionEngine {
                             (float)contact.geom.depth
                         );
                     }
+
+                    //tracking updates
+                    this.finalCollisionCount++;
                 }
             }
+            Globals.profiler.endCpuSample();
         } catch(ArrayIndexOutOfBoundsException ex){
             //I've found that ode4j occasionally throws an exception on the OdeHelper.collide function.
             //I don't know why it has out of bounds elements, but it's happening.
@@ -633,16 +665,16 @@ public class CollisionEngine {
         Matrix4d inverseTransform = new Matrix4d();
         if(this.collisionWorldData != null){
             for(Collidable collidable : collidableList){
-                if(collidable.getParentTracksCollidable()){
+                if(collidable.getParentTracksCollidable() && collidable.getReady()){
                     Entity physicsEntity = collidable.getParent();
                     DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                     DGeom geom = PhysicsEntityUtils.getDGeom(physicsEntity);
-                    Vector4d rawPos = null;
+                    Vector3d rawPos = null;
                     inverseTransform.identity();
                     if(rigidBody != null){
-                        rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin),1));
+                        rawPos = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin);
                     } else if(geom != null){
-                        rawPos = inverseTransform.transform(new Vector4d(PhysicsUtils.odeVecToJomlVec(geom.getPosition()).add(this.floatingOrigin),1));
+                        rawPos = PhysicsUtils.odeVecToJomlVec(geom.getPosition()).add(this.floatingOrigin);
                     } else {
                         continue;
                     }
@@ -664,10 +696,11 @@ public class CollisionEngine {
                         CollidableTemplate template = PhysicsEntityUtils.getPhysicsTemplate(physicsEntity);
                         if(template != null){
                             suggestedPosition.sub(template.getOffsetX(),template.getOffsetY(),template.getOffsetZ());
+                            suggestedPosition = this.suggestMovementPosition(collisionWorldData, suggestedPosition);
                             newRotation.mul(new Quaterniond(template.getRotX(),template.getRotY(),template.getRotZ(),template.getRotW()).invert());
                         }
                     }
-                    EntityUtils.getPosition(physicsEntity).set(suggestedPosition);
+                    EntityUtils.setPosition(physicsEntity, suggestedPosition);
                     EntityUtils.getRotation(physicsEntity).set(newRotation);
                 }
             }
@@ -686,53 +719,47 @@ public class CollisionEngine {
             int collected = 0;
             Vector3d newOrigin = new Vector3d();
             //calculate new origin
+            //only reference the rigid bodies because the rebase is principally concerned with physics sim
+            //pure colliders (no rigid body) don't do physics sim in ode so don't need to worry about being quite as close to 0,0,0
             for(Collidable collidable : collidableList){
                 Entity physicsEntity = collidable.getParent();
                 DBody rigidBody = PhysicsEntityUtils.getDBody(physicsEntity);
                 if(rigidBody != null){
-                    Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition()).add(this.floatingOrigin);
+                    Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
+                    currentBodyOffset.add(this.floatingOrigin);
                     if(collected == 0){
                         newOrigin.set(currentBodyOffset);
                     } else {
-                        float percentExisting = collected / (float)(collected + 1);
+                        newOrigin.add(currentBodyOffset);
-                        float percentNew = 1.0f - percentExisting;
-                        newOrigin = newOrigin.mul(percentExisting).add(currentBodyOffset.mul(percentNew));
                     }
                     collected++;
                 }
-                // DGeom geom = PhysicsEntityUtils.getDGeom(physicsEntity);
+            }
-                // if(geom != null){
+            if(collected > 0){
-                //     if(geom instanceof DSpace space){
+                newOrigin = newOrigin.mul(1.0/(double)collected);
-                //         for(DGeom child : space.getGeoms()){
-                //             Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(child.getPosition()).add(this.floatingOrigin);
-                //             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++;
-                //         }
-                //     } else {
-                //         Vector3d currentBodyOffset = PhysicsUtils.odeVecToJomlVec(geom.getPosition()).add(this.floatingOrigin);
-                //         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++;
-                //     }
-                // }
             }
             newOrigin = newOrigin.round();
             Vector3d delta = new Vector3d(this.floatingOrigin);
             delta = delta.sub(newOrigin);
 
+
             //only perform rebase if sufficiently far away
             if(delta.length() > REBASE_TRIGGER_DISTANCE){
+                //error checking
+                // if(collected > 1 && delta.length() > MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN){
+                //     System.out.println("newOrigin: " + newOrigin);
+                //     System.out.println("delta: " + delta);
+                //     throw new Error(this.getDebugStatus());
+                // }
+                // System.out.println("REbase");
+                // System.out.println(this.getStatus());
+                // System.out.println("newOrigin: " + newOrigin);
+                // System.out.println("delta: " + delta);
+                // if(delta.y > 100 || delta.y < -100){
+                //     throw new Error(this.getDebugStatus());
+                // }
+
+
                 this.floatingOrigin = newOrigin;
                 //apply new origin to all geoms
                 //calculate new origin
@@ -757,6 +784,7 @@ public class CollisionEngine {
                     }
                 }
             }
+            this.hasRebased = true;
         }
         spaceLock.unlock();
         Globals.profiler.endCpuSample();
@@ -766,12 +794,19 @@ public class CollisionEngine {
      * Registers a collision object with the server
      * @param body The body
      * @param collidable The corresponding collidable
+     * @param position The position of the body
      */
-    public void registerCollisionObject(DBody body, Collidable collidable){
+    public void registerCollisionObject(DBody body, Collidable collidable, Vector3d position){
         if(collidable == null){
             throw new Error("Collidable is null!");
         }
         spaceLock.lock();
+
+        //Body transform needs to be set before the body is added to the collidable list
+        //this makes sure that dynamic update transforms and floating origin work correctly
+        this.setBodyTransform(body, new Vector3d(position), new Quaterniond());
+
+        //actually attach to tracking structures
         this.registerPhysicsObject(body);
         bodyPointerMap.put(body,collidable);
         collidableList.add(collidable);
@@ -783,11 +818,16 @@ public class CollisionEngine {
      * @param geom The geom
      * @param collidable The corresponding collidable
      */
-    public void registerCollisionObject(DGeom geom, Collidable collidable){
+    public void registerCollisionObject(DGeom geom, Collidable collidable, Vector3d position){
         if(collidable == null){
             throw new Error("Collidable is null!");
         }
         spaceLock.lock();
+
+        //Body transform needs to be set before the body is added to the collidable list
+        //this makes sure that dynamic update transforms and floating origin work correctly
+        this.setGeomTransform(geom, position, new Quaterniond());
+
         geomPointerMap.put(geom,collidable);
         collidableList.add(collidable);
         spaceLock.unlock();
@@ -798,12 +838,16 @@ public class CollisionEngine {
      * @param geoms The list of geoms
      * @param collidable The corresponding collidable
      */
-    public void registerCollisionObject(List<DGeom> geoms, Collidable collidable){
+    public void registerCollisionObject(List<DGeom> geoms, Collidable collidable, Vector3d position){
         if(collidable == null){
             throw new Error("Collidable is null!");
         }
         spaceLock.lock();
         for(DGeom geom : geoms){
+            //Body transform needs to be set before the body is added to the collidable list
+            //this makes sure that dynamic update transforms and floating origin work correctly
+            this.setGeomTransform(geom, position, new Quaterniond());
+
             geomPointerMap.put(geom,collidable);
         }
         collidableList.add(collidable);
@@ -1294,8 +1338,17 @@ public class CollisionEngine {
      */
     protected void setBodyTransform(DBody body, Vector3d position, Quaterniond rotation){
         spaceLock.lock();
+
+        // if(this.name.equals("serverPhysics")){
+        //     if(position.distance(0,0,0) < 100){
+        //         throw new Error("Reposition server body " + position);
+        //     }
+        // }
         body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
         body.setQuaternion(PhysicsUtils.jomlQuatToOdeQuat(rotation));
+        // if(this.name.equals("serverPhysics")){
+        //     System.out.println("SetBodyTransform " + body.getPosition());
+        // }
         spaceLock.unlock();
     }
 
@@ -1334,8 +1387,26 @@ public class CollisionEngine {
      * @param angularForce The angular force
      */
     protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
+        if(!this.hasRebased){
+            return;
+        }
         if(body != null){
             spaceLock.lock();
+
+
+            // if(this.name.equals("clientPhysics") || this.name.equals("serverPhysics")){
+            //     double posX = position.x - this.floatingOrigin.x;
+            //     double posY = position.y - this.floatingOrigin.y;
+            //     double posZ = position.z - this.floatingOrigin.z;
+            //     if((posX > MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posZ > MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posX < -MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posZ < -MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN) && this.floatingOrigin.length() == 0 && this.hasRebased){
+            //         System.out.println("Sync body pos: " + posX + "," + posY + "," + posZ);
+            //         spaceLock.unlock();
+            //         return;
+            //     }
+            //     if(this.bodies.size() > 2 && this.floatingOrigin.length() == 0 && this.hasRebased){
+            //         throw new Error(this.getDebugStatus());
+            //     }
+            // }
             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));
@@ -1355,6 +1426,19 @@ public class CollisionEngine {
      */
     protected void setBodyTransform(DBody body, CollidableTemplate template, Vector3d position, Quaterniond rotation, Vector3d scale){
         spaceLock.lock();
+        // if(this.name.equals("clientPhysics") || this.name.equals("serverPhysics")){
+        //     double posX = position.x - this.floatingOrigin.x;
+        //     double posY = position.y - this.floatingOrigin.y;
+        //     double posZ = position.z - this.floatingOrigin.z;
+        //     if((posX > MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posZ > MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posX < -MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN || posZ < -MAX_EXPECTED_DIST_FROM_LOCAL_ORIGIN) && this.floatingOrigin.length() == 0 && this.hasRebased){
+        //         System.out.println("Set body pos: " + posX + "," + posY + "," + posZ);
+        //         spaceLock.unlock();
+        //         return;
+        //     }
+        //     if(this.bodies.size() > 2 && this.floatingOrigin.length() == 0 && this.hasRebased){
+        //         throw new Error(this.getDebugStatus());
+        //     }
+        // }
         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();
@@ -1496,18 +1580,6 @@ public class CollisionEngine {
         geom.setBody(null);
     }
 
-    /**
-     * Destroys a geometry
-     * @param geom The geometry
-     */
-    protected void destroyGeom(DGeom geom){
-        spaceLock.lock();
-        this.space.remove(geom);
-        geom.destroy();
-        this.geomCount--;
-        spaceLock.unlock();
-    }
-
     /**
      * Attaches a geom to a body
      * @param body the body
@@ -1537,6 +1609,7 @@ public class CollisionEngine {
      */
     public String getStatus(){
         String message = "" +
+        "Name: " + this.name + "\n" +
         "Bodies: " + this.bodies.size() + "\n" +
         "Body Ptrs: " + this.bodyPointerMap.size() + "\n" +
         "Geom Ptrs: " + this.geomPointerMap.size() + "\n" +
@@ -1544,11 +1617,29 @@ public class CollisionEngine {
         "Space geom count: " + this.space.getNumGeoms() + "\n" +
         "Tracked geom count: " + this.geomCount + "\n" +
         "Floating origin: " + this.floatingOrigin.x + "," + this.floatingOrigin.y + "," + this.floatingOrigin.z + "\n" +
+        "Near Collision Count: " + this.nearCollisionCount + "\n" +
+        "Final Collision Count: " + this.finalCollisionCount + "\n" +
         ""
         ;
         return message;
     }
 
+    /**
+     * Gets the status of the collision engine
+     * @return The status of the collision engine
+     */
+    public String getDebugStatus(){
+        String message = this.getStatus();
+        for(Collidable collidable : collidableList){
+            DBody rigidBody = PhysicsEntityUtils.getDBody(collidable.getParent());
+            if(rigidBody != null){
+                Vector3d existingPosition = PhysicsUtils.odeVecToJomlVec(rigidBody.getPosition());
+                message = message + existingPosition.x + "," + existingPosition.y + "," + existingPosition.z + "\n";
+            }
+        }
+        return message;
+    }
+
     /**
      * Gets the floating origin of the collision engine
      * @return The floating origin
@@ -1556,6 +1647,38 @@ public class CollisionEngine {
     public Vector3d getFloatingOrigin(){
         return new Vector3d(this.floatingOrigin);
     }
+
+    /**
+     * Gets the world of the engine
+     * @return The world of the engine
+     */
+    protected DWorld getWorld(){
+        return this.world;
+    }
+
+    /**
+     * Gets the space of the engine
+     * @return The space of the engine
+     */
+    protected DSpace getSpace(){
+        return this.space;
+    }
+
+    /**
+     * Gets the near collision count
+     * @return The near collision count
+     */
+    protected int getNearCollisionCount(){
+        return nearCollisionCount;
+    }
+
+    /**
+     * Gets the number of collisions that make it to the point of creating joints/impulses
+     * @return The number of collisions
+     */
+    protected int getFinalCollisionCount(){
+        return finalCollisionCount;
+    }
     
 
     /**

src/main/java/electrosphere/collision/PhysicsCallback.java

@@ -0,0 +1,212 @@
+package electrosphere.collision;
+
+import org.ode4j.math.DVector3;
+import org.ode4j.ode.DBody;
+import org.ode4j.ode.DContact;
+import org.ode4j.ode.DContactJoint;
+import org.ode4j.ode.DGeom;
+import org.ode4j.ode.DGeom.DNearCallback;
+import org.ode4j.ode.DJoint;
+import org.ode4j.ode.DRay;
+import org.ode4j.ode.OdeHelper;
+
+import electrosphere.collision.collidable.Collidable;
+import electrosphere.collision.collidable.SurfaceParams;
+import electrosphere.engine.Globals;
+import electrosphere.logger.LoggerInterface;
+
+/**
+ * Near callback for main physics engine
+ */
+public class PhysicsCallback implements DNearCallback {
+
+    /**
+     * The collision engine that will invoke this callback
+     */
+    protected CollisionEngine engine;
+
+    /**
+     * Constructor
+     */
+    public PhysicsCallback(){
+    }
+
+    @Override
+    public void call(Object data, DGeom o1, DGeom o2) {
+        if(engine.name.equals("serverPhysics")){
+            engine.nearCollisionCount++;
+        }
+
+        //ie if both are in static space
+        if(o1 == o2){
+            return;
+        }
+
+        //if neither are bodies
+        if(o1.getBody() == null && o2.getBody() == null){
+            return;
+        }
+
+		// exit without doing anything if the two bodies are connected by a joint
+		DBody b1 = o1.getBody();
+		DBody b2 = o2.getBody();
+		if(b1 != null && b2 != null && OdeHelper.areConnectedExcluding(b1,b2,DContactJoint.class)){
+            return;
+        }
+
+        //get the collidables for each geom
+        Collidable c1 = null;
+        if(b1 != null){
+            c1 = engine.bodyPointerMap.get(b1);
+        } else if(o1.getBody() == null) {
+            c1 = engine.geomPointerMap.get(o1);
+        }
+        Collidable c2 = null;
+        if(b2 != null){
+            c2 = engine.bodyPointerMap.get(b2);
+        } else if(o2.getBody() == null) {
+            c2 = engine.geomPointerMap.get(o2);
+        }
+
+        //make sure we have collidables for both
+        if(c1 == null || c2 == null){
+            String message = "Collidable is undefined!\n" +
+            "Geoms:\n" +
+            o1 + " \n" +
+            o2 + " \n" +
+            "Bodies:\n" +
+            b1 + " \n" +
+            b2 + " \n" +
+            "Colliders:\n" +
+            c1 + " \n" +
+            c2 + " \n" +
+            "Obj 1 pointers:\n" +
+            engine.bodyPointerMap.get(b1) + " \n" +
+            engine.geomPointerMap.get(o1) + " \n" +
+            "Obj 2 pointers:\n" +
+            engine.bodyPointerMap.get(b2) + " \n" +
+            engine.geomPointerMap.get(o2) + " \n" +
+            ""
+            ;
+            throw new Error(message);
+        }
+
+        Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - Full collision phase");
+        try {
+            Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - setup");
+            //null out the contact buffer
+            engine.contacts.nullify();
+            SurfaceParams surfaceParams1 = c1.getSurfaceParams();
+            SurfaceParams surfaceParams2 = null;
+            if(c2 != null){
+                surfaceParams2 = c2.getSurfaceParams();
+            }
+            for (int i=0; i< CollisionEngine.MAX_CONTACTS; i++) {
+                DContact contact = engine.contacts.get(i);
+                contact.surface.mode = surfaceParams1.getMode();
+                contact.surface.mu = surfaceParams1.getMu();
+                if(surfaceParams1.getRho() != null){
+                    contact.surface.rho = surfaceParams1.getRho();
+                } else if(surfaceParams2 != null && surfaceParams2.getRho() != null){
+                    contact.surface.rho = surfaceParams2.getRho();
+                }
+                if(surfaceParams1.getRho2() != null){
+                    contact.surface.rho2 = surfaceParams1.getRho2();
+                } else if(surfaceParams2 != null && surfaceParams2.getRho2() != null){
+                    contact.surface.rho = surfaceParams2.getRho2();
+                }
+                if(surfaceParams1.getRhoN() != null){
+                    contact.surface.rhoN = surfaceParams1.getRhoN();
+                } else if(surfaceParams2 != null && surfaceParams2.getRhoN() != null){
+                    contact.surface.rho = surfaceParams2.getRhoN();
+                }
+                if(surfaceParams1.getBounce() != null){
+                    contact.surface.bounce = surfaceParams1.getBounce();
+                } else if(surfaceParams2 != null && surfaceParams2.getBounce() != null){
+                    contact.surface.rho = surfaceParams2.getBounce();
+                }
+                if(surfaceParams1.getBounceVel() != null){
+                    contact.surface.bounce_vel = surfaceParams1.getBounceVel();
+                } else if(surfaceParams2 != null && surfaceParams2.getBounceVel() != null){
+                    contact.surface.rho = surfaceParams2.getBounceVel();
+                }
+                if(surfaceParams1.getSoftErp() != null){
+                    contact.surface.soft_erp = surfaceParams1.getSoftErp();
+                } else if(surfaceParams2 != null && surfaceParams2.getSoftErp() != null){
+                    contact.surface.rho = surfaceParams2.getSoftErp();
+                }
+                if(surfaceParams1.getSoftCfm() != null){
+                    contact.surface.soft_cfm = surfaceParams1.getSoftCfm();
+                } else if(surfaceParams2 != null && surfaceParams2.getSoftCfm() != null){
+                    contact.surface.rho = surfaceParams2.getSoftCfm();
+                }
+            }
+            Globals.profiler.endCpuSample();
+            //calculate collisions
+            Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - OdeHelper.collide");
+            int numc = OdeHelper.collide(o1,o2,CollisionEngine.MAX_CONTACTS,engine.contacts.getGeomBuffer());
+            Globals.profiler.endCpuSample();
+            //create DContacts based on each collision that occurs
+            Globals.profiler.beginAggregateCpuSample("CollisionEngine.nearCallback - contact iterations");
+            if(numc != 0){
+                for(int i=0; i<numc; i++){
+                    DContact contact = engine.contacts.get(i);
+
+                    //special code for ray casting
+                    if (o1 instanceof DRay || o2 instanceof DRay){
+                        DVector3 end = new DVector3();
+                        end.eqSum(contact.geom.pos, contact.geom.normal, contact.geom.depth);
+                        continue;
+                    }
+
+                    //
+                    //add contact to contact group
+                    DJoint c = OdeHelper.createContactJoint(engine.world,engine.contactgroup,contact);
+                    if(b1 == null){
+                        if(b2 == null){
+                        } else {
+                            c.attach(null,b2);
+                        }
+                    } else {
+                        if(b2 == null){
+                            c.attach(b1,null);
+                        } else {
+                            c.attach(b1,b2);
+                        }
+                    }
+
+                    // Use the default collision resolution
+                    CollisionEngine.resolveCollision(
+                        contact.geom,
+                        c1,
+                        c2,
+                        PhysicsUtils.odeVecToJomlVec(contact.geom.normal).mul(-1.0),
+                        PhysicsUtils.odeVecToJomlVec(contact.fdir1).mul(-1.0),
+                        PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
+                        (float)contact.geom.depth
+                    );
+                    CollisionEngine.resolveCollision(
+                        contact.geom,
+                        c2,
+                        c1,
+                        PhysicsUtils.odeVecToJomlVec(contact.geom.normal),
+                        PhysicsUtils.odeVecToJomlVec(contact.fdir1),
+                        PhysicsUtils.odeVecToJomlVec(contact.geom.pos),
+                        (float)contact.geom.depth
+                    );
+
+                    //tracking updates
+                    engine.finalCollisionCount++;
+                }
+            }
+            Globals.profiler.endCpuSample();
+        } catch(ArrayIndexOutOfBoundsException ex){
+            //I've found that ode4j occasionally throws an exception on the OdeHelper.collide function.
+            //I don't know why it has out of bounds elements, but it's happening.
+            //Catching the exception here allows the engine to keep running at least.
+            LoggerInterface.loggerEngine.ERROR("ode4j error", ex);
+        }
+        Globals.profiler.endCpuSample();
+    }
+    
+}

src/main/java/electrosphere/collision/PhysicsEntityUtils.java

@@ -67,7 +67,6 @@ public class PhysicsEntityUtils {
     public static void clientAttachCollidableTemplate(Entity rVal, CollidableTemplate physicsTemplate){
         Collidable collidable;
         double mass = 1.0f;
-        CollisionEngine engine = Globals.clientState.clientSceneWrapper.getCollisionEngine();
         if(physicsTemplate.getMass() != null){
             mass = physicsTemplate.getMass();
         }
@@ -77,97 +76,7 @@ public class PhysicsEntityUtils {
         }
         CollisionEngine.lockOde();
         if(physicsTemplate.getKinematic()){
-            DGeom geom = null;
+            PhysicsEntityUtils.clientAttachGeom(rVal, physicsTemplate, EntityUtils.getPosition(rVal));
-            switch(physicsTemplate.getType()){
-                case CollidableTemplate.COLLIDABLE_TYPE_CYLINDER: {
-
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCylinderShape(
-                        engine, 
-                        physicsTemplate.getDimension1(),
-                        physicsTemplate.getDimension2(),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and link to structures
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal, geom);
-
-                    //
-                    //store values
-                    Matrix4d offsetTransform = new Matrix4d().translationRotate(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW() //rotate
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
-                    engine.registerCollisionObject(geom, collidable);
-                } break;
-                case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCubeShape(
-                        Globals.clientState.clientSceneWrapper.getCollisionEngine(),
-                        new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and link to structures
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal,geom);
-
-                    //
-                    //store values
-                    Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
-                        1, 1, 1 //scale
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
-                    engine.registerCollisionObject(geom, collidable);
-                } break;
-                case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCapsuleShape(
-                        Globals.clientState.clientSceneWrapper.getCollisionEngine(),
-                        physicsTemplate.getDimension1(),
-                        physicsTemplate.getDimension2(),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and link to structures
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal,geom);
-
-                    //
-                    //store values
-                    Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
-                        1, 1, 1 //scale
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
-
-                    Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
-                    engine.registerCollisionObject(geom, collidable);
-                } break;
-            }
         } else {
             DBody rigidBody = null;
             switch(physicsTemplate.getType()){
@@ -231,10 +140,9 @@ public class PhysicsEntityUtils {
                     rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                     rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                     rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);
-
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
                     Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
                 } break;
                 case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
@@ -295,10 +203,9 @@ public class PhysicsEntityUtils {
                     rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                     rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                     rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);
-
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
                     Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
                 } break;
                 case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
@@ -364,29 +271,142 @@ public class PhysicsEntityUtils {
 
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+                    Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(rVal));
                     Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
                 } break;
             }
             //if we successfully attached the body, add a sync tree
-            if(rigidBody != null){
+            ClientPhysicsSyncTree.attachTree(rVal);
-                ClientPhysicsSyncTree.attachTree(rVal);
+            if(ClientGravityTree.hasClientGravityTree(rVal)){
-                if(ClientGravityTree.hasClientGravityTree(rVal)){
+                ClientGravityTree.getClientGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
-                    ClientGravityTree.getClientGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
-                }
             }
         }
         CollisionEngine.unlockOde();
     }
 
+    /**
+     * [SERVER ONLY] Attaches a collidable template to a given entity
+     * @param rVal The entity
+     * @param physicsTemplate The collidable template
+     * @return The geometry object
+     */
+    public static DGeom clientAttachGeom(Entity rVal, CollidableTemplate physicsTemplate, Vector3d position){
+        DGeom geom = null;
+        Collidable collidable;
+        double mass = 1.0f;
+        long categoryBit = Collidable.TYPE_CREATURE_BIT;
+        if(physicsTemplate.getKinematic()){
+            categoryBit = Collidable.TYPE_STATIC_BIT;
+        }
+        CollisionEngine engine = Globals.clientState.clientSceneWrapper.getCollisionEngine();
+        CollisionEngine.lockOde();
+        switch(physicsTemplate.getType()){
+            case CollidableTemplate.COLLIDABLE_TYPE_CYLINDER: {
+
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCylinderShape(
+                    engine, 
+                    physicsTemplate.getDimension1(),
+                    physicsTemplate.getDimension2(),
+                    categoryBit
+                );
+
+                //
+                //create collidable and link to structures
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                PhysicsEntityUtils.setDGeom(rVal, geom);
+
+                //
+                //store values
+                Matrix4d offsetTransform = new Matrix4d().translationRotate(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW() //rotate
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+
+                Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
+                engine.registerCollisionObject(geom, collidable, EntityUtils.getPosition(rVal));
+            } break;
+            case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCubeShape(
+                    Globals.clientState.clientSceneWrapper.getCollisionEngine(),
+                    new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
+                    categoryBit
+                );
+
+                //
+                //create collidable and link to structures
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                PhysicsEntityUtils.setDGeom(rVal,geom);
+
+                //
+                //store values
+                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
+                    1, 1, 1 //scale
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+
+                Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
+                engine.registerCollisionObject(geom, collidable, EntityUtils.getPosition(rVal));
+            } break;
+            case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCapsuleShape(
+                    Globals.clientState.clientSceneWrapper.getCollisionEngine(),
+                    physicsTemplate.getDimension1(),
+                    physicsTemplate.getDimension2(),
+                    categoryBit
+                );
+
+                //
+                //create collidable and link to structures
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                PhysicsEntityUtils.setDGeom(rVal,geom);
+
+                //
+                //store values
+                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
+                    1, 1, 1 //scale
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
+
+                Globals.clientState.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
+                engine.registerCollisionObject(geom, collidable, EntityUtils.getPosition(rVal));
+            } break;
+        }
+        //if we successfully attached the body, add a sync tree
+        ClientPhysicsSyncTree.attachTree(rVal);
+        if(ClientGravityTree.hasClientGravityTree(rVal)){
+            ClientGravityTree.getClientGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
+        }
+        CollisionEngine.unlockOde();
+        return geom;
+    }
+
 
     /**
      * [SERVER ONLY] Attaches a collidable template to a given entity
      * @param realm The realm the entity is inside of
      * @param rVal The entity
      * @param physicsTemplate The collidable template
+     * @param position the position of the body
      */
-    public static void serverAttachCollidableTemplate(Realm realm, Entity rVal, CollidableTemplate physicsTemplate){
+    public static void serverAttachCollidableTemplate(Realm realm, Entity rVal, CollidableTemplate physicsTemplate, Vector3d position){
         Collidable collidable;
         double mass = 1.0f;
         if(physicsTemplate.getMass() != null){
@@ -398,102 +418,7 @@ public class PhysicsEntityUtils {
         }
         CollisionEngine.lockOde();
         if(physicsTemplate.getKinematic()){
-            DGeom geom = null;
+            PhysicsEntityUtils.serverAttachGeom(realm,rVal,physicsTemplate,position);
-            switch(physicsTemplate.getType()){
-                case CollidableTemplate.COLLIDABLE_TYPE_CYLINDER: {
-
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCylinderShape(
-                        realm.getCollisionEngine(),
-                        physicsTemplate.getDimension1(),
-                        physicsTemplate.getDimension2(),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and attach tracking
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal, geom);
-
-                    //
-                    //store data
-                    Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
-                        1, 1, 1 //scale
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
-
-                    ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
-                    realm.getCollisionEngine().registerCollisionObject(geom, collidable);
-                } break;
-                case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCubeShape(
-                        realm.getCollisionEngine(),
-                        new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and attach tracking
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal, geom);
-
-                    //
-                    //store data
-                    Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
-                        1, 1, 1 //scale
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
-
-                    ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
-                    realm.getCollisionEngine().registerCollisionObject(geom, collidable);
-                } break;
-                case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
-                    //
-                    //create dbody
-                    geom = CollisionBodyCreation.createCapsuleShape(
-                        realm.getCollisionEngine(),
-                        physicsTemplate.getDimension1(),
-                        physicsTemplate.getDimension2(),
-                        categoryBit
-                    );
-
-                    //
-                    //create collidable and attach tracking
-                    collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
-                    PhysicsEntityUtils.setDGeom(rVal, geom);
-
-                    //
-                    //store data
-                    Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
-                        physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
-                        physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
-                        1, 1, 1 //scale
-                    );
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
-                    rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
-                    rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
-
-                    rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
-
-                    ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
-                    realm.getCollisionEngine().registerCollisionObject(geom, collidable);
-                } break;
-            }
         } else {
             DBody rigidBody = null;
             switch(physicsTemplate.getType()){
@@ -551,17 +476,17 @@ public class PhysicsEntityUtils {
                         realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                     }
                     if(physicsTemplate.getKinematic()){
-                        Globals.clientState.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
+                        realm.getCollisionEngine().setKinematic(rigidBody);
                         rigidBody.disable();
                     }
                     rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                     rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                     rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                     rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
-
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+
+                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable, position);
                     ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
                 } break;
                 case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
@@ -615,17 +540,17 @@ public class PhysicsEntityUtils {
                         realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                     }
                     if(physicsTemplate.getKinematic()){
-                        Globals.clientState.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
+                        realm.getCollisionEngine().setKinematic(rigidBody);
                         rigidBody.disable();
                     }
                     rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                     rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                     rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                     rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
-
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+
+                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable, position);
                     ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
                 } break;
                 case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
@@ -681,31 +606,154 @@ public class PhysicsEntityUtils {
                         realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                     }
                     if(physicsTemplate.getKinematic()){
-                        Globals.clientState.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
+                        realm.getCollisionEngine().setKinematic(rigidBody);
                         rigidBody.disable();
                     }
                     rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                     rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                     rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                     rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
-
                     rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
 
-                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+
+                    realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable, position);
                     ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
                 } break;
             }
             //if we successfully attached the body, add a sync tree
-            if(rigidBody != null){
+            ServerPhysicsSyncTree.attachTree(rVal);
-                ServerPhysicsSyncTree.attachTree(rVal);
+            if(ServerGravityTree.hasServerGravityTree(rVal)){
-                if(ServerGravityTree.hasServerGravityTree(rVal)){
+                ServerGravityTree.getServerGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
-                    ServerGravityTree.getServerGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
-                }
             }
         }
         CollisionEngine.unlockOde();
     }
 
+    /**
+     * [SERVER ONLY] Attaches a collidable template to a given entity
+     * @param realm The realm the entity is inside of
+     * @param rVal The entity
+     * @param physicsTemplate The collidable template
+     * @return The geometry object
+     */
+    public static DGeom serverAttachGeom(Realm realm, Entity rVal, CollidableTemplate physicsTemplate, Vector3d position){
+        if(physicsTemplate == null){
+            throw new Error("Physics template is null!");
+        }
+        Collidable collidable;
+        double mass = 1.0f;
+        long categoryBit = Collidable.TYPE_CREATURE_BIT;
+        if(physicsTemplate.getKinematic()){
+            categoryBit = Collidable.TYPE_STATIC_BIT;
+        }
+        CollisionEngine.lockOde();
+        DGeom geom = null;
+        switch(physicsTemplate.getType()){
+            case CollidableTemplate.COLLIDABLE_TYPE_CYLINDER: {
+
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCylinderShape(
+                    realm.getCollisionEngine(),
+                    physicsTemplate.getDimension1(),
+                    physicsTemplate.getDimension2(),
+                    categoryBit
+                );
+
+                //
+                //create collidable and attach tracking
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,geom);
+                PhysicsEntityUtils.setDGeom(rVal, geom);
+
+                //
+                //store data
+                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
+                    1, 1, 1 //scale
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
+                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
+
+                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
+                realm.getCollisionEngine().registerCollisionObject(geom, collidable, position);
+            } break;
+            case CollidableTemplate.COLLIDABLE_TYPE_CUBE: {
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCubeShape(
+                    realm.getCollisionEngine(),
+                    new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
+                    categoryBit
+                );
+
+                //
+                //create collidable and attach tracking
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,geom);
+                PhysicsEntityUtils.setDGeom(rVal, geom);
+
+                //
+                //store data
+                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
+                    1, 1, 1 //scale
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
+                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
+
+                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
+                realm.getCollisionEngine().registerCollisionObject(geom, collidable, position);
+            } break;
+            case CollidableTemplate.COLLIDABLE_TYPE_CAPSULE: {
+                //
+                //create dbody
+                geom = CollisionBodyCreation.createCapsuleShape(
+                    realm.getCollisionEngine(),
+                    physicsTemplate.getDimension1(),
+                    physicsTemplate.getDimension2(),
+                    categoryBit
+                );
+
+                //
+                //create collidable and attach tracking
+                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
+                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,geom);
+                PhysicsEntityUtils.setDGeom(rVal, geom);
+
+                //
+                //store data
+                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
+                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
+                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
+                    1, 1, 1 //scale
+                );
+                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
+                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
+                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
+                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);
+                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);
+
+                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
+                realm.getCollisionEngine().registerCollisionObject(geom, collidable, position);
+            } break;
+        }
+        ServerPhysicsSyncTree.attachTree(rVal);
+        if(ServerGravityTree.hasServerGravityTree(rVal)){
+            ServerGravityTree.getServerGravityTree(rVal).updatePhysicsPair(PhysicsEntityUtils.getCollidable(rVal),PhysicsEntityUtils.getDBody(rVal));
+        }
+        CollisionEngine.unlockOde();
+        return geom;
+    }
+
 
     /**
      * [CLIENT ONLY] Given an entity and a terrain chunk description, create physics for the chunk and attach it to the entity
@@ -718,7 +766,7 @@ public class PhysicsEntityUtils {
         DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), data, Collidable.TYPE_STATIC_BIT);
         CollisionBodyCreation.setAutoDisable(Globals.clientState.clientSceneWrapper.getCollisionEngine(), terrainBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, false);
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable, EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDBody(terrain,terrainBody);
         CollisionEngine.unlockOde();
         terrain.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
@@ -735,7 +783,7 @@ public class PhysicsEntityUtils {
         DGeom terrainGeom = CollisionBodyCreation.generateGeomFromTerrainData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), data, Collidable.TYPE_STATIC_BIT);
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, true);
         PhysicsEntityUtils.setCollidable(terrain, collidable);
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainGeom, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainGeom, collidable, EntityUtils.getPosition(terrain));
         CollisionEngine.unlockOde();
         PhysicsEntityUtils.setDGeom(terrain,terrainGeom);
     }
@@ -750,7 +798,7 @@ public class PhysicsEntityUtils {
         DBody terrainBody = CollisionBodyCreation.generateBodyFromMultiShapeMeshData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), data, Collidable.TYPE_STATIC_BIT);
         CollisionBodyCreation.setAutoDisable(Globals.clientState.clientSceneWrapper.getCollisionEngine(), terrainBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, false);
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable, EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDBody(terrain,terrainBody);
         terrain.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
     }
@@ -765,7 +813,7 @@ public class PhysicsEntityUtils {
         DGeom terrainBody = CollisionBodyCreation.generateColliderFromMultiShapeMeshData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), data, Collidable.TYPE_STATIC_BIT);
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, false);
         PhysicsEntityUtils.setCollidable(terrain, collidable);
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable, EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDGeom(terrain,terrainBody);
     }
 
@@ -781,7 +829,7 @@ public class PhysicsEntityUtils {
         DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(realm.getCollisionEngine(),data,Collidable.TYPE_STATIC_BIT);
         CollisionBodyCreation.setAutoDisable(realm.getCollisionEngine(), terrainBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);
         
-        realm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_STATIC, false));
+        realm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_STATIC, false), EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDBody(terrain,terrainBody);
         
         return terrainBody;
@@ -799,7 +847,7 @@ public class PhysicsEntityUtils {
         
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, true);
         PhysicsEntityUtils.setCollidable(terrain, collidable);
-        realm.getCollisionEngine().registerCollisionObject(terrainCollider, collidable);
+        realm.getCollisionEngine().registerCollisionObject(terrainCollider, collidable, EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDGeom(terrain,terrainCollider);
         
         return terrainCollider;
@@ -816,7 +864,7 @@ public class PhysicsEntityUtils {
         DBody terrainBody = CollisionBodyCreation.generateBodyFromMultiShapeMeshData(realm.getCollisionEngine(),data,Collidable.TYPE_STATIC_BIT);
         CollisionBodyCreation.setAutoDisable(realm.getCollisionEngine(), terrainBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);
         
-        realm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_STATIC, false));
+        realm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_STATIC, false), EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDBody(terrain,terrainBody);
         
         return terrainBody;
@@ -834,7 +882,7 @@ public class PhysicsEntityUtils {
         
         Collidable collidable = new Collidable(terrain,Collidable.TYPE_STATIC, false);
         PhysicsEntityUtils.setCollidable(terrain, collidable);
-        realm.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
+        realm.getCollisionEngine().registerCollisionObject(terrainBody, collidable, EntityUtils.getPosition(terrain));
         PhysicsEntityUtils.setDGeom(terrain,terrainBody);
         
         return terrainBody;
@@ -858,6 +906,10 @@ public class PhysicsEntityUtils {
             if(body != null && body.isEnabled() && !body.isKinematic()){
                 toReposition.add(entity);
             }
+            DGeom geom = PhysicsEntityUtils.getDGeom(entity);
+            if(geom != null && geom.getCategoryBits() != Collidable.TYPE_STATIC_BIT){
+                toReposition.add(entity);
+            }
         }
         ServerWorldData worldDat = realm.getServerWorldData();
         for(Entity parent : toReposition){
@@ -881,14 +933,7 @@ public class PhysicsEntityUtils {
      */
     public static void serverDestroyPhysics(Entity entity){
         Realm realm = Globals.serverState.realmManager.getEntityRealm(entity);
-        if(PhysicsEntityUtils.containsDBody(entity)){
+        realm.getCollisionEngine().destroyPhysics(entity);
-            PhysicsUtils.destroyPhysicsPair(
-                realm.getCollisionEngine(),
-                PhysicsEntityUtils.getDBody(entity),
-                PhysicsEntityUtils.getCollidable(entity)
-            );
-        }
-        PhysicsEntityUtils.setCollidable(entity, null);
         Vector3d entityPos = EntityUtils.getPosition(entity);
         ServerEntityUtils.repositionEntity(entity, entityPos);
     }
@@ -962,6 +1007,18 @@ public class PhysicsEntityUtils {
         return (DGeom)entity.getData(EntityDataStrings.PHYSICS_GEOM);
     }
 
+    /**
+     * Clears the geom and body on an entity
+     * @param entity The entity
+     */
+    public static void clearGeomAndBody(Entity entity){
+        if(PhysicsEntityUtils.getCollidable(entity) != null){
+            throw new Error("Trying to clear geom and body on an entity that still has a collidable!");
+        }
+        entity.removeData(EntityDataStrings.PHYSICS_COLLISION_BODY);
+        entity.removeData(EntityDataStrings.PHYSICS_GEOM);
+    }
+
     /**
      * Sets the position of a DGeom
      * @param collisionEngine the collision engine

src/main/java/electrosphere/collision/RayCastCallback.java

@@ -98,8 +98,8 @@ void RayCallback(void *Data, dGeomID Geometry1, dGeomID Geometry2) {
         Globals.profiler.beginAggregateCpuSample("RayCastCallback - try collisions");
         if(
             rayCastData.collidableTypeMask == null ||
-            (o1 instanceof DRay && rayCastData.collidableTypeMask.contains(collidable2.getType())) ||
+            (o1 instanceof DRay && collidable2 != null && rayCastData.collidableTypeMask.contains(collidable2.getType())) ||
-            (o2 instanceof DRay && rayCastData.collidableTypeMask.contains(collidable1.getType()))
+            (o2 instanceof DRay && collidable1 != null && rayCastData.collidableTypeMask.contains(collidable1.getType()))
         ){
             //creates a buffer to store potential collisions
             DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);   // up to MAX_CONTACTS contacts per box-box

src/main/java/electrosphere/collision/collidable/Collidable.java

@@ -37,6 +37,11 @@ public class Collidable {
      * The params for the surface of this collidable when a collision occurs
      */
     private SurfaceParams surfaceParams;
+
+    /**
+     * Tracks whether this collidable has been simulated or not
+     */
+    private boolean ready = false;
     
     //these should have corresponding category bits along with them
     public static final String TYPE_STATIC = "static";
@@ -122,6 +127,24 @@ public class Collidable {
     public void clear(){
         impulses.clear();
     }
+
+    /**
+     * Gets whether the collidable is ready or not
+     * @return true if it is ready, false otherwise
+     */
+    public boolean getReady() {
+        return ready;
+    }
+
+    /**
+     * Sets the ready status of collidable
+     * @param ready true if the collidable is ready, false otherwise
+     */
+    public void setReady(boolean ready) {
+        this.ready = ready;
+    }
+
+    
     
     
     

src/main/java/electrosphere/collision/hitbox/HitboxManager.java

@@ -35,7 +35,7 @@ public class HitboxManager {
      * @param resolutionCallback The callback that fires when a collision occurs
      */
     public HitboxManager(CollisionResolutionCallback resolutionCallback){
-        collisionEngine = new CollisionEngine();
+        collisionEngine = new CollisionEngine("hitbox");
         collisionEngine.setCollisionResolutionCallback(resolutionCallback);
     }
     

src/main/java/electrosphere/data/entity/collidable/CollidableTemplate.java

@@ -1,5 +1,7 @@
 package electrosphere.data.entity.collidable;
 
+import org.joml.Vector3d;
+
 /**
  * A template for a rigid body that should be attached to an entity
  */
@@ -105,6 +107,26 @@ public class CollidableTemplate {
      */
     String collisionType;
 
+    /**
+     * Base constructor
+     */
+    public CollidableTemplate(){
+    }
+
+    /**
+     * Gets a box template
+     * @param dims The dimensions of the box
+     * @return The template
+     */
+    public static CollidableTemplate getBoxTemplate(Vector3d dims){
+        CollidableTemplate rVal = new CollidableTemplate();
+        rVal.type = CollidableTemplate.COLLIDABLE_TYPE_CUBE;
+        rVal.dimension1 = (float)dims.x;
+        rVal.dimension2 = (float)dims.y;
+        rVal.dimension3 = (float)dims.z;
+        return rVal;
+    }
+
     /**
      * The primitive shape type
      * @return The primitive shape

src/main/java/electrosphere/engine/loadingthreads/ClientLoading.java

@@ -280,7 +280,7 @@ public class ClientLoading {
         DrawableUtils.disableCulling(skybox);
         EntityUtils.getScale(skybox).mul(SKYSPHERE_SCALE);
         Globals.clientState.clientScene.registerBehaviorTree(() -> {
-            EntityUtils.getPosition(skybox).set(EntityUtils.getPosition(Globals.clientState.playerEntity));
+            EntityUtils.setPosition(skybox, EntityUtils.getPosition(Globals.clientState.playerEntity));
         });
         Globals.assetManager.queueOverrideMeshShader(EntityUtils.getActor(skybox).getBaseModelPath(), GeometryMeshGen.SPHERE_MESH_NAME, AssetDataStrings.SHADER_SKYBOX_VERT, AssetDataStrings.SHADER_SKYBOX_FRAG);
 

src/main/java/electrosphere/engine/loadingthreads/LoadingUtils.java

@@ -160,7 +160,7 @@ public class LoadingUtils {
         }
         //set player character template
         serverPlayerConnection.setCreatureTemplate(template);
-        Character chara = Globals.serverState.characterService.createCharacter(template, serverPlayerConnection.getPlayerId());
+        Character chara = Globals.serverState.characterService.createCharacter(template, serverPlayerConnection.getPlayerId(), new Vector3d());
         Globals.clientState.clientConnection.queueOutgoingMessage(CharacterMessage.constructRequestSpawnCharacterMessage(chara.getId() + ""));
 
         //set player world-space coordinates

src/main/java/electrosphere/engine/time/Timekeeper.java

@@ -9,34 +9,54 @@ import electrosphere.engine.EngineState;
  */
 public class Timekeeper {
 
-    //the time a single simulation frame should simulate for (this is fixed)
+    /**
-    double simFrameTime = 0.0;
+     * the time a single simulation frame should simulate for (this is fixed)
+     */
+    private double simFrameTime = 0.0;
 
-    //the time that the system started (0 if using glfw reference, current system time if using java reference)
+    /**
-    double engineStartTime = 0.0;
+     * the time that the system started (0 if using glfw reference, current system time if using java reference)
+     */
+    private double engineStartTime = 0.0;
 
-    //the system time at the last call to update()
+    /**
-    double currentTime = 0.0;
+     * the system time at the last call to update()
+     */
+    private double currentTime = 0.0;
 
-    //accumulates time between current frame and next frame
+    /**
-    double frameAccumulator = 0.0;
+     * accumulates time between current frame and next frame
+     */
+    private double frameAccumulator = 0.0;
 
-    //the number of frames that have elapsed
+    /**
-    long numberOfSimFramesElapsed = 0;
+     * the number of frames that have elapsed
+     */
+    private long numberOfSimFramesElapsed = 0;
 
-    //the number of times the render pipeline has rendered a frame
+    /**
+     * the number of times the render pipeline has rendered a frame
+     */
     public long numberOfRenderedFrames = 0;
 
-    //the raw (not simulation) frametime of the most recent frame
+    /**
-    double mostRecentRawFrametime = 0;
+     * the raw (not simulation) frametime of the most recent frame
+     */
+    private double mostRecentRawFrametime = 0;
 
-    //The maximum amount of time that can overflow (ie cause more than one sim frame/render frame) before the sim frames are tossed out
+    /**
-    static double overflowMax = 20;
+     * The maximum amount of time that can overflow (ie cause more than one sim frame/render frame) before the sim frames are tossed out
+     */
+    private static double overflowMax = 20;
 
-    //the maximum number of simulation frames that can happen in a row before the main loop immediately skips more
+    /**
+     * the maximum number of simulation frames that can happen in a row before the main loop immediately skips more
+     */
     public static final int SIM_FRAME_HARDCAP = 3;
 
-    //step interval time size (for physics)
+    /**
+     * step interval time size (for physics)
+     */
     public static final float ENGINE_STEP_SIZE = 0.01f;
 
 

src/main/java/electrosphere/entity/ClientEntityUtils.java

@@ -37,10 +37,10 @@ public class ClientEntityUtils {
      * @param entity
      * @param position
      */
-    public static void reositionEntity(Entity entity, Vector3d position, Quaterniond rotation){
+    public static void repositionEntity(Entity entity, Vector3d position, Quaterniond rotation){
         //reposition entity
         CollisionObjUtils.clientPositionCharacter(entity, position, rotation);
-        EntityUtils.getPosition(entity).set(position);
+        EntityUtils.setPosition(entity, position);
     }
 
     /**

src/main/java/electrosphere/entity/DrawableUtils.java

@@ -115,7 +115,9 @@ public class DrawableUtils {
         });
         String path = Globals.assetManager.queuedAsset(model);
         entity.putData(EntityDataStrings.DATA_STRING_ACTOR, ActorUtils.createActorOfLoadingModel(path));
-        entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        if(!entity.containsKey(EntityDataStrings.DATA_STRING_POSITION)){
+            entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        }
         entity.putData(EntityDataStrings.DATA_STRING_ROTATION, new Quaterniond().identity());
         entity.putData(EntityDataStrings.DATA_STRING_SCALE, new Vector3f(1,1,1));
         entity.putData(EntityDataStrings.DATA_STRING_DRAW, true);
@@ -132,7 +134,9 @@ public class DrawableUtils {
      */
     public static void makeEntityDrawable(Entity entity, String path){
         entity.putData(EntityDataStrings.DATA_STRING_ACTOR, ActorUtils.createActorFromModelPath(path));
-        entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        if(!entity.containsKey(EntityDataStrings.DATA_STRING_POSITION)){
+            entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        }
         entity.putData(EntityDataStrings.DATA_STRING_ROTATION, new Quaterniond().identity());
         entity.putData(EntityDataStrings.DATA_STRING_SCALE, new Vector3f(1,1,1));
         entity.putData(EntityDataStrings.DATA_STRING_DRAW, true);

src/main/java/electrosphere/entity/EntityCreationUtils.java

@@ -131,7 +131,9 @@ public class EntityCreationUtils {
      */
     public static void makeEntityPoseable(Entity entity, String modelPath){
         entity.putData(EntityDataStrings.POSE_ACTOR, PoseActorUtils.createPoseActorFromModelPath(modelPath));
-        entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        if(!entity.containsKey(EntityDataStrings.DATA_STRING_POSITION)){
+            entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        }
         entity.putData(EntityDataStrings.DATA_STRING_ROTATION, new Quaterniond().identity());
         entity.putData(EntityDataStrings.DATA_STRING_SCALE, new Vector3f(1,1,1));
         entity.putData(EntityDataStrings.DATA_STRING_DRAW, true);
@@ -147,7 +149,9 @@ public class EntityCreationUtils {
      */
     public static void makeEntityDrawable(Entity entity, String modelPath){
         entity.putData(EntityDataStrings.DATA_STRING_ACTOR, ActorUtils.createActorFromModelPath(modelPath));
-        entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        if(!entity.containsKey(EntityDataStrings.DATA_STRING_POSITION)){
+            entity.putData(EntityDataStrings.DATA_STRING_POSITION, new Vector3d(0,0,0));
+        }
         entity.putData(EntityDataStrings.DATA_STRING_ROTATION, new Quaterniond().identity());
         entity.putData(EntityDataStrings.DATA_STRING_SCALE, new Vector3f(1,1,1));
         entity.putData(EntityDataStrings.DATA_STRING_DRAW, true);

src/main/java/electrosphere/entity/EntityUtils.java

@@ -16,8 +16,22 @@ import org.joml.Vector3f;
  */
 public class EntityUtils {
     
+    /**
+     * Gets the position of the entity
+     * @param e The entity
+     * @return The position of the entity
+     */
     public static Vector3d getPosition(Entity e){
-        return (Vector3d)e.getData(EntityDataStrings.DATA_STRING_POSITION);
+        return new Vector3d((Vector3d)e.getData(EntityDataStrings.DATA_STRING_POSITION));
+    }
+
+    /**
+     * Sets the position of the entity
+     * @param e The entity
+     * @param newVec The new position
+     */
+    public static void setPosition(Entity e, Vector3d newVec){
+        ((Vector3d)e.getData(EntityDataStrings.DATA_STRING_POSITION)).set(newVec);
     }
     
     public static Quaterniond getRotation(Entity e){

src/main/java/electrosphere/entity/ServerEntityUtils.java

@@ -40,10 +40,20 @@ public class ServerEntityUtils {
         if(position == null){
             throw new Error("Trying to set server entity position to null!");
         }
+        double startX = position.x;
+        double startY = position.y;
+        double startZ = position.z;
         //reposition entity, if the position isn't correct then it will spawn at 0,0,0 when the synchronization part is called
         CollisionObjUtils.serverPositionCharacter(entity, position);
         //get current server data cell
         ServerDataCell cell = realm.getDataCellManager().getDataCellAtPoint(position);
+        if(startX != position.x || startX != position.x || startX != position.x){
+            throw new Error("Position not preserved while initially positioning entity! " + startX + "," + startY + "," + startZ + "   " + position.x + "," + position.y + "," + position.z);
+        }
+        Vector3d entPos = EntityUtils.getPosition(entity);
+        if(startX != entPos.x || startX != entPos.x || startX != entPos.x){
+            throw new Error("Position not preserved while initially positioning entity! " + startX + "," + startY + "," + startZ + "   " + entPos.x + "," + entPos.y + "," + entPos.z);
+        }
         if(cell != null){
             //initialize server datacell tracking of this entity
             realm.initializeServerSideEntity(entity, cell);
@@ -56,6 +66,9 @@ public class ServerEntityUtils {
             //initialize server datacell tracking of this entity
             realm.initializeServerSideEntity(entity, cell);
         }
+        if(startX != position.x || startX != position.x || startX != position.x){
+            throw new Error("Position not preserved while initially positioning entity! " + startX + "," + startY + "," + startZ + "   " + position.x + "," + position.y + "," + position.z);
+        }
     }
     
     /**
@@ -70,6 +83,10 @@ public class ServerEntityUtils {
         if(AttachUtils.getParent(entity) != null){
             throw new Error("Trying to reposition attached entity!");
         }
+        double startX = position.x;
+        double startY = position.y;
+        double startZ = position.z;
+
         Realm realm = Globals.serverState.realmManager.getEntityRealm(entity);
         ServerWorldData worldDat = realm.getServerWorldData();
         if(
@@ -85,6 +102,11 @@ public class ServerEntityUtils {
         ServerEntityUtils.repositionEntityRecursive(realm, entity, position);
         //reposition entity
         CollisionObjUtils.serverPositionCharacter(entity, position);
+
+        //error checking
+        if(position.x != startX || position.y != startY || position.z != startZ){
+            throw new Error("Position mutated while repositioning! " + position + "  " + startX + "," + startY + "," + startZ);
+        }
     }
 
     /**

src/main/java/electrosphere/entity/scene/Scene.java

@@ -57,7 +57,7 @@ public class Scene {
     /**
      * Lock for threadsafeing the scene
      */
-    ReentrantLock lock = new ReentrantLock();
+    private ReentrantLock lock = new ReentrantLock();
     
     /**
      * Constructor

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

@@ -66,6 +66,8 @@ public class ClientCollidableTree implements BehaviorTree {
         }
         PhysicsUtils.setRigidBodyTransform(Globals.clientState.clientSceneWrapper.getCollisionEngine(), newPosition, rotation, body);
 
+        collidable.setReady(true);
+
         //capsule-specific block collision logic
         // if(body.isEnabled() && body.getFirstGeom() != null && (body.getFirstGeom() instanceof DCapsule)){
         //     //get capsule params

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

@@ -8,6 +8,7 @@ import electrosphere.entity.state.gravity.ServerGravityTree;
 import electrosphere.entity.state.movement.fall.ServerFallTree;
 
 import org.ode4j.ode.DBody;
+import org.ode4j.ode.DGeom;
 
 /**
  * Server collidable tree
@@ -41,6 +42,17 @@ public class ServerCollidableTree implements BehaviorTree {
         this.collidable = collidable;
         this.body = body;
     }
+
+    /**
+     * Constructor
+     * @param e The entity
+     * @param collidable The collidable
+     * @param geom The Geom
+     */
+    public ServerCollidableTree(Entity e, Collidable collidable, DGeom geom){
+        parent = e;
+        this.collidable = collidable;
+    }
     
     /**
      * Simulates the collidable tree
@@ -59,6 +71,8 @@ public class ServerCollidableTree implements BehaviorTree {
                 this.resetGravityFall();
             }
         }
+
+        collidable.setReady(true);
         
         //capsule-specific block collision logic
         // if(body.isEnabled() && body.getFirstGeom() != null && (body.getFirstGeom() instanceof DCapsule)){

src/main/java/electrosphere/entity/state/furniture/ServerDoorState.java

@@ -86,7 +86,7 @@ public class ServerDoorState implements BehaviorTree {
                     this.setState(DoorState.CLOSED);
                     Realm parentRealm = Globals.serverState.realmManager.getEntityRealm(this.parent);
                     Vector3d pos = EntityUtils.getPosition(this.parent);
-                    PhysicsEntityUtils.serverAttachCollidableTemplate(parentRealm, this.parent, PhysicsEntityUtils.getPhysicsTemplate(this.parent));
+                    PhysicsEntityUtils.serverAttachCollidableTemplate(parentRealm, this.parent, PhysicsEntityUtils.getPhysicsTemplate(this.parent), pos);
                     ServerEntityUtils.repositionEntity(this.parent, pos);
                 }
             ),

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

@@ -284,7 +284,8 @@ public class HitboxCollectionState {
 
         //register collidable with collision engine
         this.collidable = new Collidable(this.parent, Collidable.TYPE_OBJECT, true);
-        collisionEngine.registerCollisionObject(this.body, this.collidable);
+        Vector3d entPos = EntityUtils.getPosition(this.parent);
+        collisionEngine.registerCollisionObject(this.body, this.collidable, entPos);
         CollisionEngine.unlockOde();
     }
 

src/main/java/electrosphere/entity/state/inventory/ServerInventoryState.java

@@ -350,7 +350,6 @@ public class ServerInventoryState implements BehaviorTree {
         inventory.addItem(inventoryItem);
         //if we are the server, immediately send required packets
         ServerDataCell dataCell = DataCellSearchUtils.getEntityDataCell(item);
-        // ServerDataCell dataCell = Globals.dataCellLocationResolver.getDataCellAtPoint(EntityUtils.getPosition(item),item);
         //broadcast destroy entityq
         dataCell.broadcastNetworkMessage(EntityMessage.constructDestroyMessage(item.getId()));
 

src/main/java/electrosphere/entity/state/lod/ClientLODComponent.java

@@ -13,6 +13,7 @@ import electrosphere.entity.Entity;
 import electrosphere.net.synchronization.enums.BehaviorTreeIdEnums;
 import electrosphere.entity.btree.BehaviorTree;
 import electrosphere.entity.types.common.CommonEntityUtils;
+import electrosphere.entity.types.creature.CreatureUtils;
 import electrosphere.net.synchronization.annotation.SyncedField;
 import electrosphere.net.synchronization.annotation.SynchronizedBehaviorTree;
 
@@ -57,7 +58,7 @@ public class ClientLODComponent implements BehaviorTree {
             ){
                 CollidableTemplate physicsTemplate = commonData.getCollidable();
                 PhysicsEntityUtils.clientAttachCollidableTemplate(this.parent, physicsTemplate);
-                ClientEntityUtils.reositionEntity(parent, EntityUtils.getPosition(parent), EntityUtils.getRotation(parent));
+                ClientEntityUtils.repositionEntity(parent, EntityUtils.getPosition(parent), EntityUtils.getRotation(parent));
             }
         }
         if(cachedLodLevel != lodLevel){
@@ -66,7 +67,7 @@ public class ClientLODComponent implements BehaviorTree {
                 CommonEntityType type = CommonEntityUtils.getCommonData(this.parent);
                 if(type.getCollidable() != null && PhysicsEntityUtils.getCollidable(this.parent) == null){
                     PhysicsEntityUtils.clientAttachCollidableTemplate(parent, type.getCollidable());
-                    ClientEntityUtils.reositionEntity(parent, EntityUtils.getPosition(parent), EntityUtils.getRotation(parent));
+                    ClientEntityUtils.repositionEntity(parent, EntityUtils.getPosition(parent), EntityUtils.getRotation(parent));
                 }
             } else if(cachedLodLevel == ServerLODComponent.LOW_RES){
                 if(PhysicsEntityUtils.containsDBody(this.parent)){
@@ -76,6 +77,13 @@ public class ClientLODComponent implements BehaviorTree {
                         PhysicsEntityUtils.getCollidable(this.parent)
                     );
                 }
+                CommonEntityType type = CommonEntityUtils.getCommonData(this.parent);
+                if(type.getCollidable() != null){
+                    if(CreatureUtils.hasControllerPlayerId(parent)){
+                        throw new Error("Should not be attaching a geometry to a player entity!");
+                    }
+                    PhysicsEntityUtils.clientAttachGeom(parent, type.getCollidable(), EntityUtils.getPosition(parent));
+                }
             }
         }
     }

src/main/java/electrosphere/entity/state/lod/ServerLODComponent.java

@@ -17,6 +17,7 @@ import electrosphere.net.synchronization.enums.BehaviorTreeIdEnums;
 import electrosphere.server.datacell.utils.ServerBehaviorTreeUtils;
 import electrosphere.entity.btree.BehaviorTree;
 import electrosphere.entity.types.common.CommonEntityUtils;
+import electrosphere.entity.types.creature.CreatureUtils;
 import electrosphere.net.synchronization.annotation.SyncedField;
 import electrosphere.net.synchronization.annotation.SynchronizedBehaviorTree;
 
@@ -63,7 +64,8 @@ public class ServerLODComponent implements BehaviorTree {
                 Realm realm = Globals.serverState.realmManager.getEntityRealm(this.parent);
                 CommonEntityType type = CommonEntityUtils.getCommonData(this.parent);
                 if(type.getCollidable() != null){
-                    PhysicsEntityUtils.serverAttachCollidableTemplate(realm, this.parent, type.getCollidable());
+                    PhysicsEntityUtils.serverDestroyPhysics(this.parent);
+                    PhysicsEntityUtils.serverAttachCollidableTemplate(realm, this.parent, type.getCollidable(),EntityUtils.getPosition(parent));
                 }
             }
         } else {
@@ -71,6 +73,14 @@ public class ServerLODComponent implements BehaviorTree {
                 //make low res
                 this.setLodLevel(LOW_RES);
                 PhysicsEntityUtils.serverDestroyPhysics(this.parent);
+                Realm realm = Globals.serverState.realmManager.getEntityRealm(parent);
+                CommonEntityType type = CommonEntityUtils.getCommonData(this.parent);
+                if(type.getCollidable() != null){
+                    if(CreatureUtils.hasControllerPlayerId(parent)){
+                        throw new Error("Should not be attaching a geometry to a player entity!");
+                    }
+                    PhysicsEntityUtils.serverAttachGeom(realm, parent, type.getCollidable(), EntityUtils.getPosition(parent));
+                }
             }
         }
     }

src/main/java/electrosphere/entity/state/movement/groundmove/ClientGroundMovementTree.java

@@ -282,7 +282,7 @@ public class ClientGroundMovementTree implements BehaviorTree {
                         //this should only fire on the client, we don't want the server snap updating due to client position reporting
                         lastServerPosition = new Vector3d(message.getpositionX(),message.getpositionY(),message.getpositionZ());
                         if(position.distance(lastServerPosition) > STATE_DIFFERENCE_HARD_UPDATE_THRESHOLD){
-                            EntityUtils.getPosition(parent).set(lastServerPosition);
+                            EntityUtils.setPosition(parent, lastServerPosition);
                         } else if(position.distance(lastServerPosition) > STATE_DIFFERENCE_SOFT_UPDATE_THRESHOLD){
                             EntityUtils.getPosition(parent).lerp(lastServerPosition,SOFT_UPDATE_MULTIPLIER);
                         }

src/main/java/electrosphere/entity/state/movement/groundmove/ServerGroundMovementTree.java

@@ -10,6 +10,7 @@ import electrosphere.collision.PhysicsEntityUtils;
 import electrosphere.collision.collidable.Collidable;
 import electrosphere.data.entity.creature.movement.GroundMovementSystem;
 import electrosphere.engine.Globals;
+import electrosphere.engine.time.Timekeeper;
 import electrosphere.entity.types.creature.CreatureUtils;
 import electrosphere.entity.Entity;
 import electrosphere.entity.EntityDataStrings;
@@ -291,7 +292,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
                 }
                 if(body == null){
                     Vector3d velVec = new Vector3d(movementVector);
-                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime());
+                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime() * Timekeeper.ENGINE_STEP_SIZE);
                     velVec.add(position);
                     ServerEntityUtils.repositionEntity(parent, velVec);
                 } else {
@@ -343,7 +344,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
                 float velocity = this.getModifiedVelocity();
                 if(body == null){
                     Vector3d velVec = new Vector3d(movementVector);
-                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime());
+                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime() * Timekeeper.ENGINE_STEP_SIZE);
                     velVec.add(position);
                     ServerEntityUtils.repositionEntity(parent, velVec);
                 } else {
@@ -414,7 +415,7 @@ public class ServerGroundMovementTree implements BehaviorTree {
                 }
                 if(body == null){
                     Vector3d velVec = new Vector3d(movementVector);
-                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime());
+                    velVec.mul(velocity * Globals.engineState.timekeeper.getSimFrameTime() * Timekeeper.ENGINE_STEP_SIZE);
                     velVec.add(position);
                     ServerEntityUtils.repositionEntity(parent, velVec);
                 } else {

src/main/java/electrosphere/entity/state/physicssync/ClientPhysicsSyncTree.java

@@ -3,6 +3,7 @@ package electrosphere.entity.state.physicssync;
 import org.joml.Quaterniond;
 import org.joml.Vector3d;
 import org.ode4j.ode.DBody;
+import org.ode4j.ode.DGeom;
 
 import electrosphere.client.entity.camera.CameraEntityUtils;
 import electrosphere.client.terrain.foliage.FoliageCellManager;
@@ -59,6 +60,7 @@ public class ClientPhysicsSyncTree implements BehaviorTree {
             Vector3d angularForce = new Vector3d();
             boolean enabled = latestMessage.getbodyEnabled();
             DBody body = PhysicsEntityUtils.getDBody(parent);
+            DGeom geom = PhysicsEntityUtils.getDGeom(parent);
 
             //
             //bust distance caches if this is the player's entity and we've traveled a long distance suddenly
@@ -75,9 +77,14 @@ public class ClientPhysicsSyncTree implements BehaviorTree {
 
             //
             //Synchronize data
-            EntityUtils.getPosition(parent).set(position);
+            EntityUtils.setPosition(parent, position);
             EntityUtils.getRotation(parent).set(rotationFromServer);
-            PhysicsUtils.synchronizeData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), body, position, rotationFromServer, linearVelocity, angularVelocity, linearForce, angularForce, enabled);
+            if(body != null){
+                PhysicsUtils.synchronizeData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), body, position, rotationFromServer, linearVelocity, angularVelocity, linearForce, angularForce, enabled);
+            }
+            if(geom != null){
+                PhysicsUtils.setGeomTransform(Globals.clientState.clientSceneWrapper.getCollisionEngine(), position, rotationFromServer, geom);
+            }
 
             //
             //update facing vector if relevant

src/main/java/electrosphere/entity/state/physicssync/ServerPhysicsSyncTree.java

@@ -3,6 +3,7 @@ package electrosphere.entity.state.physicssync;
 import org.joml.Quaterniond;
 import org.joml.Vector3d;
 import org.ode4j.ode.DBody;
+import org.ode4j.ode.DGeom;
 
 import electrosphere.collision.PhysicsEntityUtils;
 import electrosphere.collision.PhysicsUtils;
@@ -44,8 +45,8 @@ public class ServerPhysicsSyncTree implements BehaviorTree {
         Vector3d position = EntityUtils.getPosition(parent);
         Quaterniond rotation = EntityUtils.getRotation(parent);
         DBody body = PhysicsEntityUtils.getDBody(parent);
-        if(body == null){
+        DGeom geom = PhysicsEntityUtils.getDGeom(parent);
-        } else {
+        if(body != null){
             //velocities
             Vector3d linearVel = PhysicsUtils.odeVecToJomlVec(body.getLinearVel());
             Vector3d angularVel = PhysicsUtils.odeVecToJomlVec(body.getAngularVel());
@@ -83,6 +84,38 @@ public class ServerPhysicsSyncTree implements BehaviorTree {
                 );
             }
         }
+        if(geom != null){
+            if(position.distance(lastSentPosition) > UPDATE_THRESHOLD || 1.0 - rotation.dot(lastSentRotation) > UPDATE_THRESHOLD){
+                lastSentPosition.set(position);
+                lastSentRotation.set(rotation);
+                DataCellSearchUtils.getEntityDataCell(parent).broadcastNetworkMessage(
+                    EntityMessage.constructsyncPhysicsMessage(
+                        parent.getId(),
+                        Globals.engineState.timekeeper.getNumberOfSimFramesElapsed(),
+                        position.x,
+                        position.y,
+                        position.z,
+                        rotation.x,
+                        rotation.y,
+                        rotation.z,
+                        rotation.w,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        0,
+                        false
+                    )
+                );
+            }
+        }
     }
 
     /**

src/main/java/electrosphere/entity/state/physicssync/upright/ClientAlwaysUprightTree.java

@@ -41,7 +41,7 @@ public class ClientAlwaysUprightTree implements BehaviorTree {
             //make sure rotation is vertical
             // sourceRotation = sourceRotation.mul(0.001, 1, 0.001, 1).normalize();
 
-            EntityUtils.getPosition(parent).set(position);
+            EntityUtils.setPosition(parent, position);
             EntityUtils.getRotation(parent).set(sourceRotation);
             PhysicsUtils.synchronizeData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce);
         }

src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java

@@ -51,7 +51,7 @@ public class ServerAlwaysUprightTree implements BehaviorTree {
                 sourceRotation = new Quaterniond().rotationTo(SpatialMathUtils.getOriginVector(), new Vector3d(facingVector.x,0,facingVector.z)).normalize();
             }
 
-            EntityUtils.getPosition(parent).set(position);
+            EntityUtils.setPosition(parent, position);
             EntityUtils.getRotation(parent).set(sourceRotation);
             PhysicsUtils.synchronizeData(realm.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce);
         }

src/main/java/electrosphere/entity/types/collision/CollisionObjUtils.java

@@ -32,7 +32,7 @@ public class CollisionObjUtils {
         Vector3d position = EntityUtils.getPosition(entity);
         Quaterniond rotation = EntityUtils.getRotation(entity);
         Collidable collidable = new Collidable(entity, collidableType, true);
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(collisionObject, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(collisionObject, collidable, EntityUtils.getPosition(entity));
 
         PhysicsEntityUtils.setDBody(entity, collisionObject);
 
@@ -55,7 +55,7 @@ public class CollisionObjUtils {
         Vector3d position = EntityUtils.getPosition(entity);
         Collidable collidable = new Collidable(entity, collidableType, true);
         Realm realm = Globals.serverState.realmManager.getEntityRealm(entity);
-        realm.getCollisionEngine().registerCollisionObject(collisionObject, collidable);
+        realm.getCollisionEngine().registerCollisionObject(collisionObject, collidable, EntityUtils.getPosition(entity));
 
         PhysicsEntityUtils.setDBody(entity, collisionObject);
 
@@ -73,7 +73,15 @@ public class CollisionObjUtils {
      * @param position The server
      */
     public static void serverPositionCharacter(Entity e, Vector3d position){
-        EntityUtils.getPosition(e).set(position);
+        CollisionEngine.lockOde();
+        double startX = position.x;
+        double startY = position.y;
+        double startZ = position.z;
+        EntityUtils.setPosition(e, position);
+        Vector3d entPos = EntityUtils.getPosition(e);
+        if(startX != entPos.x || startX != entPos.x || startX != entPos.x){
+            throw new Error("Failed to position entity! " + startX + "," + startY + "," + startZ + "   " + entPos.x + "," + entPos.y + "," + entPos.z);
+        }
         Quaterniond rotation = EntityUtils.getRotation(e);
         DBody body = PhysicsEntityUtils.getDBody(e);
         DGeom geom = PhysicsEntityUtils.getDGeom(e);
@@ -86,7 +94,7 @@ public class CollisionObjUtils {
             if(template == null){
                 PhysicsUtils.setGeomTransform(
                     collisionEngine,
-                    position,
+                    new Vector3d(position),
                     rotation,
                     geom
                 );
@@ -99,6 +107,14 @@ public class CollisionObjUtils {
                 );
             }
         }
+        // Vector3d finalRef = EntityUtils.getPosition(e);
+        // if(finalRef != entPos){
+        //     throw new Error("Reference changed while positioning character! " + entPos.x + "," + entPos.y + "," + entPos.z + "  " + finalRef.x + "," + finalRef.y + "," + finalRef.z);
+        // }
+        if(startX != entPos.x || startX != entPos.x || startX != entPos.x){
+            throw new Error("Position not preserved while positioning entity! " + startX + "," + startY + "," + startZ + "   " + entPos.x + "," + entPos.y + "," + entPos.z + "   " + position.x + "," + position.y + "," + position.z);
+        }
+        CollisionEngine.unlockOde();
     }
 
     /**
@@ -108,7 +124,7 @@ public class CollisionObjUtils {
      * @param rotation The rotation
      */
     public static void clientPositionCharacter(Entity e, Vector3d position, Quaterniond rotation){
-        EntityUtils.getPosition(e).set(position);
+        EntityUtils.setPosition(e, position);
         DBody body = PhysicsEntityUtils.getDBody(e);
         if(body != null){
             PhysicsUtils.setRigidBodyTransform(Globals.clientState.clientSceneWrapper.getCollisionEngine(), position, rotation, body);

src/main/java/electrosphere/entity/types/common/CommonEntityUtils.java

@@ -432,6 +432,10 @@ public class CommonEntityUtils {
      */
     public static Entity serverApplyCommonEntityTransforms(Realm realm, Vector3d position, Entity entity, CommonEntityType rawType){
 
+        double startX = position.x;
+        double startY = position.y;
+        double startZ = position.z;
+
         //
         //Set typing stuff
         //
@@ -498,6 +502,9 @@ public class CommonEntityUtils {
         // Hitbox stuff
         //
         //
+        if(position.x != startX || position.y != startY || position.z != startZ){
+            throw new Error("Position mutated while spawning entity!");
+        }
         if(rawType.getHitboxes() != null){
             HitboxCollectionState.attachHitboxState(realm.getHitboxManager(), true, entity, rawType.getHitboxes());
         }
@@ -507,9 +514,12 @@ public class CommonEntityUtils {
         //
         //
         if(rawType.getCollidable() != null){
+            //actually attach collidable
             CollidableTemplate physicsTemplate = rawType.getCollidable();
             if(Globals.serverState.lodEmitterService.isFullLod(position)){
-                PhysicsEntityUtils.serverAttachCollidableTemplate(realm, entity, physicsTemplate);
+                PhysicsEntityUtils.serverAttachCollidableTemplate(realm, entity, physicsTemplate,position);
+            } else {
+                PhysicsEntityUtils.serverAttachGeom(realm, entity, physicsTemplate, position);
             }
             ServerLODComponent.attachTree(entity);
         }

src/main/java/electrosphere/entity/types/creature/CreatureUtils.java

@@ -209,6 +209,11 @@ public class CreatureUtils {
      * @return The creature entity
      */
     public static Entity serverSpawnBasicCreature(Realm realm, Vector3d position, String type, ObjectTemplate template){
+
+        double posX = position.x;
+        double posY = position.y;
+        double posZ = position.z;
+
         CreatureData rawType = Globals.gameConfigCurrent.getCreatureTypeLoader().getType(type);
         Entity rVal = EntityCreationUtils.createServerEntity(realm, position);
 
@@ -287,6 +292,11 @@ public class CreatureUtils {
         storedTemplate.objectType = rawType.getId();
         //store template on creature
         CreatureUtils.setCreatureTemplate(rVal, storedTemplate);
+
+        //error check position modification
+        if(posX != position.x || posY != position.y || posZ != position.z){
+            throw new Error("Creature has mutated position! " + posX + "," + posY + "," + posZ + "     " + position.x + "," + position.y + "," + position.z);
+        }
         
 
         //position entity
@@ -300,6 +310,10 @@ public class CreatureUtils {
         if(Globals.serverState.realmManager.getEntityRealm(rVal) == null){
             LoggerInterface.loggerEngine.ERROR(new IllegalStateException("Created creature without it being assigned to a realm!"));
         }
+        //make sure position hasn't changed
+        if(posX != position.x || posY != position.y || posZ != position.z){
+            throw new Error("Creature has mutated position! " + posX + "," + posY + "," + posZ + "     " + position.x + "," + position.y + "," + position.z);
+        }
 
         return rVal;
     }

src/main/java/electrosphere/entity/types/terrain/BlockChunkEntity.java

@@ -79,7 +79,7 @@ public class BlockChunkEntity {
                         EntityCreationUtils.makeEntityDrawablePreexistingModel(solidsEnt, modelPath);
                         if(levelOfDetail == BlockChunkData.LOD_FULL_RES){
                             PhysicsEntityUtils.clientAttachTriGeomCollider(solidsEnt, data);
-                            ClientEntityUtils.reositionEntity(solidsEnt, new Vector3d(EntityUtils.getPosition(solidsEnt)), new Quaterniond());
+                            ClientEntityUtils.repositionEntity(solidsEnt, new Vector3d(EntityUtils.getPosition(solidsEnt)), new Quaterniond());
                         } else {
                             EntityCreationUtils.bypassShadowPass(solidsEnt);
                             EntityCreationUtils.bypassVolumetics(solidsEnt);

src/main/java/electrosphere/entity/types/tree/ProceduralTree.java

@@ -144,7 +144,7 @@ public class ProceduralTree {
         trunkChild.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
         trunkChild.putData(EntityDataStrings.PHYSICS_MASS, TREE_MASS);
 
-        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+        Globals.clientState.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(trunkChild));
 
 
         //generate branches
@@ -549,7 +549,7 @@ public class ProceduralTree {
         trunkChild.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
         trunkChild.putData(EntityDataStrings.PHYSICS_MASS, TREE_MASS);
 
-        realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
+        realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable, EntityUtils.getPosition(trunkChild));
 
         return trunkChild;
     }

src/main/java/electrosphere/net/server/player/Player.java

@@ -38,37 +38,37 @@ public class Player {
     /**
      * The corresponding connection handler
      */
-    ServerConnectionHandler connectionHandler;
+    private ServerConnectionHandler connectionHandler;
 
     /**
      * The id of the player
      */
-    int id;
+    private int id;
 
     /**
      * The database's id of the player
      */
-    int dbId;
+    private int dbId;
 
     /**
      * The world position of this player
      */
-    Vector3i worldPos;
+    private Vector3i worldPos;
 
     /**
      * The simulation radius of this player
      */
-    int simulationRadius = DEFAULT_SIMULATION_RADIUS;
+    private int simulationRadius = DEFAULT_SIMULATION_RADIUS;
 
     /**
      * The player's primary entity
      */
-    Entity playerEntity;
+    private Entity playerEntity;
 
     /**
      * Tracks whether the player's entity has been sent or not
      */
-    boolean hasSentPlayerEntity = false;
+    private boolean hasSentPlayerEntity = false;
     
     /**
      * Constructor

src/main/java/electrosphere/net/server/protocol/CharacterProtocol.java

@@ -69,8 +69,7 @@ public class CharacterProtocol implements ServerProtocolTemplate<CharacterMessag
             case REQUESTCREATECHARACTER: {
                 ObjectTemplate template = Utilities.deserialize(message.getdata(), ObjectTemplate.class);
                 if(template != null){
-                    Character charaData = Globals.serverState.characterService.createCharacter(template, connectionHandler.getPlayer().getDBID());
+                    Character charaData = Globals.serverState.characterService.createCharacter(template, connectionHandler.getPlayer().getDBID(),Globals.serverState.realmManager.first().getSpawnPoint());
-                    charaData.setPos(Globals.serverState.realmManager.first().getSpawnPoint());
                     connectionHandler.setCreatureTemplate(Utilities.deserialize(message.getdata(), ObjectTemplate.class));
                     connectionHandler.setCharacterId(charaData.getId());
                     connectionHandler.addMessagetoOutgoingQueue(CharacterMessage.constructResponseCreateCharacterSuccessMessage());

src/main/java/electrosphere/server/ai/nodes/plan/MacroPathfindingNode.java

@@ -145,9 +145,11 @@ public class MacroPathfindingNode implements AITreeNode {
             if(targetRaw instanceof Vector3d){
                 targetPos = (Vector3d)targetRaw;
             } else if(targetRaw instanceof Entity){
-                targetPos = EntityUtils.getPosition((Entity)targetRaw);
+                targetPos = new Vector3d(EntityUtils.getPosition((Entity)targetRaw));
-            } else if(targetRaw instanceof VirtualStructure){
+            } else if(targetRaw instanceof VirtualStructure struct){
-                targetPos = ((VirtualStructure)targetRaw).getPos();
+                targetPos = struct.getPos();
+            } else if(targetRaw instanceof MacroRegion region){
+                targetPos = region.getPos();
             } else {
                 throw new Error("Unsupported target type " + targetRaw);
             }
@@ -173,7 +175,7 @@ public class MacroPathfindingNode implements AITreeNode {
         if(macroData == null){
             throw new Error("Macro data undefined!");
         }
-        Vector3d entityPos = EntityUtils.getPosition(entity);
+        Vector3d entityPos = new Vector3d(EntityUtils.getPosition(entity));
         return macroData.getPathCache().getPathingNode(entityPos);
     }
 

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

@@ -10,6 +10,7 @@ import org.joml.Vector3d;
 
 import electrosphere.collision.CollisionEngine;
 import electrosphere.collision.CollisionWorldData;
+import electrosphere.collision.PhysicsCallback;
 import electrosphere.collision.hitbox.HitboxManager;
 import electrosphere.engine.Globals;
 import electrosphere.entity.Entity;
@@ -59,16 +60,17 @@ public class RealmManager {
      */
     public Realm createRealm(){
         //create chemistry engine
-        CollisionEngine chemistryEngine = new CollisionEngine();
+        CollisionEngine chemistryEngine = new CollisionEngine("serverChem");
         chemistryEngine.setCollisionResolutionCallback(new ServerChemistryCollisionCallback());
-        return new Realm(
+        Realm rVal = new Realm(
             new ServerWorldData(),
-            new CollisionEngine(),
+            CollisionEngine.create("serverPhysics", new PhysicsCallback()),
             chemistryEngine,
             new HitboxManager(new ServerHitboxResolutionCallback()),
             ServerContentManager.createServerContentManager(false, null),
             null
         );
+        return rVal;
     }
 
     /**
@@ -77,10 +79,10 @@ public class RealmManager {
      */
     public Realm createGriddedRealm(ServerWorldData serverWorldData, ServerContentManager serverContentManager){
         //create collision engine
-        CollisionEngine collisionEngine = new CollisionEngine();
+        CollisionEngine collisionEngine = CollisionEngine.create("serverPhysics", new PhysicsCallback());
         collisionEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
         //create chemistry engine
-        CollisionEngine chemistryEngine = new CollisionEngine();
+        CollisionEngine chemistryEngine = new CollisionEngine("serverChem");
         chemistryEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
         chemistryEngine.setCollisionResolutionCallback(new ServerChemistryCollisionCallback());
         //create realm
@@ -111,11 +113,11 @@ public class RealmManager {
         ServerWorldData serverWorldData = ServerWorldData.createFixedWorldData(minPoint, maxPoint);
 
         //create collision engine
-        CollisionEngine collisionEngine = new CollisionEngine();
+        CollisionEngine collisionEngine = CollisionEngine.create("serverPhysics", new PhysicsCallback());
         collisionEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
 
         //create chemistry engine
-        CollisionEngine chemistryEngine = new CollisionEngine();
+        CollisionEngine chemistryEngine = new CollisionEngine("serverChem");
         chemistryEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
         chemistryEngine.setCollisionResolutionCallback(new ServerChemistryCollisionCallback());
 

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

@@ -434,7 +434,8 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
                     }
                     if(cell.containsPlayer(player) && !this.shouldContainPlayer(distance, playerSimulationRadius)){
                         if(cell.getScene().containsEntity(player.getPlayerEntity())){
-                            throw new Error("Trying to remove player from a cell that contains its entity!");
+                            // throw new Error("Trying to remove player from a cell that contains its entity!");
+                            continue;
                         }
                         cell.removePlayer(player);
                         this.broadcastDestructionToPlayer(player, cell);
@@ -566,11 +567,21 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
                 //don't trigger the chunk to be re-created
                 Globals.profiler.beginCpuSample("GriddedDataCellManager.unloadPlayerlessChunks - Store data");
                 this.loaderService.queueLocationBasedOperation(key, () -> {
-                    serverContentManager.saveSerializationToDisk(key, serializedEntities);
+                    try {
-                    serverTerrainManager.savePositionToDisk(worldPos);
+                        serverContentManager.saveSerializationToDisk(key, serializedEntities);
+                        serverTerrainManager.savePositionToDisk(worldPos);
+                    } catch(Throwable e){
+                        e.printStackTrace();
+                    }
                 });
                 Globals.profiler.endCpuSample();
 
+                //destroy physics
+                PhysicsDataCell physicsCell = this.posPhysicsMap.get(key);
+                if(physicsCell != null){
+                    physicsCell.destroyPhysics();
+                }
+
                 //deregister from all tracking structures
                 parent.deregisterCell(cell);
                 groundDataCells.remove(key);
@@ -895,9 +906,7 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
         this.loaderService.queueLocationBasedOperation(key, () -> {
             try {
                 serverContentManager.generateContentForDataCell(parent, localWorldPos, rVal, cellKey);
-            } catch(Error e){
+            } catch(Throwable e){
-                e.printStackTrace();
-            } catch(Exception e){
                 e.printStackTrace();
             }
         });

src/main/java/electrosphere/server/datacell/physics/PhysicsDataCell.java

@@ -102,8 +102,10 @@ public class PhysicsDataCell {
      */
     public void destroyPhysics(){
         Realm realm = Globals.serverState.realmManager.getEntityRealm(physicsEntity);
-        realm.getCollisionEngine().destroyPhysics(physicsEntity);
+        if(realm != null){
-        realm.getCollisionEngine().destroyPhysics(blockPhysicsEntity);
+            realm.getCollisionEngine().destroyPhysics(physicsEntity);
+            realm.getCollisionEngine().destroyPhysics(blockPhysicsEntity);
+        }
     }
 
     /**

src/main/java/electrosphere/server/macro/character/CharacterUtils.java

@@ -99,8 +99,7 @@ public class CharacterUtils {
     public static Character spawnCharacter(Realm realm, Vector3d position, String race){
         Race raceData = Globals.gameConfigCurrent.getRaceMap().getRace(race);
         String creatureType = raceData.getAssociatedCreature();
-        Character rVal = Globals.serverState.characterService.createCharacter(ObjectTemplate.createDefault(EntityType.CREATURE, creatureType), CharacterService.NO_PLAYER);
+        Character rVal = Globals.serverState.characterService.createCharacter(ObjectTemplate.createDefault(EntityType.CREATURE, creatureType), CharacterService.NO_PLAYER, position);
-        rVal.setPos(position);
         Race.setRace(rVal, Race.create(race, creatureType));
         realm.getDataCellManager().evaluateMacroObject(rVal);
         return rVal;

src/main/java/electrosphere/server/macro/civilization/town/TownPopulator.java

@@ -30,12 +30,12 @@ public class TownPopulator {
         List<VirtualStructure> structs = town.getStructures(macroData);
         Random rand = new Random(town.getId());
         for(VirtualStructure struct : structs){
+            Vector3d placePos = new Vector3d(struct.getPos()).add(1,1,1);
             ObjectTemplate template = ObjectTemplate.create(EntityType.CREATURE, "human");
-            Character chara = Globals.serverState.characterService.createCharacter(template, CharacterService.NO_PLAYER);
+            Character chara = Globals.serverState.characterService.createCharacter(template, CharacterService.NO_PLAYER, placePos);
             Race.setRace(chara, Globals.gameConfigCurrent.getRaceMap().getRace("human"));
             CharacterUtils.addShelter(chara, struct);
             town.addResident(chara);
-            chara.setPos(new Vector3d(struct.getPos()).add(1,1,1));
         }
 
         //assign jobs to created characters

src/main/java/electrosphere/server/service/CharacterService.java

@@ -6,6 +6,7 @@ import java.util.List;
 import java.util.Map;
 import java.util.concurrent.locks.ReentrantLock;
 
+import org.joml.Vector3d;
 import org.joml.Vector3i;
 
 import com.google.gson.Gson;
@@ -61,13 +62,15 @@ public class CharacterService extends SignalServiceImpl {
      * Creates a character in the database
      * @param template The creature template for the character
      * @param playerId The player's id
+     * @param pos The position to place the character at
      */
-    public Character createCharacter(ObjectTemplate template, int playerId){
+    public Character createCharacter(ObjectTemplate template, int playerId, Vector3d pos){
         if(template == null){
             throw new Error("Template is null!");
         }
         lock.lock();
         Character toStore = new Character(template);
+        toStore.setPos(pos);
         toStore.setPlayerId(playerId);
         DatabaseResult result = Globals.serverState.dbController.executePreparedQuery(
             "INSERT INTO charaData (playerId,dataVal) VALUES (?,?) RETURNING id;",

src/test/java/electrosphere/collision/CollisionEngineStaticSpaceTests.java

@@ -0,0 +1,76 @@
+package electrosphere.collision;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.joml.Vector3d;
+
+import electrosphere.data.entity.collidable.CollidableTemplate;
+import electrosphere.engine.Globals;
+import electrosphere.entity.Entity;
+import electrosphere.entity.EntityCreationUtils;
+import electrosphere.server.datacell.Realm;
+import electrosphere.test.annotations.IntegrationTest;
+import electrosphere.test.template.EntityTestTemplate;
+
+/**
+ * Tests for specifically the static space geoms
+ */
+public class CollisionEngineStaticSpaceTests extends EntityTestTemplate {
+    
+    @IntegrationTest
+    public void staticSpaceTest1(){
+        Realm realm = Globals.serverState.realmManager.first();
+        CollisionEngine collisionEngine = realm.getCollisionEngine();
+
+        //base plane + static space
+        assertEquals(2, collisionEngine.getSpace().getNumGeoms());
+    }
+
+    @IntegrationTest
+    public void test_nearCollisionCount_1(){
+        Realm realm = Globals.serverState.realmManager.first();
+        CollisionEngine collisionEngine = realm.getCollisionEngine();
+
+        //simulate the physics
+        collisionEngine.simulatePhysics();
+
+        //base plane + static space  TIMES  number of calls to simulate physics
+        int expected = 0 * CollisionEngine.PHYSICS_SIMULATION_RESOLUTION;
+        assertEquals(expected, collisionEngine.getFinalCollisionCount());
+    }
+
+    @IntegrationTest
+    public void test_nearCollisionCount_2(){
+        Realm realm = Globals.serverState.realmManager.first();
+        CollisionEngine collisionEngine = realm.getCollisionEngine();
+
+        collisionEngine.createCubeGeom(new Vector3d(1,1,1), 0);
+
+        //simulate the physics
+        collisionEngine.simulatePhysics();
+
+        assertEquals(0, collisionEngine.getFinalCollisionCount());
+    }
+
+    @IntegrationTest
+    public void test_nearCollisionCount_3(){
+        Realm realm = Globals.serverState.realmManager.first();
+        CollisionEngine collisionEngine = realm.getCollisionEngine();
+
+        Entity ent1 = EntityCreationUtils.createServerEntity(realm, new Vector3d(0));
+        PhysicsEntityUtils.serverAttachGeom(realm, ent1, CollidableTemplate.getBoxTemplate(new Vector3d(1,1,1)), new Vector3d(0));
+
+        PhysicsEntityUtils.serverAttachCollidableTemplate(realm, ent1, CollidableTemplate.getBoxTemplate(new Vector3d(1,1,1)), new Vector3d(0));
+
+        int expectedBoxBoxCollisions = 4;
+
+        int expectedTotalCollisions = expectedBoxBoxCollisions * CollisionEngine.PHYSICS_SIMULATION_RESOLUTION;
+        
+
+        //simulate the physics
+        collisionEngine.simulatePhysics();
+
+        assertEquals(expectedTotalCollisions, collisionEngine.getFinalCollisionCount());
+    }
+
+}

src/test/java/electrosphere/test/testutils/TestEngineUtils.java

@@ -4,6 +4,7 @@ import static org.junit.jupiter.api.Assertions.*;
 
 import java.util.concurrent.TimeUnit;
 
+import org.joml.Vector3d;
 import org.junit.jupiter.api.Assertions;
 
 import java.util.function.Supplier;
@@ -165,7 +166,7 @@ public class TestEngineUtils {
         ObjectTemplate creatureTemplate = ObjectTemplate.createDefault(EntityType.CREATURE, "human");
         ServerConnectionHandler serverConnection = Globals.serverState.server.getFirstConnection();
         serverConnection.setCreatureTemplate(creatureTemplate);
-        Character chara = Globals.serverState.characterService.createCharacter(creatureTemplate, serverConnection.getPlayerId());
+        Character chara = Globals.serverState.characterService.createCharacter(creatureTemplate, serverConnection.getPlayerId(), new Vector3d());
         serverConnection.setCharacterId(chara.getId());
         PlayerCharacterCreation.spawnPlayerCharacter(serverConnection);
     }