From aa0956477d639c755adae62ac2eb4178deda7a25 Mon Sep 17 00:00:00 2001
From: austin <austin@austinhoover.net>
Date: Wed, 4 Jun 2025 11:53:42 -0400
Subject: [PATCH] physics performance work

---
 .../electrosphere/client/ClientState.java     |   3 +-
 .../collision/CollisionBodyCreation.java      |   2 +-
 .../collision/CollisionEngine.java            | 211 +++++++++++++----
 .../collision/PhysicsCallback.java            | 221 ++++++++++++++++++
 .../collision/PhysicsEntityUtils.java         |  23 +-
 .../electrosphere/collision/PhysicsUtils.java |   9 +
 .../collision/RayCastCallback.java            |   4 +-
 .../entity/collidable/CollidableTemplate.java |  22 ++
 .../server/datacell/RealmManager.java         |  10 +-
 .../gridded/GriddedDataCellManager.java       |   6 +
 .../datacell/physics/PhysicsDataCell.java     |   6 +-
 .../CollisionEngineStaticSpaceTests.java      |  78 +++++++
 12 files changed, 526 insertions(+), 69 deletions(-)
 create mode 100644 src/main/java/electrosphere/collision/PhysicsCallback.java
 create mode 100644 src/test/java/electrosphere/collision/CollisionEngineStaticSpaceTests.java

diff --git a/src/main/java/electrosphere/client/ClientState.java b/src/main/java/electrosphere/client/ClientState.java
index 1d5b9d7f..a64f7462 100644
--- a/src/main/java/electrosphere/client/ClientState.java
+++ b/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("clientPhysics"), CollisionEngine.create("clientChem", new ClientChemistryCollisionCallback()), new CollisionEngine("clientInteraction"));
+        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());
     }
 
diff --git a/src/main/java/electrosphere/collision/CollisionBodyCreation.java b/src/main/java/electrosphere/collision/CollisionBodyCreation.java
index 09042e07..32172698 100644
--- a/src/main/java/electrosphere/collision/CollisionBodyCreation.java
+++ b/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);
     }
 
     /**
diff --git a/src/main/java/electrosphere/collision/CollisionEngine.java b/src/main/java/electrosphere/collision/CollisionEngine.java
index f382af42..770bc2e4 100644
--- a/src/main/java/electrosphere/collision/CollisionEngine.java
+++ b/src/main/java/electrosphere/collision/CollisionEngine.java
@@ -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;
@@ -114,12 +113,17 @@ public class CollisionEngine {
     /**
      * The world object
      */
-    private DWorld world;
+    protected DWorld world;
 
     /**
      * The main space in the world
      */
-    private DBhvSpace space;
+    protected DBhvSpace space;
+
+    /**
+     * Space for storing static geoms
+     */
+    protected DSpace staticGeomSpace;
 
     /**
      * Lock for thread-safeing all ODE calls
@@ -129,7 +133,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>
@@ -139,27 +143,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.
@@ -183,16 +187,21 @@ 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
      */
