Refactor Physics further

src/dusk/physics/physicsbodytype.h

@@ -9,11 +9,19 @@
 #include "dusk.h"
 
 typedef enum {
-  /** Never moves. Acts as an immovable collision surface. */
+  /**
+   * Never moves. Acts as an immovable collision surface.
+   */
   PHYSICS_BODY_STATIC,
-  /** Simulated by the world step: gravity, forces, and collision response. */
+
+  /**
+   * Simulated by the world step: gravity, forces, and collision response.
+   */
   PHYSICS_BODY_DYNAMIC,
-  /** Moved programmatically via physicsWorldMoveBody; collides but is not
+  
-   *  driven by the simulation. Typical use: player character controller. */
+  /**
+   * Moved programmatically via physicsWorldMoveBody; collides but is not
+   * driven by the simulation. Typical use: player character controller.
+   */
   PHYSICS_BODY_KINEMATIC
 } physicsbodytype_t;

src/dusk/physics/physicstest.c

@@ -7,19 +7,236 @@
 
 #include "physicstest.h"
 
+/* ============================================================
+ * Forward declarations
+ * ============================================================ */
 
-/* ===========================================================
+/**
- * Low-level collision primitives.
+ * Tests overlap between two axis-aligned bounding boxes.
- * Convention for all helpers:
+ * outNormal points from B toward A.
- *   outNormal  — points from shape-B toward shape-A
+ *
- *                (add outNormal * outDepth to A to separate it from B)
+ * @param ac  Center of AABB A.
- *   outDepth   — positive penetration depth
+ * @param ah  Half-extents of AABB A.
- *   return     — true if overlapping
+ * @param bc  Center of AABB B.
- * =========================================================== */
+ * @param bh  Half-extents of AABB B.
+ * @param outNormal  Push-out normal (B toward A).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestAabbVsAabb(
+  const vec3 ac, const vec3 ah,
+  const vec3 bc, const vec3 bh,
+  vec3 outNormal, float_t *outDepth
+);
 
