Testing physics

src/dusk/display/mesh/cube.c

@@ -12,8 +12,8 @@ mesh_t CUBE_MESH_SIMPLE;
 meshvertex_t CUBE_MESH_SIMPLE_VERTICES[CUBE_VERTEX_COUNT];
 
 errorret_t cubeInit() {
-  vec3 min = { 0.0f, 0.0f, 0.0f };
+  vec3 min = { -0.5f, -0.5f, -0.5f };
-  vec3 max = { 1.0f, 1.0f, 1.0f };
+  vec3 max = {  0.5f,  0.5f,  0.5f };
   cubeBuffer(CUBE_MESH_SIMPLE_VERTICES, min, max);
   errorChain(meshInit(
     &CUBE_MESH_SIMPLE,

src/dusk/display/mesh/cube.h

@@ -17,7 +17,9 @@ extern mesh_t CUBE_MESH_SIMPLE;
 extern meshvertex_t CUBE_MESH_SIMPLE_VERTICES[CUBE_VERTEX_COUNT];
 
 /**
- * Initializes the simple unit cube mesh (0,0,0) to (1,1,1).
+ * Initializes the simple unit cube mesh, centered at (0,0,0), spanning
+ * (-0.5,-0.5,-0.5) to (0.5,0.5,0.5) -- matching the centered convention
+ * physics shapes and the sphere mesh use (position = center).
  *
  * @return Error for initialization of the cube mesh.
  */

src/dusk/engine/engine.c

@@ -14,6 +14,9 @@
 #include "scene/scene.h"
 #include "entity/entitymanager.h"
 #include "entity/component/display/entityposition.h"
+#include "entity/component/display/entityrenderable.h"
+#include "entity/component/physics/entityphysics.h"
+#include "display/color.h"
 #include "asset/asset.h"
 #include "ui/ui.h"
 #include "assert/assert.h"
@@ -48,7 +51,8 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) {
   errorChain(networkInit());
   errorChain(sceneInit());
 
-  // Test: a spinning cube, viewed by a static camera.
+  // Test: three colored cubes falling onto each other and a static floor,
+  // viewed by a static camera.
   ENGINE.testSceneId = sceneCreate();
   sceneSetActive(ENGINE.testSceneId);
   entitymanager_t *testEntities = sceneGetEntities(ENGINE.testSceneId);
@@ -60,22 +64,64 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) {
   entityAddComponent(testEntities, testCamera, COMPONENT_TYPE_CAMERA);
   entityPositionLookAt(
     testEntities, testCamera, testCameraPosition,
-    (vec3){ 3.0f, 3.0f, 3.0f },
+    (vec3){ 4.0f, 3.0f, 4.0f },
     (vec3){ 0.0f, 0.0f, 0.0f },
     (vec3){ 0.0f, 1.0f, 0.0f }
   );
 
-  ENGINE.testCubeEntity = entityManagerAdd(testEntities);
+  // Static floor: a wide, thin box. Its render scale matches its physics
-  ENGINE.testCubePositionComponent = entityAddComponent(
+  // half-extents exactly, since CUBE_MESH_SIMPLE and PHYSICS_SHAPE_CUBE are
-    testEntities, ENGINE.testCubeEntity, COMPONENT_TYPE_POSITION
+  // both centered on the entity's position.
+  entityid_t floorEntity = entityManagerAdd(testEntities);
+  componentid_t floorPosition = entityAddComponent(
+    testEntities, floorEntity, COMPONENT_TYPE_POSITION
   );
-  entityAddComponent(
+  entityPositionSetLocalPosition(
-    testEntities, ENGINE.testCubeEntity, COMPONENT_TYPE_RENDERABLE
+    testEntities, floorEntity, floorPosition, (vec3){ 0.0f, -1.0f, 0.0f }
   );
-  entityUpdateAdd(
+  entityPositionSetLocalScale(
-    testEntities, ENGINE.testCubeEntity, engineTestCubeRotate,
+    testEntities, floorEntity, floorPosition, (vec3){ 10.0f, 1.0f, 10.0f }
-    ENGINE.testCubePositionComponent, NULL
   );
+  componentid_t floorPhysics = entityAddComponent(
+    testEntities, floorEntity, COMPONENT_TYPE_PHYSICS
+  );
+  entityPhysicsSetBodyType(
+    testEntities, floorEntity, floorPhysics, PHYSICS_BODY_STATIC
+  );
+  entityPhysicsSetShape(testEntities, floorEntity, floorPhysics, (physicsshape_t){
+    .type = PHYSICS_SHAPE_CUBE,
+    .data.cube.halfExtents = { 5.0f, 0.5f, 5.0f }
+  });
+  componentid_t floorRenderable = entityAddComponent(
+    testEntities, floorEntity, COMPONENT_TYPE_RENDERABLE
+  );
+  entityRenderableSetColor(testEntities, floorEntity, floorRenderable, COLOR_GRAY);
+
+  // Three dynamic cubes, staggered above the floor with slight offsets so
+  // they tumble and land on each other instead of falling in perfect sync.
+  vec3 cubeStartPositions[3] = {
+    { -0.3f, 1.5f,  0.1f },
+    {  0.1f, 3.0f, -0.2f },
+    { -0.1f, 4.5f,  0.2f },
+  };
+  color_t cubeColors[3] = { COLOR_RED, COLOR_GREEN, COLOR_BLUE };
+
+  for(uint8_t i = 0; i < 3; i++) {
+    entityid_t cubeEntity = entityManagerAdd(testEntities);
+    componentid_t cubePosition = entityAddComponent(
+      testEntities, cubeEntity, COMPONENT_TYPE_POSITION
+    );
+    entityPositionSetLocalPosition(
+      testEntities, cubeEntity, cubePosition, cubeStartPositions[i]
+    );
+    entityAddComponent(testEntities, cubeEntity, COMPONENT_TYPE_PHYSICS);
+    componentid_t cubeRenderable = entityAddComponent(
+      testEntities, cubeEntity, COMPONENT_TYPE_RENDERABLE
+    );
+    entityRenderableSetColor(
+      testEntities, cubeEntity, cubeRenderable, cubeColors[i]
+    );
+  }
 
   networkRequestConnection(
     engineNetworkOnConnected,
@@ -207,16 +253,3 @@ void engineNetworkOnDisconnect(errorret_t error, void *user) {
 void engineNetworkDisconnectTestOnComplete(void *user) {
   consolePrint("Network disconnect test complete");
 }
-
-void engineTestCubeRotate(
-  entitymanager_t *mgr,
-  const entityid_t entityId,
-  const componentid_t componentId,
-  void *user
-) {
-  ENGINE.testCubeRotation += TIME.delta;
-  entityPositionSetLocalRotation(
-    mgr, entityId, componentId,
-    (vec3){ 0.0f, ENGINE.testCubeRotation, 0.0f }
-  );
-}

src/dusk/engine/engine.h

@@ -23,11 +23,9 @@ typedef struct {
   bool_t networkDisconnectTestPending;
   float_t networkDisconnectTestAt;
 
-  // Test: a spinning cube, viewed by a static camera.
+  // Test: three colored cubes falling onto each other and a static floor,
+  // viewed by a static camera.
   sceneid_t testSceneId;
-  entityid_t testCubeEntity;
-  componentid_t testCubePositionComponent;
-  float_t testCubeRotation;
 } engine_t;
 
 extern engine_t ENGINE;
@@ -98,21 +96,3 @@ void engineNetworkOnDisconnect(errorret_t error, void *user);
  * @param user Unused.
  */
 void engineNetworkDisconnectTestOnComplete(void *user);
-
-/**
- * Entity update callback that spins the test cube around Y. Registered
- * once in engineInit() via entityUpdateAdd(), so it only runs from
- * sceneFixedUpdate() -- once per fixed timestep, never once per rendered
- * frame.
- *
- * @param mgr The entity manager that owns the entity.
- * @param entityId The test cube's entity ID.
- * @param componentId The test cube's position component ID.
- * @param user Unused.
- */
-void engineTestCubeRotate(
-  entitymanager_t *mgr,
-  const entityid_t entityId,
-  const componentid_t componentId,
-  void *user
-);

src/dusk/entity/component/display/entityrenderable.c

@@ -64,6 +64,22 @@ void entityRenderableSetPriority(
   r->priority = priority;
 }
 
+void entityRenderableSetColor(
+  entitymanager_t *mgr,
+  const entityid_t entityId,
+  const componentid_t componentId,
+  const color_t color
+) {
+  entityrenderable_t *r = componentGetData(
+    mgr, entityId, componentId, COMPONENT_TYPE_RENDERABLE
+  );
+  assertTrue(
+    r->type == ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL,
+    "Renderable must be ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL to set color"
+  );
+  r->data.material.material.unlit.color = color;
+}
+
 void entityRenderableSetDraw(
   entitymanager_t *mgr,
   const entityid_t entityId,

src/dusk/entity/component/display/entityrenderable.h

@@ -124,6 +124,23 @@ void entityRenderableSetPriority(
   const int8_t priority
 );
 
+/**
+ * Sets the unlit material color. Only meaningful when the renderable is
+ * (or defaults to) ENTITY_RENDERABLE_TYPE_SHADER_MATERIAL -- asserts
+ * otherwise.
+ *
+ * @param mgr The entity manager that owns the entity.
+ * @param entityId The entity to configure.
+ * @param componentId The renderable component.
+ * @param color The color to tint the material with.
+ */
+void entityRenderableSetColor(
+  entitymanager_t *mgr,
+  const entityid_t entityId,
+  const componentid_t componentId,
+  const color_t color
+);
+
 /**
  * Sets the draw callback, switching the type to
  * ENTITY_RENDERABLE_TYPE_CUSTOM.

src/dusk/physics/physicsshapemesh.c

@@ -21,6 +21,39 @@ void physicsShapeMeshGetVertex(
   out[2] = p[2];
 }
 
+void physicsShapeMeshComputeBounds(physicsshapemesh_t *mesh) {
+  assertNotNull(mesh, "Mesh cannot be null");
+
+  const uint32_t vertexCount = mesh->triangleCount * 3;
+  if(vertexCount == 0) {
+    glm_vec3_zero(mesh->boundsCenter);
+    mesh->boundsRadius = 0.0f;
+    return;
+  }
+
+  vec3 minPoint, maxPoint;
+  physicsShapeMeshGetVertex(mesh, 0, minPoint);
+  glm_vec3_copy(minPoint, maxPoint);
+
+  for(uint32_t i = 1; i < vertexCount; i++) {
+    vec3 v;
+    physicsShapeMeshGetVertex(mesh, i, v);
+    if(v[0] < minPoint[0]) minPoint[0] = v[0];
+    if(v[1] < minPoint[1]) minPoint[1] = v[1];
+    if(v[2] < minPoint[2]) minPoint[2] = v[2];
+    if(v[0] > maxPoint[0]) maxPoint[0] = v[0];
+    if(v[1] > maxPoint[1]) maxPoint[1] = v[1];
+    if(v[2] > maxPoint[2]) maxPoint[2] = v[2];
+  }
+
+  glm_vec3_add(minPoint, maxPoint, mesh->boundsCenter);
+  glm_vec3_scale(mesh->boundsCenter, 0.5f, mesh->boundsCenter);
+
+  vec3 extent;
+  glm_vec3_sub(maxPoint, mesh->boundsCenter, extent);
+  mesh->boundsRadius = glm_vec3_norm(extent);
+}
+
 bool_t physicsShapeMeshTestSphere(
   const physicsshapemesh_t *mesh, const vec3 meshPos,
   const vec3 sphereCenter, const float_t sphereRadius,
@@ -28,6 +61,15 @@ bool_t physicsShapeMeshTestSphere(
 ) {
   assertNotNull(mesh, "Mesh cannot be null");
 
+  // Broad-phase: reject in O(1) against the cached bounding sphere before
+  // scanning every triangle.
+  vec3 worldBoundsCenter;
+  glm_vec3_add((float_t *)mesh->boundsCenter, (float_t *)meshPos, worldBoundsCenter);
+  float_t rejectRadius = mesh->boundsRadius + sphereRadius;
+  vec3 toCenter;
+  glm_vec3_sub((float_t *)sphereCenter, worldBoundsCenter, toCenter);
+  if(glm_vec3_norm2(toCenter) >= rejectRadius * rejectRadius) return false;
+
   bool_t found = false;
   float_t bestDist2 = 0.0f;
   vec3 bestPoint;
@@ -77,6 +119,19 @@ bool_t physicsShapeMeshTestCapsule(
   const float_t capsuleHalfHeight,
   vec3 outNormal, float_t *outDepth
 ) {
+  assertNotNull(mesh, "Mesh cannot be null");
+
+  // Broad-phase: reject the whole capsule (skipping every sample sphere
+  // below) against the cached bounding sphere before doing any real work.
+  // The capsule's own bounding sphere (radius + half-height) is a looser
+  // bound than its true shape, but cheap and always conservative.
+  vec3 worldBoundsCenter;
+  glm_vec3_add((float_t *)mesh->boundsCenter, (float_t *)meshPos, worldBoundsCenter);
+  float_t rejectRadius = mesh->boundsRadius + capsuleRadius + capsuleHalfHeight;
+  vec3 toCenter;
+  glm_vec3_sub((float_t *)capsuleCenter, worldBoundsCenter, toCenter);
+  if(glm_vec3_norm2(toCenter) >= rejectRadius * rejectRadius) return false;
+
   vec3 capA = {
     capsuleCenter[0], capsuleCenter[1] - capsuleHalfHeight, capsuleCenter[2]
   };
@@ -142,8 +197,9 @@ bool_t physicsShapeMeshTest(
   }
 }
 
-physicsshape_t physicsShapeMeshCreate(const physicsshapemesh_t *mesh) {
+physicsshape_t physicsShapeMeshCreate(physicsshapemesh_t *mesh) {
   assertNotNull(mesh, "Mesh cannot be null");
+  physicsShapeMeshComputeBounds(mesh);
   return (physicsshape_t){
     .type = PHYSICS_SHAPE_CUSTOM,
     .data.custom = { .userData = (void *)mesh, .test = physicsShapeMeshTest }

src/dusk/physics/physicsshapemesh.h

@@ -29,10 +29,13 @@
  * outlive any physicsshape_t built from it, so it's typically a field
  * alongside whatever owns the mesh (e.g. a terrain chunk).
  *
- * Every query scans all triangleCount triangles (no broad-phase/BVH), so
+ * A query first rejects against the cached local-space bounding sphere
- * for good performance keep each mesh shape scoped to one reasonably-sized
+ * (boundsCenter/boundsRadius, see physicsShapeMeshComputeBounds) in O(1);
- * chunk rather than an entire world, and only add nearby chunks' physics
+ * only once that overlaps does it fall through to scanning all
- * bodies to the scene.
+ * triangleCount triangles. There's still no finer broad-phase/BVH within
+ * a mesh, so for good performance keep each mesh shape scoped to one
+ * reasonably-sized chunk rather than an entire world, and only add nearby
+ * chunks' physics bodies to the scene.
  */
 typedef struct {
   /** Pointer to the first triangle vertex's position (3 consecutive float_t). */
@@ -41,6 +44,15 @@ typedef struct {
   size_t stride;
   /** Number of triangles. `vertices` holds triangleCount * 3 positions. */
   uint32_t triangleCount;
+
+  /**
+   * Cached local-space bounding sphere, set by
+   * physicsShapeMeshComputeBounds() (called once by physicsShapeMeshCreate).
+   * Used to reject a query in O(1) before scanning every triangle.
+   */
+  vec3 boundsCenter;
+  /** Cached local-space bounding sphere radius. See boundsCenter. */
+  float_t boundsRadius;
 } physicsshapemesh_t;
 
 /**
@@ -56,6 +68,18 @@ void physicsShapeMeshGetVertex(
   const physicsshapemesh_t *mesh, const uint32_t index, vec3 out
 );
 
+/**
+ * Computes and caches mesh's local-space bounding sphere (boundsCenter/
+ * boundsRadius) from its current vertices, by scanning every vertex once.
+ * Called automatically by physicsShapeMeshCreate(); call again yourself
+ * only if you mutate the underlying vertex buffer afterward (e.g. terrain
+ * deformation) and need the cached bounds to stay accurate.
+ *
+ * @param mesh The mesh to compute bounds for; boundsCenter/boundsRadius
+ * are written in place.
+ */
+void physicsShapeMeshComputeBounds(physicsshapemesh_t *mesh);
+
 /**
  * Tests a sphere against every triangle in the mesh, keeping only the
  * single closest triangle. outNormal points from the mesh surface toward
@@ -120,9 +144,12 @@ bool_t physicsShapeMeshTest(
 
 /**
  * Builds a PHYSICS_SHAPE_CUSTOM shape backed by the given triangle mesh.
+ * Calls physicsShapeMeshComputeBounds(mesh) once to populate its cached
+ * bounding sphere.
  *
- * @param mesh Mesh descriptor. Must outlive the returned shape (see
+ * @param mesh Mesh descriptor; boundsCenter/boundsRadius are computed and
+ * written in place. Must outlive the returned shape (see
  * physicsshapemesh_t).
  * @return A physicsshape_t wired to test against this mesh.
  */
-physicsshape_t physicsShapeMeshCreate(const physicsshapemesh_t *mesh);
+physicsshape_t physicsShapeMeshCreate(physicsshapemesh_t *mesh);

src/dusk/physics/physicstest.c

@@ -321,12 +321,12 @@ bool_t physicsTestCapsuleVsCapsule(
 }
 
 bool_t physicsTestDispatch(
-  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 aPos, const physicsshape_t *aShape,
-  const vec3 bPos, const physicsshape_t bShape,
+  const vec3 bPos, const physicsshape_t *bShape,
   vec3 outNormal, float_t *outDepth
 ) {
-  physicshapetype_t ta = aShape.type;
+  physicshapetype_t ta = aShape->type;
-  physicshapetype_t tb = bShape.type;
+  physicshapetype_t tb = bShape->type;
 
   assertFalse(
     ta == PHYSICS_SHAPE_CUSTOM && tb == PHYSICS_SHAPE_CUSTOM,
@@ -337,8 +337,8 @@ bool_t physicsTestDispatch(
     // Callback returns self(A)->other(B); this function's contract needs
     // B->A, so negate.
     vec3 tmp; float_t d;
-    if(!aShape.data.custom.test(
+    if(!aShape->data.custom.test(
-      aPos, aShape.data.custom.userData, bPos, &bShape, tmp, &d
+      aPos, aShape->data.custom.userData, bPos, bShape, tmp, &d
     )) return false;
     glm_vec3_scale(tmp, -1.0f, outNormal);
     *outDepth = d;
@@ -348,30 +348,30 @@ bool_t physicsTestDispatch(
   if(tb == PHYSICS_SHAPE_CUSTOM) {
     // Callback returns self(B)->other(A), which already matches this
     // function's B->A contract -- no negation needed.
-    return bShape.data.custom.test(
+    return bShape->data.custom.test(
-      bPos, bShape.data.custom.userData, aPos, &aShape, outNormal, outDepth
+      bPos, bShape->data.custom.userData, aPos, aShape, outNormal, outDepth
     );
   }
 
   if(tb == PHYSICS_SHAPE_PLANE) {
-    const float_t *pn = bShape.data.plane.normal;
+    const float_t *pn = bShape->data.plane.normal;
-    const float_t  pd = bShape.data.plane.distance;
+    const float_t  pd = bShape->data.plane.distance;
     switch(ta) {
       case PHYSICS_SHAPE_CUBE:
         return physicsTestAabbVsPlane(
-          aPos, aShape.data.cube.halfExtents,
+          aPos, aShape->data.cube.halfExtents,
           pn, pd, outNormal, outDepth
         );
       case PHYSICS_SHAPE_SPHERE:
         return physicsTestSphereVsPlane(
-          aPos, aShape.data.sphere.radius,
+          aPos, aShape->data.sphere.radius,
           pn, pd, outNormal, outDepth
         );
       case PHYSICS_SHAPE_CAPSULE:
         return physicsTestCapsuleVsPlane(
           aPos,
-          aShape.data.capsule.radius,
+          aShape->data.capsule.radius,
-          aShape.data.capsule.halfHeight,
+          aShape->data.capsule.halfHeight,
           pn, pd, outNormal, outDepth
         );
       default:
@@ -392,18 +392,18 @@ bool_t physicsTestDispatch(
   switch(ta) {
     case PHYSICS_SHAPE_CUBE: {
       const float_t *ac = aPos;
-      const float_t *ah = aShape.data.cube.halfExtents;
+      const float_t *ah = aShape->data.cube.halfExtents;
       switch(tb) {
         case PHYSICS_SHAPE_CUBE:
           return physicsTestAabbVsAabb(
             ac, ah,
-            bPos, bShape.data.cube.halfExtents,
+            bPos, bShape->data.cube.halfExtents,
             outNormal, outDepth
           );
         case PHYSICS_SHAPE_SPHERE: {
           vec3 tmp; float_t d;
           if(!physicsTestSphereVsAabb(
-            bPos, bShape.data.sphere.radius,
+            bPos, bShape->data.sphere.radius,
             ac, ah, tmp, &d
           )) return false;
           glm_vec3_scale(tmp, -1.0f, outNormal);
@@ -414,8 +414,8 @@ bool_t physicsTestDispatch(
           vec3 tmp; float_t d;
           if(!physicsTestCapsuleVsAabb(
             bPos,
-            bShape.data.capsule.radius,
+            bShape->data.capsule.radius,
-            bShape.data.capsule.halfHeight,
+            bShape->data.capsule.halfHeight,
             ac, ah, tmp, &d
           )) return false;
           glm_vec3_scale(tmp, -1.0f, outNormal);
@@ -427,26 +427,26 @@ bool_t physicsTestDispatch(
     }
 
     case PHYSICS_SHAPE_SPHERE: {
-      const float_t sr = aShape.data.sphere.radius;
+      const float_t sr = aShape->data.sphere.radius;
       switch(tb) {
         case PHYSICS_SHAPE_CUBE:
           return physicsTestSphereVsAabb(
             aPos, sr,
-            bPos, bShape.data.cube.halfExtents,
+            bPos, bShape->data.cube.halfExtents,
             outNormal, outDepth
           );
         case PHYSICS_SHAPE_SPHERE:
           return physicsTestSphereVsSphere(
             aPos, sr,
-            bPos, bShape.data.sphere.radius,
+            bPos, bShape->data.sphere.radius,
             outNormal, outDepth
           );
         case PHYSICS_SHAPE_CAPSULE: {
           vec3 tmp; float_t d;
           if(!physicsTestCapsuleVsSphere(
             bPos,
-            bShape.data.capsule.radius,
+            bShape->data.capsule.radius,
-            bShape.data.capsule.halfHeight,
+            bShape->data.capsule.halfHeight,
             aPos, sr, tmp, &d
           )) return false;
           glm_vec3_scale(tmp, -1.0f, outNormal);
@@ -458,27 +458,27 @@ bool_t physicsTestDispatch(
     }
 
     case PHYSICS_SHAPE_CAPSULE: {
-      const float_t cr  = aShape.data.capsule.radius;
+      const float_t cr  = aShape->data.capsule.radius;
-      const float_t chh = aShape.data.capsule.halfHeight;
+      const float_t chh = aShape->data.capsule.halfHeight;
       switch(tb) {
         case PHYSICS_SHAPE_CUBE:
           return physicsTestCapsuleVsAabb(
             aPos, cr, chh,
-            bPos, bShape.data.cube.halfExtents,
+            bPos, bShape->data.cube.halfExtents,
             outNormal, outDepth
           );
         case PHYSICS_SHAPE_SPHERE:
           return physicsTestCapsuleVsSphere(
             aPos, cr, chh,
-            bPos, bShape.data.sphere.radius,
+            bPos, bShape->data.sphere.radius,
             outNormal, outDepth
           );
         case PHYSICS_SHAPE_CAPSULE:
           return physicsTestCapsuleVsCapsule(
             aPos, cr, chh,
             bPos,
-            bShape.data.capsule.radius,
+            bShape->data.capsule.radius,
-            bShape.data.capsule.halfHeight,
+            bShape->data.capsule.halfHeight,
             outNormal, outDepth
           );
         default: return false;
@@ -490,8 +490,8 @@ bool_t physicsTestDispatch(
 }
 
 bool_t physicsTestShapeVsShape(
-  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 aPos, const physicsshape_t *aShape,
-  const vec3 bPos, const physicsshape_t bShape,
+  const vec3 bPos, const physicsshape_t *bShape,
   vec3 outNormal, float_t *outDepth
 ) {
   return physicsTestDispatch(

src/dusk/physics/physicstest.h

@@ -235,8 +235,8 @@ bool_t physicsTestCapsuleVsCapsule(
  * @return true if overlapping.
  */
 bool_t physicsTestDispatch(
-  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 aPos, const physicsshape_t *aShape,
-  const vec3 bPos, const physicsshape_t bShape,
+  const vec3 bPos, const physicsshape_t *bShape,
   vec3 outNormal, float_t *outDepth
 );
 
@@ -257,9 +257,9 @@ bool_t physicsTestDispatch(
  */
 bool_t physicsTestShapeVsShape(
   const vec3 aPos,
-  const physicsshape_t aShape,
+  const physicsshape_t *aShape,
   const vec3 bPos,
-  const physicsshape_t bShape,
+  const physicsshape_t *bShape,
   vec3 outNormal,
   float_t *outDepth
 );

src/dusk/physics/physicsworld.c

@@ -55,12 +55,28 @@ void physicsWorldStep(
     physBodies[i] = entityPhysicsGet(mgr, physEnts[i], physComps[i]);
   }
 
+  // Partition once into dynamic vs non-dynamic (static/kinematic) indices,
+  // so the phases below only ever iterate the subset they actually care
+  // about instead of re-scanning (and skipping past) every physics entity
+  // each time.
+  entityid_t dynamicIndices[ENTITY_COUNT_MAX];
+  entityid_t otherIndices[ENTITY_COUNT_MAX];
+  entityid_t dynamicCount = 0;
+  entityid_t otherCount = 0;
+  for(entityid_t i = 0; i < physCount; i++) {
+    if(physBodies[i]->type == PHYSICS_BODY_DYNAMIC) {
+      dynamicIndices[dynamicCount++] = i;
+    } else {
+      otherIndices[otherCount++] = i;
+    }
+  }
+
   // Phase 1: integrate dynamic bodies (gravity + velocity -> position).
   // Writes directly to pos->position, matrix rebuilt at the end.
-  for(entityid_t i = 0; i < physCount; i++) {
+  for(entityid_t di = 0; di < dynamicCount; di++) {
+    entityid_t i = dynamicIndices[di];
     if(!positions[i]) continue;
     entityphysics_t *phys = physBodies[i];
-    if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
 
     phys->onGround = false;
 
@@ -75,22 +91,21 @@ void physicsWorldStep(
   }
 
   // Phase 2: dynamic vs static/kinematic.
-  for(entityid_t i = 0; i < physCount; i++) {
+  for(entityid_t di = 0; di < dynamicCount; di++) {
+    entityid_t i = dynamicIndices[di];
     if(!positions[i]) continue;
     entityphysics_t *phys = physBodies[i];
-    if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
-
     float_t *pos = positions[i]->position;
 
-    for(entityid_t j = 0; j < physCount; j++) {
+    for(entityid_t oj = 0; oj < otherCount; oj++) {
-      if(i == j || !positions[j]) continue;
+      entityid_t j = otherIndices[oj];
+      if(!positions[j]) continue;
       entityphysics_t *otherPhys = physBodies[j];
-      if(otherPhys->type == PHYSICS_BODY_DYNAMIC) continue;
 
       vec3 normal; float_t depth;
       if(!physicsTestShapeVsShape(
-        pos, phys->shape,
+        pos, &phys->shape,
-        positions[j]->position, otherPhys->shape,
+        positions[j]->position, &otherPhys->shape,
         normal, &depth
       )) continue;
 
@@ -110,23 +125,21 @@ void physicsWorldStep(
   }
 
   // Phase 3: dynamic vs dynamic.
-  for(entityid_t i = 0; i < physCount; i++) {
+  for(entityid_t di = 0; di < dynamicCount; di++) {
+    entityid_t i = dynamicIndices[di];
     if(!positions[i]) continue;
     entityphysics_t *physA = physBodies[i];
-    if(physA->type != PHYSICS_BODY_DYNAMIC) continue;
-
     float_t *posA = positions[i]->position;
 
-    for(entityid_t j = i + 1; j < physCount; j++) {
+    for(entityid_t dj = di + 1; dj < dynamicCount; dj++) {
+      entityid_t j = dynamicIndices[dj];
       if(!positions[j]) continue;
       entityphysics_t *physB = physBodies[j];
-      if(physB->type != PHYSICS_BODY_DYNAMIC) continue;
-
       float_t *posB = positions[j]->position;
 
       vec3 normal; float_t depth;
       if(!physicsTestShapeVsShape(
-        posA, physA->shape, posB, physB->shape, normal, &depth
+        posA, &physA->shape, posB, &physB->shape, normal, &depth
       )) continue;
 
       posA[0] += normal[0] * depth * 0.5f;
@@ -154,9 +167,9 @@ void physicsWorldStep(
   }
 
   // Rebuild transforms for all dynamic bodies once, after all phases.
-  for(entityid_t i = 0; i < physCount; i++) {
+  for(entityid_t di = 0; di < dynamicCount; di++) {
+    entityid_t i = dynamicIndices[di];
     if(!positions[i]) continue;
-    if(physBodies[i]->type != PHYSICS_BODY_DYNAMIC) continue;
     entityPositionRebuild(mgr, positions[i]);
   }
 }

src/dusk/system/system.c

@@ -16,6 +16,10 @@
   #error "systemInitPlatform is not defined"
 #endif
 
+#ifndef systemGetCyclesPlatform
+  #error "systemGetCyclesPlatform is not defined"
+#endif
+
 errorret_t systemInit() {
   return systemInitPlatform();
 }
@@ -36,4 +40,8 @@ systemplatform_t systemGetPlatform(void) {
   #else
     return SYSTEM_PLATFORM_LINUX;
   #endif
+}
+
+uint64_t systemGetCycles(void) {
+  return systemGetCyclesPlatform();
 }

src/dusk/system/system.h

@@ -47,4 +47,15 @@ systemdialogtype_t systemGetActiveDialogType();
  *
  * @return The current platform.
  */
-systemplatform_t systemGetPlatform(void);
+systemplatform_t systemGetPlatform(void);
+
+/**
+ * Returns the current CPU cycle counter value. This is intended for
+ * profiling only -- take the difference between two calls to measure
+ * elapsed cycles for a section of code. The counter's rate, width, and
+ * wrap-around behavior differ per platform, so the raw value itself is
+ * meaningless outside of such a comparison.
+ *
+ * @return The current CPU cycle count.
+ */
+uint64_t systemGetCycles(void);

src/duskdolphin/system/systemdolphin.c

@@ -39,4 +39,8 @@ int32_t systemGetAspectRatioDolphin(void) {
 
 int32_t systemGetLanguageDolphin(void) {
   return CONF_GetLanguage();
+}
+
+uint64_t systemGetCyclesDolphin(void) {
+  return gettime();
 }

src/duskdolphin/system/systemdolphin.h

@@ -48,4 +48,12 @@ int32_t systemGetAspectRatioDolphin(void);
 int32_t systemGetLanguageDolphin(void);
 
 // There's actually a tonne more things Wii can return, this is it for now
-// though.
+// though.
+
+/**
+ * Returns the current CPU cycle counter value, read directly from the
+ * PowerPC time base register.
+ *
+ * @return The current CPU cycle count.
+ */
+uint64_t systemGetCyclesDolphin(void);

src/duskdolphin/system/systemplatform.h

@@ -9,4 +9,5 @@
 #include "system/systemdolphin.h"
 
 #define systemInitPlatform systemInitDolphin
-#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeDolphin
+#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeDolphin
+#define systemGetCyclesPlatform systemGetCyclesDolphin

src/dusklinux/system/systemlinux.c

@@ -7,10 +7,30 @@
 
 #include "systemlinux.h"
 
+#if defined(__x86_64__) || defined(__i386__)
+  #include <x86intrin.h>
+#else
+  #include <time.h>
+#endif
+
 errorret_t systemInitLinux() {
   errorOk();
 }
 
 systemdialogtype_t systemGetActiveDialogTypeLinux() {
   return SYSTEM_DIALOG_TYPE_NONE;
+}
+
+uint64_t systemGetCyclesLinux(void) {
+  #if defined(__x86_64__) || defined(__i386__)
+    return __rdtsc();
+  #elif defined(__aarch64__)
+    uint64_t cycles;
+    __asm__ volatile("mrs %0, cntvct_el0" : "=r" (cycles));
+    return cycles;
+  #else
+    struct timespec ts;
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return (uint64_t)ts.tv_sec * 1000000000ULL + (uint64_t)ts.tv_nsec;
+  #endif
 }

src/dusklinux/system/systemlinux.h

@@ -15,7 +15,16 @@ errorret_t systemInitLinux(void);
 
 /**
  * Currently just returns SYSTEM_DIALOG_TYPE_NONE.
- * 
+ *
  * @return Currently open system dialog type.
  */
-systemdialogtype_t systemGetActiveDialogTypeLinux();
+systemdialogtype_t systemGetActiveDialogTypeLinux();
+
+/**
+ * Returns the current CPU cycle counter value. Uses the hardware
+ * timestamp counter on x86/x86_64 and the virtual counter register on
+ * ARM64, falling back to a monotonic nanosecond clock elsewhere.
+ *
+ * @return The current CPU cycle count.
+ */
+uint64_t systemGetCyclesLinux(void);

src/dusklinux/system/systemplatform.h

@@ -9,4 +9,5 @@
 #include "system/systemlinux.h"
 
 #define systemInitPlatform systemInitLinux
-#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeLinux
+#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypeLinux
+#define systemGetCyclesPlatform systemGetCyclesLinux

src/duskpsp/system/systemplatform.h

@@ -9,4 +9,5 @@
 #include "system/systempsp.h"
 
 #define systemInitPlatform systemInitPSP
-#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypePSP
+#define systemGetActiveDialogTypePlatform systemGetActiveDialogTypePSP
+#define systemGetCyclesPlatform systemGetCyclesPSP

src/duskpsp/system/systempsp.c

@@ -61,4 +61,10 @@ int_t systemPSPGetCrossButtonSetting() {
   return (
     ret == 1 ? PSP_UTILITY_ACCEPT_CROSS : PSP_UTILITY_ACCEPT_CIRCLE
   );
+}
+
+uint64_t systemGetCyclesPSP(void) {
+  uint32_t count;
+  asm volatile("mfc0 %0, $9" : "=r" (count));
+  return (uint64_t)count;
 }

src/duskpsp/system/systempsp.h

@@ -37,4 +37,12 @@ int_t systemPSPGetLanguage();
  * 
  * @return PSP_UTILITY_ACCEPT_CROSS or PSP_UTILITY_ACCEPT_CIRCLE.
  */
-int_t systemPSPGetCrossButtonSetting();
+int_t systemPSPGetCrossButtonSetting();
+
+/**
+ * Returns the current CPU cycle counter value, read directly from the
+ * Allegrex COP0 Count register.
+ *
+ * @return The current CPU cycle count.
+ */
+uint64_t systemGetCyclesPSP(void);

test/physics/test_physicsshapemesh.c

@@ -163,7 +163,7 @@ static void test_physicsShapeMeshCreateIntegratesWithDispatch(void **state) {
   // Landscape as B, matching physicsWorldStep's dynamic(A)-vs-static(B).
   vec3 normal; float_t depth;
   assert_true(physicsTestShapeVsShape(
-    sphereCenter, sphere, meshPos, landscape, normal, &depth
+    sphereCenter, &sphere, meshPos, &landscape, normal, &depth
   ));
   assert_float_equal(depth, 0.2f, 0.0001f);
   assert_float_equal(normal[1], 1.0f, 0.0001f);

test/physics/test_physicstest.c

@@ -91,12 +91,12 @@ static void test_dispatchSymmetryAndPlaneRouting(void **state) {
 
   vec3 normalAB; float_t depthAB;
   assert_true(physicsTestShapeVsShape(
-    aPos, cubeA, bPos, sphereB, normalAB, &depthAB
+    aPos, &cubeA, bPos, &sphereB, normalAB, &depthAB
   ));
 
   vec3 normalBA; float_t depthBA;
   assert_true(physicsTestShapeVsShape(
-    bPos, sphereB, aPos, cubeA, normalBA, &depthBA
+    bPos, &sphereB, aPos, &cubeA, normalBA, &depthBA
   ));
 
   // Swapping A/B should give the same depth and a negated normal.
@@ -111,7 +111,7 @@ static void test_dispatchSymmetryAndPlaneRouting(void **state) {
   vec3 cubePos = { 0.0f, 0.4f, 0.0f };
   vec3 normal; float_t depth;
   assert_true(physicsTestShapeVsShape(
-    cubePos, cubeA, bPos, plane, normal, &depth
+    cubePos, &cubeA, bPos, &plane, normal, &depth
   ));
   assert_float_equal(depth, 0.1f, 0.0001f);
 
@@ -173,7 +173,7 @@ static void test_customShapeFlatLandscape(void **state) {
   // body as A, static/custom body as B).
   vec3 normal; float_t depth;
   assert_true(physicsTestShapeVsShape(
-    spherePos, sphere, landscapePos, landscape, normal, &depth
+    spherePos, &sphere, landscapePos, &landscape, normal, &depth
   ));
   assert_float_equal(depth, 0.2f, 0.0001f);
   assert_float_equal(normal[0], 0.0f, 0.0001f);
@@ -183,7 +183,7 @@ static void test_customShapeFlatLandscape(void **state) {
   // Landscape as A: same depth, negated normal.
   vec3 normalSwapped; float_t depthSwapped;
   assert_true(physicsTestShapeVsShape(
-    landscapePos, landscape, spherePos, sphere, normalSwapped, &depthSwapped
+    landscapePos, &landscape, spherePos, &sphere, normalSwapped, &depthSwapped
   ));
   assert_float_equal(depthSwapped, depth, 0.0001f);
   assert_float_equal(normalSwapped[1], -normal[1], 0.0001f);
@@ -191,7 +191,7 @@ static void test_customShapeFlatLandscape(void **state) {
   // Sphere far above the ground: no overlap.
   vec3 sphereFar = { 0.0f, 10.0f, 0.0f };
   assert_false(physicsTestShapeVsShape(
-    sphereFar, sphere, landscapePos, landscape, normal, &depth
+    sphereFar, &sphere, landscapePos, &landscape, normal, &depth
   ));
 
   assert_int_equal(memoryGetAllocatedCount(), 0);
@@ -208,7 +208,7 @@ static void test_customVsCustomAsserts(void **state) {
   vec3 normal; float_t depth;
 
   expect_assert_failure(physicsTestShapeVsShape(
-    pos, landscapeA, pos, landscapeB, normal, &depth
+    pos, &landscapeA, pos, &landscapeB, normal, &depth
   ));
 }