@@ -202,12 +211,12 @@ public class CollisionEngine {
     /**
      * 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
      */
-    private final String name;
+    protected final String name;
     
     /**
      * Constructor
@@ -222,9 +231,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
@@ -243,6 +249,18 @@ public class CollisionEngine {
         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;
+        rVal.staticGeomSpace = OdeHelper.createBHVSpace(Collidable.TYPE_STATIC_BIT);
+        rVal.staticGeomSpace.setCategoryBits(Collidable.TYPE_STATIC_BIT);
+        return rVal;
+    }
+
 
     /**
      * Resolves collisions in the engine
@@ -333,6 +351,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
@@ -340,7 +361,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");
@@ -378,8 +398,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;
@@ -400,11 +428,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){
@@ -444,59 +467,118 @@ 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);
 
+                    //this triggers if o2 is the static geom space and we did an inner collide to find the geom within that space to collide with
+                    if(o2 == this.staticGeomSpace){
+                        //get the opposite geom from the collision
+                        if(contact.geom.g1 == o1){
+                            o2 = contact.geom.g2;
+                        } else if(contact.geom.g2 == o1){
+                            o2 = contact.geom.g1;
+                        } else {
+                            String message = "Failed to get collidable from geom within static geom space!\n" +
+                            "Geoms:\n" +
+                            o1 + " \n" +
+                            o2 + " \n" +
+                            "Bodies:\n" +
+                            b1 + " \n" +
+                            b2 + " \n" +
+                            "Colliders:\n" +
+                            c1 + " \n" +
+                            c2 + " \n" +
+                            "Obj 1 pointers:\n" +
+                            this.bodyPointerMap.get(b1) + " \n" +
+                            this.geomPointerMap.get(o1) + " \n" +
+                            "Obj 2 pointers:\n" +
+                            this.bodyPointerMap.get(b2) + " \n" +
+                            this.geomPointerMap.get(o2) + " \n" +
+                            "Contact geoms:\n" +
+                            contact.geom.g1 + "\n" +
+                            contact.geom.g2 + "\n" +
+                            "";
+                            throw new Error(message);
+                        }
+                        c2 = geomPointerMap.get(o2);
+                        if(c2 == null){
+                            String message = "Failed to get collidable from geom within static geom space!\n" +
+                            "Geoms:\n" +
+                            o1 + " \n" +
+                            o2 + " \n" +
+                            "Bodies:\n" +
+                            b1 + " \n" +
+                            b2 + " \n" +
+                            "Colliders:\n" +
+                            c1 + " \n" +
+                            c2 + " \n" +
+                            "Obj 1 pointers:\n" +
+                            this.bodyPointerMap.get(b1) + " \n" +
+                            this.geomPointerMap.get(o1) + " \n" +
+                            "Obj 2 pointers:\n" +
+                            this.bodyPointerMap.get(b2) + " \n" +
+                            this.geomPointerMap.get(o2) + " \n" +
+                            "";
+                            throw new Error(message);
+                        }
+                    }
+
                     //special code for ray casting
                     if (o1 instanceof DRay || o2 instanceof DRay){
                         DVector3 end = new DVector3();
@@ -565,8 +647,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.
@@ -1587,18 +1673,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
@@ -1608,6 +1682,19 @@ public class CollisionEngine {
         geom.setBody(body);
     }
 
+    /**
+     * Adds the geometry to the static geom space
+     * @param staticGeom The geom (ie a cylinder, cube, etc)
+     */
+    protected void addToStaticSpace(DGeom staticGeom){
+        // spaceLock.lock();
+        // if(staticGeom.getSpace() != null){
+        //     staticGeom.getSpace().remove(staticGeom);
+        // }
+        // this.staticGeomSpace.add(staticGeom);
+        // spaceLock.unlock();
+    }
+
     /**
      * Locks the ode library
      */
@@ -1636,6 +1723,8 @@ 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;
@@ -1664,6 +1753,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;
+    }
     
 
     /**
diff --git a/src/main/java/electrosphere/collision/PhysicsCallback.java b/src/main/java/electrosphere/collision/PhysicsCallback.java
new file mode 100644
index 00000000..2a359e40
--- /dev/null
+++ b/src/main/java/electrosphere/collision/PhysicsCallback.java
@@ -0,0 +1,221 @@
+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;
+        }
+
+        if(o1 == engine.staticGeomSpace){
+            OdeHelper.spaceCollide2(o2, engine.staticGeomSpace, 0, this);
+            return;
+        }
+        if(o2 == engine.staticGeomSpace){
+            OdeHelper.spaceCollide2(o1, engine.staticGeomSpace, 0, this);
+            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();
+    }
+    
+}
diff --git a/src/main/java/electrosphere/collision/PhysicsEntityUtils.java b/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
index 71e983de..fb6987c6 100644
--- a/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
+++ b/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
@@ -399,7 +399,8 @@ public class PhysicsEntityUtils {
         }
         CollisionEngine.lockOde();
         if(physicsTemplate.getKinematic()){
-            PhysicsEntityUtils.serverAttachGeom(realm,rVal,physicsTemplate);
+            DGeom geom = PhysicsEntityUtils.serverAttachGeom(realm,rVal,physicsTemplate);
+            PhysicsUtils.makeGeomStatic(realm.getCollisionEngine(), geom);
         } else {
             DBody rigidBody = null;
             switch(physicsTemplate.getType()){
@@ -625,10 +626,7 @@ public class PhysicsEntityUtils {
         }
         Collidable collidable;
         double mass = 1.0f;
-        long categoryBit = Collidable.TYPE_CREATURE_BIT;
-        if(physicsTemplate.getKinematic()){
-            categoryBit = Collidable.TYPE_STATIC_BIT;
-        }
+        long categoryBit = Collidable.TYPE_STATIC_BIT;
         CollisionEngine.lockOde();
         DGeom geom = null;
         switch(physicsTemplate.getType()){
@@ -726,6 +724,9 @@ public class PhysicsEntityUtils {
                 realm.getCollisionEngine().registerCollisionObject(geom, collidable);
             } break;
         }
+        if(geom != null){
+            realm.getCollisionEngine().addToStaticSpace(geom);
+        }
         CollisionEngine.unlockOde();
         return geom;
     }
@@ -825,6 +826,7 @@ public class PhysicsEntityUtils {
         PhysicsEntityUtils.setCollidable(terrain, collidable);
         realm.getCollisionEngine().registerCollisionObject(terrainCollider, collidable);
         PhysicsEntityUtils.setDGeom(terrain,terrainCollider);
+        PhysicsUtils.makeGeomStatic(realm.getCollisionEngine(), terrainCollider);
         
         return terrainCollider;
     }
@@ -860,6 +862,7 @@ public class PhysicsEntityUtils {
         PhysicsEntityUtils.setCollidable(terrain, collidable);
         realm.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
         PhysicsEntityUtils.setDGeom(terrain,terrainBody);
+        PhysicsUtils.makeGeomStatic(realm.getCollisionEngine(), terrainBody);
         
         return terrainBody;
     }
@@ -905,15 +908,7 @@ public class PhysicsEntityUtils {
      */
     public static void serverDestroyPhysics(Entity entity){
         Realm realm = Globals.serverState.realmManager.getEntityRealm(entity);
-        if(PhysicsEntityUtils.containsDBody(entity)){
-            PhysicsUtils.destroyPhysicsPair(
-                realm.getCollisionEngine(),
-                PhysicsEntityUtils.getDBody(entity),
-                PhysicsEntityUtils.getCollidable(entity)
-            );
-        }
-        PhysicsEntityUtils.setCollidable(entity, null);
-        PhysicsEntityUtils.clearGeomAndBody(entity);
+        realm.getCollisionEngine().destroyPhysics(entity);
         Vector3d entityPos = EntityUtils.getPosition(entity);
         ServerEntityUtils.repositionEntity(entity, entityPos);
     }