-static bool aabbVsAabb(
+/**
-  const vec3 ac, const vec3 ah,   /* A center, half-extents */
+ * Tests overlap between two spheres.
-  const vec3 bc, const vec3 bh,   /* B center, half-extents */
+ * outNormal points from B toward A.
+ *
+ * @param ac  Center of sphere A.
+ * @param ar  Radius of sphere A.
+ * @param bc  Center of sphere B.
+ * @param br  Radius of sphere B.
+ * @param outNormal  Push-out normal (B toward A).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestSphereVsSphere(
+  const vec3 ac, const float_t ar,
+  const vec3 bc, const float_t br,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between a sphere and an axis-aligned bounding box.
+ * outNormal points from the AABB toward the sphere.
+ *
+ * @param sc  Center of the sphere.
+ * @param sr  Radius of the sphere.
+ * @param ac  Center of the AABB.
+ * @param ah  Half-extents of the AABB.
+ * @param outNormal  Push-out normal (AABB toward sphere).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestSphereVsAabb(
+  const vec3 sc, const float_t sr,
+  const vec3 ac, const vec3 ah,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between a sphere and an infinite plane.
+ * outNormal equals the plane normal (pointing away from the surface).
+ *
+ * @param sc  Center of the sphere.
+ * @param sr  Radius of the sphere.
+ * @param pn  Plane normal (unit vector, world-space).
+ * @param pd  Plane offset: dot(pn, surfacePoint) == pd.
+ * @param outNormal  Push-out normal (equals pn).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestSphereVsPlane(
+  const vec3 sc, const float_t sr,
+  const vec3 pn, const float_t pd,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between an AABB and an infinite plane.
+ * outNormal equals the plane normal.
+ *
+ * @param ac  Center of the AABB.
+ * @param ah  Half-extents of the AABB.
+ * @param pn  Plane normal (unit vector, world-space).
+ * @param pd  Plane offset (see physicsTestSphereVsPlane).
+ * @param outNormal  Push-out normal (equals pn).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestAabbVsPlane(
+  const vec3 ac, const vec3 ah,
+  const vec3 pn, const float_t pd,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Finds the closest point on segment [a, b] to query point p.
+ *
+ * @param a    Start of the segment.
+ * @param b    End of the segment.
+ * @param p    Query point.
+ * @param out  Receives the closest point on [a, b] to p.
+ */
+static void physicsTestClosestPointOnSegment(
+  const vec3 a, const vec3 b, const vec3 p, vec3 out
+);
+
+/**
+ * Finds the closest points between two line segments.
+ *
+ * @param a1    Start of segment 1.
+ * @param b1    End of segment 1.
+ * @param a2    Start of segment 2.
+ * @param b2    End of segment 2.
+ * @param outP1 Receives the closest point on segment 1.
+ * @param outP2 Receives the closest point on segment 2.
+ */
+static void physicsTestClosestPointsBetweenSegments(
+  const vec3 a1, const vec3 b1,
+  const vec3 a2, const vec3 b2,
+  vec3 outP1, vec3 outP2
+);
+
+/**
+ * Tests overlap between a Y-axis-aligned capsule and a sphere.
+ * outNormal points from the sphere toward the capsule.
+ *
+ * @param cc   Center of the capsule.
+ * @param cr   Radius of the capsule.
+ * @param chh  Half-height of the capsule's cylindrical segment.
+ * @param sc   Center of the sphere.
+ * @param sr   Radius of the sphere.
+ * @param outNormal  Push-out normal (sphere toward capsule).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestCapsuleVsSphere(
+  const vec3 cc, const float_t cr, const float_t chh,
+  const vec3 sc, const float_t sr,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between a Y-axis-aligned capsule and an AABB.
+ * outNormal points from the AABB toward the capsule.
+ *
+ * @param cc   Center of the capsule.
+ * @param cr   Radius of the capsule.
+ * @param chh  Half-height of the capsule's cylindrical segment.
+ * @param ac   Center of the AABB.
+ * @param ah   Half-extents of the AABB.
+ * @param outNormal  Push-out normal (AABB toward capsule).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestCapsuleVsAabb(
+  const vec3 cc, const float_t cr, const float_t chh,
+  const vec3 ac, const vec3 ah,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between a Y-axis-aligned capsule and an infinite plane.
+ * outNormal equals the plane normal.
+ *
+ * @param cc   Center of the capsule.
+ * @param cr   Radius of the capsule.
+ * @param chh  Half-height of the capsule's cylindrical segment.
+ * @param pn   Plane normal (unit vector, world-space).
+ * @param pd   Plane offset (see physicsTestSphereVsPlane).
+ * @param outNormal  Push-out normal (equals pn).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestCapsuleVsPlane(
+  const vec3 cc, const float_t cr, const float_t chh,
+  const vec3 pn, const float_t pd,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Tests overlap between two Y-axis-aligned capsules.
+ * outNormal points from capsule B toward capsule A.
+ *
+ * @param c1   Center of capsule A.
+ * @param r1   Radius of capsule A.
+ * @param hh1  Half-height of capsule A's cylindrical segment.
+ * @param c2   Center of capsule B.
+ * @param r2   Radius of capsule B.
+ * @param hh2  Half-height of capsule B's cylindrical segment.
+ * @param outNormal  Push-out normal (B toward A).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestCapsuleVsCapsule(
+  const vec3 c1, const float_t r1, const float_t hh1,
+  const vec3 c2, const float_t r2, const float_t hh2,
+  vec3 outNormal, float_t *outDepth
+);
+
+/**
+ * Routes a shape-pair collision test to the correct primitive.
+ * When A is a plane, delegates with swapped arguments and negates
+ * the resulting normal. outNormal points from B toward A.
+ *
+ * @param aPos    Position of shape A.
+ * @param aShape  Shape descriptor of A.
+ * @param bPos    Position of shape B.
+ * @param bShape  Shape descriptor of B.
+ * @param outNormal  Push-out normal (B toward A).
+ * @param outDepth   Penetration depth.
+ * @return true if overlapping.
+ */
+static bool physicsTestDispatch(
+  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 bPos, const physicsshape_t bShape,
+  vec3 outNormal, float_t *outDepth
+);
+
+/* ============================================================
+ * Collision primitives.
+ * Convention:
+ *   outNormal — points from shape-B toward shape-A
+ *               (add outNormal * outDepth to A to separate)
+ *   outDepth  — positive penetration depth
+ *   return    — true if overlapping
+ * ============================================================ */
+
+static bool physicsTestAabbVsAabb(
+  const vec3 ac, const vec3 ah,
+  const vec3 bc, const vec3 bh,
   vec3 outNormal, float_t *outDepth
 ) {
   float_t dx = ac[0] - bc[0];
@@ -46,13 +263,13 @@ static bool aabbVsAabb(
   return true;
 }
 
-static bool sphereVsSphere(
+static bool physicsTestSphereVsSphere(
   const vec3 ac, const float_t ar,
   const vec3 bc, const float_t br,
   vec3 outNormal, float_t *outDepth
 ) {
   vec3 diff;
-  glm_vec3_sub((float_t *)ac, (float_t *)bc, diff);  /* A - B */
+  glm_vec3_sub((float_t *)ac, (float_t *)bc, diff);
   float_t dist2 = glm_vec3_norm2(diff);
   float_t sumR  = ar + br;
 
@@ -64,13 +281,14 @@ static bool sphereVsSphere(
   if (dist > 1e-6f) {
     glm_vec3_scale(diff, 1.0f / dist, outNormal);
   } else {
-    outNormal[0] = 0.0f; outNormal[1] = 1.0f; outNormal[2] = 0.0f;
+    outNormal[0] = 0.0f;
+    outNormal[1] = 1.0f;
+    outNormal[2] = 0.0f;
   }
   return true;
 }
 
-/* outNormal: from AABB (b) toward sphere (a) */
+static bool physicsTestSphereVsAabb(
-static bool sphereVsAabb(
   const vec3 sc, const float_t sr,
   const vec3 ac, const vec3 ah,
   vec3 outNormal, float_t *outDepth
@@ -93,7 +311,7 @@ static bool sphereVsAabb(
     *outDepth = sr - dist;
     glm_vec3_scale(diff, 1.0f / dist, outNormal);
   } else {
-    /* Sphere center is inside the AABB — find nearest face. */
+    /* Sphere center inside the AABB — push out via nearest face. */
     float_t faces[6] = {
       (ac[0] + ah[0]) - sc[0],
       sc[0] - (ac[0] - ah[0]),
@@ -109,7 +327,7 @@ static bool sphereVsAabb(
     for (int k = 1; k < 6; k++) {
       if (faces[k] < faces[mi]) mi = k;
     }
-    *outDepth = sr + faces[mi];
+    *outDepth    = sr + faces[mi];
     outNormal[0] = normals[mi][0];
     outNormal[1] = normals[mi][1];
     outNormal[2] = normals[mi][2];
@@ -117,8 +335,7 @@ static bool sphereVsAabb(
   return true;
 }
 
-/* outNormal: plane normal (from plane toward A) */
+static bool physicsTestSphereVsPlane(
-static bool sphereVsPlane(
   const vec3 sc, const float_t sr,
   const vec3 pn, const float_t pd,
   vec3 outNormal, float_t *outDepth
@@ -130,7 +347,7 @@ static bool sphereVsPlane(
   return true;
 }
 
-static bool aabbVsPlane(
+static bool physicsTestAabbVsPlane(
   const vec3 ac, const vec3 ah,
   const vec3 pn, const float_t pd,
   vec3 outNormal, float_t *outDepth
@@ -145,7 +362,7 @@ static bool aabbVsPlane(
   return true;
 }
 
-static void closestPointOnSegment(
+static void physicsTestClosestPointOnSegment(
   const vec3 a, const vec3 b, const vec3 p, vec3 out
 ) {
   vec3 ab, ap;
@@ -158,7 +375,7 @@ static void closestPointOnSegment(
   glm_vec3_lerp((float_t *)a, (float_t *)b, t, out);
 }
 
-static void closestPointsBetweenSegments(
+static void physicsTestClosestPointsBetweenSegments(
   const vec3 a1, const vec3 b1,
   const vec3 a2, const vec3 b2,
   vec3 outP1, vec3 outP2
@@ -187,7 +404,7 @@ static void closestPointsBetweenSegments(
       s = 0.0f;
       t = glm_clamp(-c / a, 0.0f, 1.0f);
     } else {
-      float_t b    = glm_vec3_dot(d1, d2);
+      float_t b     = glm_vec3_dot(d1, d2);
       float_t denom = a * e - b * b;
       t = (fabsf(denom) > 1e-10f)
         ? glm_clamp((b * f - c * e) / denom, 0.0f, 1.0f)
@@ -202,8 +419,7 @@ static void closestPointsBetweenSegments(
 
 /* capsule axis: (cc.x, cc.y ± halfHeight, cc.z), oriented along Y. */
 
-/* outNormal: from sphere toward capsule */
+static bool physicsTestCapsuleVsSphere(
-static bool capsuleVsSphere(
   const vec3 cc, const float_t cr, const float_t chh,
   const vec3 sc, const float_t sr,
   vec3 outNormal, float_t *outDepth
@@ -211,12 +427,13 @@ static bool capsuleVsSphere(
   vec3 capA = { cc[0], cc[1] - chh, cc[2] };
   vec3 capB = { cc[0], cc[1] + chh, cc[2] };
   vec3 closest;
-  closestPointOnSegment(capA, capB, sc, closest);
+  physicsTestClosestPointOnSegment(capA, capB, sc, closest);
-  return sphereVsSphere(closest, cr, sc, sr, outNormal, outDepth);
+  return physicsTestSphereVsSphere(
+    closest, cr, sc, sr, outNormal, outDepth
+  );
 }
 
-/* outNormal: from AABB toward capsule */
+static bool physicsTestCapsuleVsAabb(
-static bool capsuleVsAabb(
   const vec3 cc, const float_t cr, const float_t chh,
   const vec3 ac, const vec3 ah,
   vec3 outNormal, float_t *outDepth
@@ -224,12 +441,13 @@ static bool capsuleVsAabb(
   vec3 capA = { cc[0], cc[1] - chh, cc[2] };
   vec3 capB = { cc[0], cc[1] + chh, cc[2] };
   vec3 closest;
-  closestPointOnSegment(capA, capB, ac, closest);
+  physicsTestClosestPointOnSegment(capA, capB, ac, closest);
-  return sphereVsAabb(closest, cr, ac, ah, outNormal, outDepth);
+  return physicsTestSphereVsAabb(
+    closest, cr, ac, ah, outNormal, outDepth
+  );
 }
 
-/* outNormal: from plane toward capsule */
+static bool physicsTestCapsuleVsPlane(
-static bool capsuleVsPlane(
   const vec3 cc, const float_t cr, const float_t chh,
   const vec3 pn, const float_t pd,
   vec3 outNormal, float_t *outDepth
@@ -245,8 +463,7 @@ static bool capsuleVsPlane(
   return true;
 }
 
-/* outNormal: from capsule-B toward capsule-A */
+static bool physicsTestCapsuleVsCapsule(
-static bool capsuleVsCapsule(
   const vec3 c1, const float_t r1, const float_t hh1,
   const vec3 c2, const float_t r2, const float_t hh2,
   vec3 outNormal, float_t *outDepth
@@ -256,14 +473,14 @@ static bool capsuleVsCapsule(
   vec3 a2 = { c2[0], c2[1] - hh2, c2[2] };
   vec3 b2 = { c2[0], c2[1] + hh2, c2[2] };
   vec3 p1, p2;
-  closestPointsBetweenSegments(a1, b1, a2, b2, p1, p2);
+  physicsTestClosestPointsBetweenSegments(a1, b1, a2, b2, p1, p2);
-  return sphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
+  return physicsTestSphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
 }
 
-/* ===========================================================
+/* ============================================================
- * Dispatch: tests two shapes and returns push-out for A.
+ * Dispatch
- * outNormal points from B toward A.
+ * ============================================================ */
- * =========================================================== */
+
 static bool physicsTestDispatch(
   const vec3 aPos, const physicsshape_t aShape,
   const vec3 bPos, const physicsshape_t bShape,
@@ -272,26 +489,39 @@ static bool physicsTestDispatch(
   physicshapetype_t ta = aShape.type;
   physicshapetype_t tb = bShape.type;
 
-  /* Plane is always the reference surface; treat as B. */
+  /* Plane is always the reference surface; keep it as B. */
   if (tb == PHYSICS_SHAPE_PLANE) {
     const float_t *pn = bShape.data.plane.normal;
     const float_t  pd = bShape.data.plane.distance;
     switch (ta) {
       case PHYSICS_SHAPE_CUBE:
-        return aabbVsPlane(aPos, aShape.data.cube.halfExtents, pn, pd, outNormal, outDepth);
+        return physicsTestAabbVsPlane(
+          aPos, aShape.data.cube.halfExtents,
+          pn, pd, outNormal, outDepth
+        );
       case PHYSICS_SHAPE_SPHERE:
-        return sphereVsPlane(aPos, aShape.data.sphere.radius, pn, pd, outNormal, outDepth);
+        return physicsTestSphereVsPlane(
+          aPos, aShape.data.sphere.radius,
+          pn, pd, outNormal, outDepth
+        );
       case PHYSICS_SHAPE_CAPSULE:
-        return capsuleVsPlane(aPos, aShape.data.capsule.radius, aShape.data.capsule.halfHeight, pn, pd, outNormal, outDepth);
+        return physicsTestCapsuleVsPlane(
+          aPos,
+          aShape.data.capsule.radius,
+          aShape.data.capsule.halfHeight,
+          pn, pd, outNormal, outDepth
+        );
       default:
         return false;
     }
   }
 
-  /* If A is a plane, swap roles and negate. */
+  /* If A is a plane, swap roles and negate the normal. */
   if (ta == PHYSICS_SHAPE_PLANE) {
     vec3 tmp; float_t d;
-    if (!physicsTestDispatch(bPos, bShape, aPos, aShape, tmp, &d)) return false;
+    if (!physicsTestDispatch(
+      bPos, bShape, aPos, aShape, tmp, &d
+    )) return false;
     glm_vec3_scale(tmp, -1.0f, outNormal);
     *outDepth = d;
     return true;
@@ -299,19 +529,36 @@ static bool physicsTestDispatch(
 
   switch (ta) {
     case PHYSICS_SHAPE_CUBE: {
-      const float_t *ac = aPos, *ah = aShape.data.cube.halfExtents;
+      const float_t *ac = aPos;
+      const float_t *ah = aShape.data.cube.halfExtents;
       switch (tb) {
         case PHYSICS_SHAPE_CUBE:
-          return aabbVsAabb(ac, ah, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
+          return physicsTestAabbVsAabb(
+            ac, ah,
+            bPos, bShape.data.cube.halfExtents,
+            outNormal, outDepth
+          );
         case PHYSICS_SHAPE_SPHERE: {
           vec3 tmp; float_t d;
-          if (!sphereVsAabb(bPos, bShape.data.sphere.radius, ac, ah, tmp, &d)) return false;
+          if (!physicsTestSphereVsAabb(
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
+            bPos, bShape.data.sphere.radius,
+            ac, ah, tmp, &d
+          )) return false;
+          glm_vec3_scale(tmp, -1.0f, outNormal);
+          *outDepth = d;
+          return true;
         }
         case PHYSICS_SHAPE_CAPSULE: {
           vec3 tmp; float_t d;
-          if (!capsuleVsAabb(bPos, bShape.data.capsule.radius, bShape.data.capsule.halfHeight, ac, ah, tmp, &d)) return false;
+          if (!physicsTestCapsuleVsAabb(
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
+            bPos,
+            bShape.data.capsule.radius,
+            bShape.data.capsule.halfHeight,
+            ac, ah, tmp, &d
+          )) return false;
+          glm_vec3_scale(tmp, -1.0f, outNormal);
+          *outDepth = d;
+          return true;
         }
         default: return false;
       }
@@ -321,13 +568,28 @@ static bool physicsTestDispatch(
       const float_t sr = aShape.data.sphere.radius;
       switch (tb) {
         case PHYSICS_SHAPE_CUBE:
-          return sphereVsAabb(aPos, sr, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
+          return physicsTestSphereVsAabb(
+            aPos, sr,
+            bPos, bShape.data.cube.halfExtents,
+            outNormal, outDepth
+          );
         case PHYSICS_SHAPE_SPHERE:
-          return sphereVsSphere(aPos, sr, bPos, bShape.data.sphere.radius, outNormal, outDepth);
+          return physicsTestSphereVsSphere(
+            aPos, sr,
+            bPos, bShape.data.sphere.radius,
+            outNormal, outDepth
+          );
         case PHYSICS_SHAPE_CAPSULE: {
           vec3 tmp; float_t d;
-          if (!capsuleVsSphere(bPos, bShape.data.capsule.radius, bShape.data.capsule.halfHeight, aPos, sr, tmp, &d)) return false;
+          if (!physicsTestCapsuleVsSphere(
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
+            bPos,
+            bShape.data.capsule.radius,
+            bShape.data.capsule.halfHeight,
+            aPos, sr, tmp, &d
+          )) return false;
+          glm_vec3_scale(tmp, -1.0f, outNormal);
+          *outDepth = d;
+          return true;
         }
         default: return false;
       }
@@ -338,11 +600,25 @@ static bool physicsTestDispatch(
       const float_t chh = aShape.data.capsule.halfHeight;
       switch (tb) {
         case PHYSICS_SHAPE_CUBE:
-          return capsuleVsAabb(aPos, cr, chh, bPos, bShape.data.cube.halfExtents, outNormal, outDepth);
+          return physicsTestCapsuleVsAabb(
+            aPos, cr, chh,
+            bPos, bShape.data.cube.halfExtents,
+            outNormal, outDepth
+          );
         case PHYSICS_SHAPE_SPHERE:
-          return capsuleVsSphere(aPos, cr, chh, bPos, bShape.data.sphere.radius, outNormal, outDepth);
+          return physicsTestCapsuleVsSphere(
+            aPos, cr, chh,
+            bPos, bShape.data.sphere.radius,
+            outNormal, outDepth
+          );
         case PHYSICS_SHAPE_CAPSULE:
-          return capsuleVsCapsule(aPos, cr, chh, bPos, bShape.data.capsule.radius, bShape.data.capsule.halfHeight, outNormal, outDepth);
+          return physicsTestCapsuleVsCapsule(
+            aPos, cr, chh,
+            bPos,
+            bShape.data.capsule.radius,
+            bShape.data.capsule.halfHeight,
+            outNormal, outDepth
+          );
         default: return false;
       }
     }
@@ -356,5 +632,7 @@ bool_t physicsTestShapeVsShape(
   const vec3 bPos, const physicsshape_t bShape,
   vec3 outNormal, float_t *outDepth
 ) {
-  return physicsTestDispatch(aPos, aShape, bPos, bShape, outNormal, outDepth);
+  return physicsTestDispatch(
+    aPos, aShape, bPos, bShape, outNormal, outDepth
+  );
 }

src/dusk/physics/physicstest.h

@@ -1,6 +1,6 @@
 /**
  * Copyright (c) 2026 Dominic Masters
- * 
+ *
  * This software is released under the MIT License.
  * https://opensource.org/licenses/MIT
  */
@@ -9,24 +9,25 @@
 #include "physicsshape.h"
 
 /**
- * Tests for collision between two shapes. Returns true if they overlap, and if
+ * Tests for collision between two shapes. Returns true if they
- * so, outputs the push-out normal and depth.
+ * overlap, and if so, outputs the push-out normal and depth.
- * 
+ *
- * @param aPos Position of shape A.
+ * outNormal always points from shape B toward shape A, so adding
- * @param aShape Shape of A.
+ * (outNormal * outDepth) to A's position separates the two shapes.
- * @param bPos Position of shape B.
+ *
- * @param bShape Shape of B.
+ * @param aPos    Position of shape A.
- * @param outNormal Output push-out normal (points from B toward A).
+ * @param aShape  Shape descriptor of A.
- * @param outDepth Output penetration depth (positive).
+ * @param bPos    Position of shape B.
- * @return true if shapes overlap, false otherwise.
+ * @param bShape  Shape descriptor of B.
+ * @param outNormal  Push-out normal, pointing from B toward A.
+ * @param outDepth   Penetration depth (positive when overlapping).
+ * @return true if the shapes overlap, false otherwise.
  */
 bool_t physicsTestShapeVsShape(
   const vec3 aPos,
   const physicsshape_t aShape,
-  
   const vec3 bPos,
   const physicsshape_t bShape,
-
   vec3 outNormal,
   float_t *outDepth
-);
+);

src/dusk/physics/physicsworld.c

@@ -54,7 +54,7 @@ void physicsWorldStep(const float_t dt) {
     entityPositionSetPosition(id, posComp, pos);
   }
 
-  /* Phase 2: dynamic vs static/kinematic — push dynamic fully. */
+  // Phase 2: Dynamic vs Static/Kinematic.
   for(entityid_t i = 0; i < physCount; i++) {
     entityid_t id = physEnts[i];
     componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
@@ -69,7 +69,9 @@ void physicsWorldStep(const float_t dt) {
     for(entityid_t j = 0; j < physCount; j++) {
       if(i == j) continue;
       entityid_t otherId = physEnts[j];
-      componentid_t otherPosComp = entityGetComponent(otherId, COMPONENT_TYPE_POSITION);
+      componentid_t otherPosComp = entityGetComponent(
+        otherId, COMPONENT_TYPE_POSITION
+      );
       if(otherPosComp == 0xFF) continue;
 
       entityphysics_t *otherPhys = entityPhysicsGet(otherId, physComps[j]);
@@ -99,7 +101,7 @@ void physicsWorldStep(const float_t dt) {
     }
   }
 
-  /* Phase 3: dynamic vs dynamic — push each half-way, exchange velocities. */
+  // Phase 3: Dynamic vs Dynamic
   for(entityid_t i = 0; i < physCount; i++) {
     entityid_t idA = physEnts[i];
     componentid_t posCompA = entityGetComponent(idA, COMPONENT_TYPE_POSITION);

src/dusk/physics/physicsworld.h

@@ -27,18 +27,4 @@ void physicsWorldInit(void);
  * 
  * @param dt The time delta in seconds since the last step.
  */
-void physicsWorldStep(const float_t dt);
+void physicsWorldStep(const float_t dt);
-
-/**
- * Moves a KINEMATIC body by motion and immediately resolves overlaps against
- * all STATIC and DYNAMIC bodies. Sets onGround when landing on a surface.
- *
- * @param world  The physics world.
- * @param body   The kinematic body to move (must be PHYSICS_BODY_KINEMATIC).
- * @param motion World-space displacement for this frame.
- */
-// void physicsWorldMoveBody(
-//   physicsworld_t *world,
-//   physicsbody_t *body,
-//   const vec3 motion
-// );