diff --git a/src/main/java/electrosphere/collision/PhysicsUtils.java b/src/main/java/electrosphere/collision/PhysicsUtils.java
index 34316175..172c3f91 100644
--- a/src/main/java/electrosphere/collision/PhysicsUtils.java
+++ b/src/main/java/electrosphere/collision/PhysicsUtils.java
@@ -136,6 +136,15 @@ public class PhysicsUtils {
         collisionEngine.synchronizeData(body, position, rotation, linearVel, angularVel, linearForce, angularForce);
     }
 
+    /**
+     * Adds a geom to the static geom space
+     * @param collisionEngine The collision engine
+     * @param geom The geom
+     */
+    public static void makeGeomStatic(CollisionEngine collisionEngine, DGeom geom){
+        collisionEngine.addToStaticSpace(geom);
+    }
+
 
     /**
      * Sets the position + rotation + scale of a body
diff --git a/src/main/java/electrosphere/collision/RayCastCallback.java b/src/main/java/electrosphere/collision/RayCastCallback.java
index f20b7d53..2ac550f4 100644
--- a/src/main/java/electrosphere/collision/RayCastCallback.java
+++ b/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())) ||
-            (o2 instanceof DRay && rayCastData.collidableTypeMask.contains(collidable1.getType()))
+            (o1 instanceof DRay && collidable2 != null && rayCastData.collidableTypeMask.contains(collidable2.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
diff --git a/src/main/java/electrosphere/data/entity/collidable/CollidableTemplate.java b/src/main/java/electrosphere/data/entity/collidable/CollidableTemplate.java
index f12ed1f4..2fe4a21a 100644
--- a/src/main/java/electrosphere/data/entity/collidable/CollidableTemplate.java
+++ b/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
diff --git a/src/main/java/electrosphere/server/datacell/RealmManager.java b/src/main/java/electrosphere/server/datacell/RealmManager.java
index 1f42bb25..0ff4be9b 100644
--- a/src/main/java/electrosphere/server/datacell/RealmManager.java
+++ b/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;
@@ -61,14 +62,15 @@ public class RealmManager {
         //create chemistry engine
         CollisionEngine chemistryEngine = new CollisionEngine("serverChem");
         chemistryEngine.setCollisionResolutionCallback(new ServerChemistryCollisionCallback());
-        return new Realm(
+        Realm rVal = new Realm(
             new ServerWorldData(),
-            new CollisionEngine("serverPhysics"),
+            CollisionEngine.create("serverPhysics", new PhysicsCallback()),
             chemistryEngine,
             new HitboxManager(new ServerHitboxResolutionCallback()),
             ServerContentManager.createServerContentManager(false, null),
             null
         );
+        return rVal;
     }
 
     /**
@@ -77,7 +79,7 @@ public class RealmManager {
      */
     public Realm createGriddedRealm(ServerWorldData serverWorldData, ServerContentManager serverContentManager){
         //create collision engine
-        CollisionEngine collisionEngine = new CollisionEngine("serverPhysics");
+        CollisionEngine collisionEngine = CollisionEngine.create("serverPhysics", new PhysicsCallback());
         collisionEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
         //create chemistry engine
         CollisionEngine chemistryEngine = new CollisionEngine("serverChem");
@@ -111,7 +113,7 @@ public class RealmManager {
         ServerWorldData serverWorldData = ServerWorldData.createFixedWorldData(minPoint, maxPoint);
 
         //create collision engine
-        CollisionEngine collisionEngine = new CollisionEngine("serverPhysics");
+        CollisionEngine collisionEngine = CollisionEngine.create("serverPhysics", new PhysicsCallback());
         collisionEngine.setCollisionWorldData(new CollisionWorldData(serverWorldData));
 
         //create chemistry engine
diff --git a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java
index aaa7b946..e404ce25 100644
--- a/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java
+++ b/src/main/java/electrosphere/server/datacell/gridded/GriddedDataCellManager.java
@@ -576,6 +576,12 @@ public class GriddedDataCellManager implements DataCellManager, VoxelCellManager
                 });
                 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);
diff --git a/src/main/java/electrosphere/server/datacell/physics/PhysicsDataCell.java b/src/main/java/electrosphere/server/datacell/physics/PhysicsDataCell.java
index 0b5ff53f..5fe3c8c6 100644
--- a/src/main/java/electrosphere/server/datacell/physics/PhysicsDataCell.java
+++ b/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);
-        realm.getCollisionEngine().destroyPhysics(blockPhysicsEntity);
+        if(realm != null){
+            realm.getCollisionEngine().destroyPhysics(physicsEntity);
+            realm.getCollisionEngine().destroyPhysics(blockPhysicsEntity);
+        }
     }
 
     /**
diff --git a/src/test/java/electrosphere/collision/CollisionEngineStaticSpaceTests.java b/src/test/java/electrosphere/collision/CollisionEngineStaticSpaceTests.java
new file mode 100644
index 00000000..e1da6da1
--- /dev/null
+++ b/src/test/java/electrosphere/collision/CollisionEngineStaticSpaceTests.java
@@ -0,0 +1,78 @@
+package electrosphere.collision;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.joml.Vector3d;
+import org.ode4j.ode.DBox;
+
+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();
+
+        DBox box = collisionEngine.createCubeGeom(new Vector3d(1,1,1), 0);
+        collisionEngine.addToStaticSpace(box);
+
+        //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)));
+
+        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());
+    }
+
+}