Physics refactor

src/dusk/engine/engine.c

@@ -17,6 +17,7 @@
 #include "script/scriptmanager.h"
 #include "assert/assert.h"
 #include "entity/entitymanager.h"
+#include "entity/component/physics/entityphysics.h"
 #include "game/game.h"
 #include "physics/physicsmanager.h"
 #include "display/mesh/cube.h"
@@ -24,10 +25,9 @@
 
 engine_t ENGINE;
 
-/* Physics demo entities */
+/* Kept module-level only because engineUpdate needs them for the reset. */
-static entityid_t    phFloorEnt, phBoxEnt;
+static entityid_t    phBoxEnt;
-static componentid_t phFloorPos, phFloorMesh, phFloorMat;
+static componentid_t phBoxPhys;
-static componentid_t phBoxPos,   phBoxMesh,   phBoxMat,   phBoxPhys;
 
 errorret_t engineInit(const int32_t argc, const char_t **argv) {
   memoryZero(&ENGINE, sizeof(engine_t));
@@ -62,50 +62,38 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) {
   entityCameraSetZFar(cam, camCam, distance * 6.0f);
 
   /* ---- Static floor (visual + physics) ---- */
-  phFloorEnt  = entityManagerAdd();
+  entityid_t floorEnt     = entityManagerAdd();
-  phFloorPos  = entityAddComponent(phFloorEnt, COMPONENT_TYPE_POSITION);
+  componentid_t floorPos  = entityAddComponent(floorEnt, COMPONENT_TYPE_POSITION);
-  phFloorMesh = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MESH);
+  componentid_t floorMesh = entityAddComponent(floorEnt, COMPONENT_TYPE_MESH);
-  phFloorMat  = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MATERIAL);
+  componentid_t floorMat  = entityAddComponent(floorEnt, COMPONENT_TYPE_MATERIAL);
+  componentid_t floorPhys = entityAddComponent(floorEnt, COMPONENT_TYPE_PHYSICS);
 
-  /* Scale the unit XZ plane to 10×10, centred on origin */
+  entityPositionSetPosition(floorEnt, floorPos, (vec3){ -5.0f, 0.0f, -5.0f });
-  entityPositionSetPosition(phFloorEnt, phFloorPos, (vec3){ -5.0f, 0.0f, -5.0f });
+  entityPositionSetScale(floorEnt, floorPos, (vec3){ 10.0f, 1.0f, 10.0f });
-  entityPositionSetScale(phFloorEnt, phFloorPos, (vec3){ 10.0f, 1.0f, 10.0f });
+  entityMeshSetMesh(floorEnt, floorMesh, &PLANE_MESH_SIMPLE);
-  entityMeshSetMesh(phFloorEnt, phFloorMesh, &PLANE_MESH_SIMPLE);
+  entityMaterialGetShaderMaterial(floorEnt, floorMat)->unlit.color = COLOR_GREEN;
-  entityMaterialGetShaderMaterial(phFloorEnt, phFloorMat)->unlit.color = COLOR_GREEN;
 
-  /* No PHYSICS component for the floor — we add the body manually so it never
+  entityphysics_t *floorPhysData = entityPhysicsGet(floorEnt, floorPhys);
-   * gets disposed by the entity system before we're done with it. */
+  floorPhysData->type                       = PHYSICS_BODY_STATIC;
-  physicsbody_t *floorBody = physicsWorldAddBody(&PHYSICS_WORLD);
+  floorPhysData->shape.type                 = PHYSICS_SHAPE_PLANE;
-  floorBody->type                   = PHYSICS_BODY_STATIC;
+  floorPhysData->shape.data.plane.normal[0] = 0.0f;
-  floorBody->shape.type             = PHYSICS_SHAPE_PLANE;
+  floorPhysData->shape.data.plane.normal[1] = 1.0f;
-  floorBody->shape.data.plane.normal[0]   = 0.0f;
+  floorPhysData->shape.data.plane.normal[2] = 0.0f;
-  floorBody->shape.data.plane.normal[1]   = 1.0f;
+  floorPhysData->shape.data.plane.distance  = 0.0f;
-  floorBody->shape.data.plane.normal[2]   = 0.0f;
-  floorBody->shape.data.plane.distance    = 0.0f;
 
   /* ---- Dynamic box ---- */
   phBoxEnt  = entityManagerAdd();
-  phBoxPos  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
+  componentid_t boxPos  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
-  phBoxMesh = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MESH);
+  componentid_t boxMesh = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MESH);
-  phBoxMat  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MATERIAL);
+  componentid_t boxMat  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MATERIAL);
   phBoxPhys = entityAddComponent(phBoxEnt, COMPONENT_TYPE_PHYSICS);
 
-  entityMeshSetMesh(phBoxEnt, phBoxMesh, &CUBE_MESH_SIMPLE);
+  entityMeshSetMesh(phBoxEnt, boxMesh, &CUBE_MESH_SIMPLE);
-  entityMaterialGetShaderMaterial(phBoxEnt, phBoxMat)->unlit.color = COLOR_RED;
+  entityMaterialGetShaderMaterial(phBoxEnt, boxMat)->unlit.color = COLOR_RED;
 
-  /* Start the box 4 units above the floor; default shape is a unit AABB */
+  /* Physics position lives in the POSITION component. CUBE_MESH_SIMPLE is
-  physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
+   * centred at origin (-0.5..0.5), so entity position == physics centre. */
-  boxBody->position[0] = 0.0f;
+  entityPositionSetPosition(phBoxEnt, boxPos, (vec3){ 0.0f, 4.0f, 0.0f });
-  boxBody->position[1] = 4.0f;
-  boxBody->position[2] = 0.0f;
-
-  /* Sync visual position for first frame */
-  vec3 visualPos = {
-    boxBody->position[0] - boxBody->shape.data.cube.halfExtents[0],
-    boxBody->position[1] - boxBody->shape.data.cube.halfExtents[1],
-    boxBody->position[2] - boxBody->shape.data.cube.halfExtents[2]
-  };
-  entityPositionSetPosition(phBoxEnt, phBoxPos, visualPos);
 
   /* Run the init script. */
   scriptcontext_t ctx;
@@ -123,27 +111,16 @@ errorret_t engineUpdate(void) {
   uiUpdate();
   errorChain(sceneUpdate());
 
-  // Reset cube
+  /* Reset the box to its start position on demand. */
   if(inputIsDown(INPUT_ACTION_ACCEPT)) {
-    entityPositionSetPosition(phBoxEnt, phBoxPos, (vec3){ 0.0f, 4.0f, 0.0f });
+    componentid_t posComp = entityGetComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
-    physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
+    entityPositionSetPosition(phBoxEnt, posComp, (vec3){ 0.0f, 4.0f, 0.0f });
-    physicsBodySetPosition(boxBody, (vec3){ 0.0f, 4.0f, 0.0f });
+    entityPhysicsSetVelocity(phBoxEnt, phBoxPhys, (vec3){ 0.0f, 0.0f, 0.0f });
-    physicsBodySetVelocity(boxBody, (vec3){ 0.0f, 0.0f, 0.0f });
   }
 
-
+  /* Step physics — positions are updated directly on POSITION components. */
-  /* Step physics simulation */
   physicsManagerUpdate();
 
-  /* Sync dynamic box visual to physics body (centre → corner offset) */
-  physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
-  vec3 visualPos = {
-    boxBody->position[0] - boxBody->shape.data.cube.halfExtents[0],
-    boxBody->position[1] - boxBody->shape.data.cube.halfExtents[1],
-    boxBody->position[2] - boxBody->shape.data.cube.halfExtents[2]
-  };
-  entityPositionSetPosition(phBoxEnt, phBoxPos, visualPos);
-
   errorChain(gameUpdate());
   errorChain(displayUpdate());
 

src/dusk/entity/component/physics/entityphysics.c

@@ -10,65 +10,98 @@
 #include "entity/component/display/entityposition.h"
 #include "physics/physicsmanager.h"
 #include "assert/assert.h"
+#include "util/memory.h"
 
 void entityPhysicsInit(
   const entityid_t entityId,
   const componentid_t componentId
 ) {
-  entityphysics_t *phys = componentGetData(
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
-    entityId, componentId, COMPONENT_TYPE_PHYSICS
+  assertNotNull(phys, "Failed to get physics component data");
-  );
 
-  phys->body = physicsWorldAddBody(&PHYSICS_WORLD);
+  memoryZero(phys, sizeof(entityphysics_t));
-  assertNotNull(phys->body, "Physics world body limit reached");
+  
+  // Default to cube
+  phys->type = PHYSICS_BODY_DYNAMIC;
+  phys->shape.type = PHYSICS_SHAPE_CUBE;
+  phys->shape.data.cube.halfExtents[0] = 0.5f;
+  phys->shape.data.cube.halfExtents[1] = 0.5f;
+  phys->shape.data.cube.halfExtents[2] = 0.5f;
+  phys->gravityScale = 1.0f;
+  phys->onGround = false;
 }
 
-void entityPhysicsSyncPosition(
+entityphysics_t *entityPhysicsGet(
   const entityid_t entityId,
   const componentid_t componentId
 ) {
-  entityphysics_t *phys = componentGetData(
+  return componentGetData(entityId, componentId, COMPONENT_TYPE_PHYSICS);
-    entityId, componentId, COMPONENT_TYPE_PHYSICS
-  );
-  assertNotNull(phys->body, "Physics body is NULL");
-
-  /* Find the entity's POSITION component and update it. */
-  componentid_t posId = entityGetComponent(entityId, COMPONENT_TYPE_POSITION);
-  if (posId == 0xFF) return; /* entity has no POSITION component */
-
-  entityPositionSetPosition(entityId, posId, phys->body->position);
 }
 
-physicsbody_t *entityPhysicsGetBody(
+void entityPhysicsSetShape(
-  const entityid_t entityId,
-  const componentid_t componentId
-) {
-  entityphysics_t *phys = componentGetData(
-    entityId, componentId, COMPONENT_TYPE_PHYSICS
-  );
-  return phys->body;
-}
-
-void entityPhysicsMove(
   const entityid_t entityId,
   const componentid_t componentId,
-  const vec3 motion
+  const physicsshape_t shape
 ) {
-  entityphysics_t *phys = componentGetData(
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
-    entityId, componentId, COMPONENT_TYPE_PHYSICS
+  assertNotNull(phys, "Failed to get physics component data");
-  );
+  phys->shape = shape;
-  assertNotNull(phys->body, "Physics body is NULL");
+  // TODO: Do I need to reset the state for ground/active?
-  physicsWorldMoveBody(&PHYSICS_WORLD, phys->body, motion);
+}
+
+physicsshape_t entityPhysicsGetShape(
+  const entityid_t entityId,
+  const componentid_t componentId
+) {
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
+  assertNotNull(phys, "Failed to get physics component data");
+  return phys->shape;
+}
+
+void entityPhysicsGetVelocity(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 dest
+) {
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
+  assertNotNull(phys, "Failed to get physics component data");
+
+  glm_vec3_copy(phys->velocity, dest);
+}
+
+void entityPhysicsSetVelocity(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 velocity
+) {
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
+  assertNotNull(phys, "Failed to get physics component data");
+  glm_vec3_copy(velocity, phys->velocity);
+}
+
+void entityPhysicsApplyImpulse(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 impulse
+) {
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
+  assertNotNull(phys, "Failed to get physics component data");
+
+  if (phys->type == PHYSICS_BODY_STATIC) return;
+  glm_vec3_add(phys->velocity, impulse, phys->velocity);
+}
+
+bool_t entityPhysicsIsOnGround(
+  const entityid_t entityId,
+  const componentid_t componentId
+) {
+  entityphysics_t *phys = entityPhysicsGet(entityId, componentId);
+  assertNotNull(phys, "Failed to get physics component data");
+  return phys->onGround;
 }
 
 void entityPhysicsDispose(
   const entityid_t entityId,
   const componentid_t componentId
 ) {
-  entityphysics_t *phys = componentGetData(
-    entityId, componentId, COMPONENT_TYPE_PHYSICS
-  );
-  if (!phys->body) return;
-  physicsWorldRemoveBody(&PHYSICS_WORLD, phys->body);
-  phys->body = NULL;
 }

src/dusk/entity/component/physics/entityphysics.h

@@ -7,11 +7,15 @@
 
 #pragma once
 #include "entity/entitybase.h"
-#include "physics/physicsbody.h"
+#include "physics/physicsshape.h"
+#include "physics/physicsbodytype.h"
 
 typedef struct {
-  /** Pointer into PHYSICS_WORLD.bodies[]. Allocated on component init. */
+  physicsbodytype_t type;
-  physicsbody_t *body;
+  physicsshape_t shape;
+  vec3 velocity;
+  float_t gravityScale;
+  bool_t onGround;
 } entityphysics_t;
 
 /**
@@ -24,30 +28,98 @@ void entityPhysicsInit(
 );
 
 /**
- * Copies the physics body's position back into the entity's POSITION
+ * Gets the underlying physics structure (temporarily) for the given entity.
- * component (if present). Call this after physicsManagerStep each frame.
+ * This is really just intended for doing operations faster than using the
+ * getters and setters, but it is preferred that you use those.
+ * 
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @return The physics component data for the given entity and component ID.
  */
-void entityPhysicsSyncPosition(
+entityphysics_t *entityPhysicsGet(
   const entityid_t entityId,
   const componentid_t componentId
 );
 
 /**
- * Returns the raw physics body for direct manipulation.
+ * Sets the shape of the entity's physics body. This will not reset the body
+ * state, so if you change from a cube to a sphere, it will keep the same
+ * velocity and onGround state.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @param shape The new shape to set on the physics body.
  */
-physicsbody_t *entityPhysicsGetBody(
+void entityPhysicsSetShape(
-  const entityid_t entityId,
-  const componentid_t componentId
-);
-
-/**
- * Moves a KINEMATIC body by the given world-space motion vector and resolves
- * collisions. Convenience wrapper around physicsWorldMoveBody.
- */
-void entityPhysicsMove(
   const entityid_t entityId,
   const componentid_t componentId,
-  const vec3 motion
+  const physicsshape_t shape
+);
+
+/**
+ * Gets the shape of the entity's physics body.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @return The shape of the physics body.
+ */
+physicsshape_t entityPhysicsGetShape(
+  const entityid_t entityId,
+  const componentid_t componentId
+);
+
+/**
+ * Gets the velocity of the entity's physics body.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @param dest The destination vec3 to write the velocity to.
+ */
+void entityPhysicsGetVelocity(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 dest
+);
+
+/**
+ * Sets the velocity of the entity's physics body. This is not an impulse, so
+ * it will be affected by mass and drag.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @param velocity The new velocity to set on the physics body.
+ */
+void entityPhysicsSetVelocity(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 velocity
+);
+
+/**
+ * Applies an impulse to the entity's physics body. This is an immediate
+ * velocity change that is not affected by mass or drag. No-op on STATIC bodies.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @param impulse The impulse to apply to the physics body.
+ */
+void entityPhysicsApplyImpulse(
+  const entityid_t entityId,
+  const componentid_t componentId,
+  vec3 impulse
+);
+
+/**
+ * Returns true if the entity's physics body rested on a surface during the last
+ * step or move.
+ *
+ * @param entityId The entity ID.
+ * @param componentId The component ID.
+ * @return True if the body is on the ground, false otherwise.
+ */
+bool_t entityPhysicsIsOnGround(
+  const entityid_t entityId,
+  const componentid_t componentId
 );
 
 /**

src/dusk/entity/entity.h

@@ -34,7 +34,8 @@ componentid_t entityAddComponent(
 );
 
 /**
- * Gets the ID of the component of the given type on the entity with the given ID.
+ * Gets the ID of the component of the given type on the entity with the given
+ * ID, or 0xFF if the entity lacks the component.
  * 
  * @param entityId The ID of the entity to get the component from.
  * @param type The type of the component to get.

src/dusk/physics/CMakeLists.txt

@@ -7,6 +7,6 @@
 target_sources(${DUSK_LIBRARY_TARGET_NAME}
   PUBLIC
     physicsmanager.c
-    physicsbody.c
     physicsworld.c
+    physicstest.c
 )

src/dusk/physics/physicsbody.c

@@ -1,33 +0,0 @@
-/**
- * Copyright (c) 2026 Dominic Masters
- *
- * This software is released under the MIT License.
- * https://opensource.org/licenses/MIT
- */
-
-#include "physicsbody.h"
-
-void physicsBodySetPosition(physicsbody_t *body, const vec3 position) {
-  glm_vec3_copy((float_t *)position, body->position);
-}
-
-void physicsBodyGetPosition(const physicsbody_t *body, vec3 out) {
-  glm_vec3_copy((float_t *)body->position, out);
-}
-
-void physicsBodySetVelocity(physicsbody_t *body, const vec3 velocity) {
-  glm_vec3_copy((float_t *)velocity, body->velocity);
-}
-
-void physicsBodyGetVelocity(const physicsbody_t *body, vec3 out) {
-  glm_vec3_copy((float_t *)body->velocity, out);
-}
-
-void physicsBodyApplyImpulse(physicsbody_t *body, const vec3 impulse) {
-  if (body->type == PHYSICS_BODY_STATIC) return;
-  glm_vec3_add(body->velocity, (float_t *)impulse, body->velocity);
-}
-
-bool physicsBodyIsOnGround(const physicsbody_t *body) {
-  return body->onGround;
-}

src/dusk/physics/physicsbody.h

@@ -1,57 +0,0 @@
-/**
- * Copyright (c) 2026 Dominic Masters
- *
- * This software is released under the MIT License.
- * https://opensource.org/licenses/MIT
- */
-
-#pragma once
-#include "physicsshape.h"
-#include "physicsbodytype.h"
-
-typedef struct {
-  bool active;
-  physicsbodytype_t type;
-  physicsshape_t shape;
-  vec3 position;
-  /**
-   * Linear velocity (m/s).  For KINEMATIC bodies this is not driven by the
-   * simulation — set it yourself and pass velocity*dt to physicsWorldMoveBody.
-   */
-  vec3 velocity;
-  /** Multiplier applied to world gravity (1.0 = normal, 0.0 = no gravity). */
-  float_t gravityScale;
-  /** Set to true after a downward-facing collision is resolved. Reset each
-   *  step / each physicsWorldMoveBody call. */
-  bool onGround;
-} physicsbody_t;
-
-/**
- * Copies position into the body.
- */
-void physicsBodySetPosition(physicsbody_t *body, const vec3 position);
-
-/**
- * Copies the body's current position into out.
- */
-void physicsBodyGetPosition(const physicsbody_t *body, vec3 out);
-
-/**
- * Copies velocity into the body.
- */
-void physicsBodySetVelocity(physicsbody_t *body, const vec3 velocity);
-
-/**
- * Copies the body's current velocity into out.
- */
-void physicsBodyGetVelocity(const physicsbody_t *body, vec3 out);
-
-/**
- * Adds impulse (immediate velocity change) to a body. No-op on STATIC bodies.
- */
-void physicsBodyApplyImpulse(physicsbody_t *body, const vec3 impulse);
-
-/**
- * Returns true when the body rested on a surface during the last step or move.
- */
-bool physicsBodyIsOnGround(const physicsbody_t *body);

src/dusk/physics/physicsbodytype.h

@@ -6,6 +6,7 @@
  */
 
 #pragma once
+#include "dusk.h"
 
 typedef enum {
   /** Never moves. Acts as an immovable collision surface. */

src/dusk/physics/physicsmanager.c

@@ -8,10 +8,8 @@
 #include "physicsmanager.h"
 #include "time/time.h"
 
-physicsworld_t PHYSICS_WORLD;
-
 void physicsManagerInit(void) {
-  physicsWorldInit(&PHYSICS_WORLD);
+  physicsWorldInit();
 }
 
 void physicsManagerUpdate() {
@@ -19,5 +17,5 @@ void physicsManagerUpdate() {
     if(TIME.dynamicUpdate) return; // Don't update on dynamic updates.
   #endif
 
-  physicsWorldStep(&PHYSICS_WORLD, TIME.delta);
+  physicsWorldStep(TIME.delta);
-}
+}

src/dusk/physics/physicsmanager.h

@@ -8,14 +8,12 @@
 #pragma once
 #include "physicsworld.h"
 
-extern physicsworld_t PHYSICS_WORLD;
-
 /**
- * Initializes the global physics world with default gravity (0, -9.81, 0).
+ * Initializes the physics manager.
  */
 void physicsManagerInit(void);
 
 /**
  * Advances the physics simulation.
  */
-void physicsManagerUpdate();
+void physicsManagerUpdate();

src/dusk/physics/physicstest.c

@@ -0,0 +1,360 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#include "physicstest.h"
+
+
+/* ===========================================================
+ * Low-level collision primitives.
+ * Convention for all helpers:
+ *   outNormal  — points from shape-B toward shape-A
+ *                (add outNormal * outDepth to A to separate it from B)
+ *   outDepth   — positive penetration depth
+ *   return     — true if overlapping
+ * =========================================================== */
+
+static bool aabbVsAabb(
+  const vec3 ac, const vec3 ah,   /* A center, half-extents */
+  const vec3 bc, const vec3 bh,   /* B center, half-extents */
+  vec3 outNormal, float_t *outDepth
+) {
+  float_t dx = ac[0] - bc[0];
+  float_t dy = ac[1] - bc[1];
+  float_t dz = ac[2] - bc[2];
+
+  float_t px = (ah[0] + bh[0]) - fabsf(dx);
+  float_t py = (ah[1] + bh[1]) - fabsf(dy);
+  float_t pz = (ah[2] + bh[2]) - fabsf(dz);
+
+  if (px <= 0.0f || py <= 0.0f || pz <= 0.0f) return false;
+
+  outNormal[0] = outNormal[1] = outNormal[2] = 0.0f;
+  if (px < py && px < pz) {
+    *outDepth    = px;
+    outNormal[0] = dx >= 0.0f ? 1.0f : -1.0f;
+  } else if (py < pz) {
+    *outDepth    = py;
+    outNormal[1] = dy >= 0.0f ? 1.0f : -1.0f;
+  } else {
+    *outDepth    = pz;
+    outNormal[2] = dz >= 0.0f ? 1.0f : -1.0f;
+  }
+  return true;
+}
+
+static bool sphereVsSphere(
+  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 */
+  float_t dist2 = glm_vec3_norm2(diff);
+  float_t sumR  = ar + br;
+
+  if (dist2 >= sumR * sumR) return false;
+
+  float_t dist = sqrtf(dist2);
+  *outDepth = sumR - dist;
+
+  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;
+  }
+  return true;
+}
+
+/* outNormal: from AABB (b) toward sphere (a) */
+static bool sphereVsAabb(
+  const vec3 sc, const float_t sr,
+  const vec3 ac, const vec3 ah,
+  vec3 outNormal, float_t *outDepth
+) {
+  vec3 closest = {
+    glm_clamp(sc[0], ac[0] - ah[0], ac[0] + ah[0]),
+    glm_clamp(sc[1], ac[1] - ah[1], ac[1] + ah[1]),
+    glm_clamp(sc[2], ac[2] - ah[2], ac[2] + ah[2])
+  };
+
+  vec3 diff;
+  glm_vec3_sub((float_t *)sc, closest, diff);
+  float_t dist2 = glm_vec3_norm2(diff);
+
+  bool inside = (dist2 < 1e-10f);
+  if (!inside && dist2 >= sr * sr) return false;
+
+  if (!inside) {
+    float_t dist = sqrtf(dist2);
+    *outDepth = sr - dist;
+    glm_vec3_scale(diff, 1.0f / dist, outNormal);
+  } else {
+    /* Sphere center is inside the AABB — find nearest face. */
+    float_t faces[6] = {
+      (ac[0] + ah[0]) - sc[0],
+      sc[0] - (ac[0] - ah[0]),
+      (ac[1] + ah[1]) - sc[1],
+      sc[1] - (ac[1] - ah[1]),
+      (ac[2] + ah[2]) - sc[2],
+      sc[2] - (ac[2] - ah[2])
+    };
+    static const float_t normals[6][3] = {
+      {1,0,0},{-1,0,0},{0,1,0},{0,-1,0},{0,0,1},{0,0,-1}
+    };
+    int mi = 0;
+    for (int k = 1; k < 6; k++) {
+      if (faces[k] < faces[mi]) mi = k;
+    }
+    *outDepth = sr + faces[mi];
+    outNormal[0] = normals[mi][0];
+    outNormal[1] = normals[mi][1];
+    outNormal[2] = normals[mi][2];
+  }
+  return true;
+}
+
+/* outNormal: plane normal (from plane toward A) */
+static bool sphereVsPlane(
+  const vec3 sc, const float_t sr,
+  const vec3 pn, const float_t pd,
+  vec3 outNormal, float_t *outDepth
+) {
+  float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)sc) - pd;
+  *outDepth = sr - signedDist;
+  if (*outDepth <= 0.0f) return false;
+  glm_vec3_copy((float_t *)pn, outNormal);
+  return true;
+}
+
+static bool aabbVsPlane(
+  const vec3 ac, const vec3 ah,
+  const vec3 pn, const float_t pd,
+  vec3 outNormal, float_t *outDepth
+) {
+  float_t proj = fabsf(pn[0] * ah[0])
+               + fabsf(pn[1] * ah[1])
+               + fabsf(pn[2] * ah[2]);
+  float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)ac) - pd;
+  *outDepth = proj - signedDist;
+  if (*outDepth <= 0.0f) return false;
+  glm_vec3_copy((float_t *)pn, outNormal);
+  return true;
+}
+
+static void closestPointOnSegment(
+  const vec3 a, const vec3 b, const vec3 p, vec3 out
+) {
+  vec3 ab, ap;
+  glm_vec3_sub((float_t *)b, (float_t *)a, ab);
+  glm_vec3_sub((float_t *)p, (float_t *)a, ap);
+  float_t denom = glm_vec3_dot(ab, ab);
+  float_t t = (denom > 1e-10f)
+    ? glm_clamp(glm_vec3_dot(ap, ab) / denom, 0.0f, 1.0f)
+    : 0.0f;
+  glm_vec3_lerp((float_t *)a, (float_t *)b, t, out);
+}
+
+static void closestPointsBetweenSegments(
+  const vec3 a1, const vec3 b1,
+  const vec3 a2, const vec3 b2,
+  vec3 outP1, vec3 outP2
+) {
+  vec3 d1, d2, r;
+  glm_vec3_sub((float_t *)b1, (float_t *)a1, d1);
+  glm_vec3_sub((float_t *)b2, (float_t *)a2, d2);
+  glm_vec3_sub((float_t *)a1, (float_t *)a2, r);
+
+  float_t a  = glm_vec3_dot(d1, d1);
+  float_t e  = glm_vec3_dot(d2, d2);
+  float_t f  = glm_vec3_dot(d2, r);
+  float_t s, t;
+
+  if (a <= 1e-10f && e <= 1e-10f) {
+    glm_vec3_copy((float_t *)a1, outP1);
+    glm_vec3_copy((float_t *)a2, outP2);
+    return;
+  }
+  if (a <= 1e-10f) {
+    t = 0.0f;
+    s = glm_clamp(f / e, 0.0f, 1.0f);
+  } else {
+    float_t c = glm_vec3_dot(d1, r);
+    if (e <= 1e-10f) {
+      s = 0.0f;
+      t = glm_clamp(-c / a, 0.0f, 1.0f);
+    } else {
+      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)
+        : 0.0f;
+      s = glm_clamp((b * t + f) / e, 0.0f, 1.0f);
+      t = glm_clamp((b * s - c) / a, 0.0f, 1.0f);
+    }
+  }
+  glm_vec3_lerp((float_t *)a1, (float_t *)b1, t, outP1);
+  glm_vec3_lerp((float_t *)a2, (float_t *)b2, s, outP2);
+}
+
+/* capsule axis: (cc.x, cc.y ± halfHeight, cc.z), oriented along Y. */
+
+/* outNormal: from sphere toward capsule */
+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
+) {
+  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);
+  return sphereVsSphere(closest, cr, sc, sr, outNormal, outDepth);
+}
+
+/* outNormal: from AABB toward capsule */
+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
+) {
+  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);
+  return sphereVsAabb(closest, cr, ac, ah, outNormal, outDepth);
+}
+
+/* outNormal: from plane toward capsule */
+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
+) {
+  vec3 capA = { cc[0], cc[1] - chh, cc[2] };
+  vec3 capB = { cc[0], cc[1] + chh, cc[2] };
+  float_t da = glm_vec3_dot((float_t *)pn, capA) - pd;
+  float_t db = glm_vec3_dot((float_t *)pn, capB) - pd;
+  float_t minDist = (da < db) ? da : db;
+  *outDepth = cr - minDist;
+  if (*outDepth <= 0.0f) return false;
+  glm_vec3_copy((float_t *)pn, outNormal);
+  return true;
+}
+
+/* outNormal: from capsule-B toward capsule-A */
+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
+) {
+  vec3 a1 = { c1[0], c1[1] - hh1, c1[2] };
+  vec3 b1 = { c1[0], c1[1] + hh1, c1[2] };
+  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);
+  return sphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
+}
+
+/* ===========================================================
+ * Dispatch: tests two shapes and returns push-out for A.
+ * outNormal points from B toward A.
+ * =========================================================== */
+static bool physicsTestDispatch(
+  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 bPos, const physicsshape_t bShape,
+  vec3 outNormal, float_t *outDepth
+) {
+  physicshapetype_t ta = aShape.type;
+  physicshapetype_t tb = bShape.type;
+
+  /* Plane is always the reference surface; treat 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);
+      case PHYSICS_SHAPE_SPHERE:
+        return sphereVsPlane(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);
+      default:
+        return false;
+    }
+  }
+
+  /* If A is a plane, swap roles and negate. */
+  if (ta == PHYSICS_SHAPE_PLANE) {
+    vec3 tmp; float_t d;
+    if (!physicsTestDispatch(bPos, bShape, aPos, aShape, tmp, &d)) return false;
+    glm_vec3_scale(tmp, -1.0f, outNormal);
+    *outDepth = d;
+    return true;
+  }
+
+  switch (ta) {
+    case PHYSICS_SHAPE_CUBE: {
+      const float_t *ac = aPos, *ah = aShape.data.cube.halfExtents;
+      switch (tb) {
+        case PHYSICS_SHAPE_CUBE:
+          return aabbVsAabb(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;
+          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;
+          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
+        }
+        default: return false;
+      }
+    }
+
+    case PHYSICS_SHAPE_SPHERE: {
+      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);
+        case PHYSICS_SHAPE_SPHERE:
+          return sphereVsSphere(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;
+          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
+        }
+        default: return false;
+      }
+    }
+
+    case PHYSICS_SHAPE_CAPSULE: {
+      const float_t cr  = aShape.data.capsule.radius;
+      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);
+        case PHYSICS_SHAPE_SPHERE:
+          return capsuleVsSphere(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);
+        default: return false;
+      }
+    }
+
+    default: return false;
+  }
+}
+
+bool_t physicsTestShapeVsShape(
+  const vec3 aPos, const physicsshape_t aShape,
+  const vec3 bPos, const physicsshape_t bShape,
+  vec3 outNormal, float_t *outDepth
+) {
+  return physicsTestDispatch(aPos, aShape, bPos, bShape, outNormal, outDepth);
+}

src/dusk/physics/physicstest.h

@@ -0,0 +1,32 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ * 
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+#include "physicsshape.h"
+
+/**
+ * Tests for collision between two shapes. Returns true if they overlap, and if
+ * so, outputs the push-out normal and depth.
+ * 
+ * @param aPos Position of shape A.
+ * @param aShape Shape of A.
+ * @param bPos Position of shape B.
+ * @param bShape Shape of B.
+ * @param outNormal Output push-out normal (points from B toward A).
+ * @param outDepth Output penetration depth (positive).
+ * @return true if 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

@@ -8,507 +8,148 @@
 #include "physicsworld.h"
 #include "assert/assert.h"
 #include "util/memory.h"
+#include "entity/entity.h"
+#include "entity/component.h"
+#include "physicstest.h"
 
-/* ---- Threshold for "close enough to ground" (cos ~45°) ---- */
+physicsworld_t PHYSICS_WORLD;
-#define PHYSICS_GROUND_THRESHOLD 0.707f
 
-/* ===========================================================
+void physicsWorldInit() {
- * Low-level collision primitives.
+  memoryZero(&PHYSICS_WORLD, sizeof(physicsworld_t));
- * Convention for all helpers:
- *   outNormal  — points from shape-B toward shape-A
- *                (add outNormal * outDepth to A to separate it from B)
- *   outDepth   — positive penetration depth
- *   return     — true if overlapping
- * =========================================================== */
 
-static bool aabbVsAabb(
+  PHYSICS_WORLD.gravity[0] =  0.0f;
-  const vec3 ac, const vec3 ah,   /* A center, half-extents */
+  PHYSICS_WORLD.gravity[1] = -9.81f;
-  const vec3 bc, const vec3 bh,   /* B center, half-extents */
+  PHYSICS_WORLD.gravity[2] =  0.0f;
-  vec3 outNormal, float_t *outDepth
-) {
-  float_t dx = ac[0] - bc[0];
-  float_t dy = ac[1] - bc[1];
-  float_t dz = ac[2] - bc[2];
-
-  float_t px = (ah[0] + bh[0]) - fabsf(dx);
-  float_t py = (ah[1] + bh[1]) - fabsf(dy);
-  float_t pz = (ah[2] + bh[2]) - fabsf(dz);
-
-  if (px <= 0.0f || py <= 0.0f || pz <= 0.0f) return false;
-
-  outNormal[0] = outNormal[1] = outNormal[2] = 0.0f;
-  if (px < py && px < pz) {
-    *outDepth    = px;
-    outNormal[0] = dx >= 0.0f ? 1.0f : -1.0f;
-  } else if (py < pz) {
-    *outDepth    = py;
-    outNormal[1] = dy >= 0.0f ? 1.0f : -1.0f;
-  } else {
-    *outDepth    = pz;
-    outNormal[2] = dz >= 0.0f ? 1.0f : -1.0f;
-  }
-  return true;
 }
 
-static bool sphereVsSphere(
+void physicsWorldStep(const float_t dt) {
-  const vec3 ac, const float_t ar,
+  assertTrue(dt > 0.0f, "Delta time must be positive");
-  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 */
-  float_t dist2 = glm_vec3_norm2(diff);
-  float_t sumR  = ar + br;
 
-  if (dist2 >= sumR * sumR) return false;
+  entityid_t physEnts[ENTITY_COUNT_MAX];
+  componentid_t physComps[ENTITY_COUNT_MAX];
+  entityid_t physCount = componentGetEntitiesWithComponent(
+    COMPONENT_TYPE_PHYSICS, physEnts, physComps
+  );
 
-  float_t dist = sqrtf(dist2);
+  /* Phase 1: integrate dynamic bodies (gravity + velocity → position). */
-  *outDepth = sumR - dist;
+  for(entityid_t i = 0; i < physCount; i++) {
+    entityid_t id = physEnts[i];
+    componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
+    if(posComp == 0xFF) continue;
 
-  if (dist > 1e-6f) {
+    entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
-    glm_vec3_scale(diff, 1.0f / dist, outNormal);
+    if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
-  } else {
-    outNormal[0] = 0.0f; outNormal[1] = 1.0f; outNormal[2] = 0.0f;
-  }
-  return true;
-}
 
-/* outNormal: from AABB (b) toward sphere (a) */
+    phys->onGround = false;
-static bool sphereVsAabb(
-  const vec3 sc, const float_t sr,
-  const vec3 ac, const vec3 ah,
-  vec3 outNormal, float_t *outDepth
-) {
-  vec3 closest = {
-    glm_clamp(sc[0], ac[0] - ah[0], ac[0] + ah[0]),
-    glm_clamp(sc[1], ac[1] - ah[1], ac[1] + ah[1]),
-    glm_clamp(sc[2], ac[2] - ah[2], ac[2] + ah[2])
-  };
 
-  vec3 diff;
+    phys->velocity[0] += PHYSICS_WORLD.gravity[0] * phys->gravityScale * dt;
-  glm_vec3_sub((float_t *)sc, closest, diff);
+    phys->velocity[1] += PHYSICS_WORLD.gravity[1] * phys->gravityScale * dt;
-  float_t dist2 = glm_vec3_norm2(diff);
+    phys->velocity[2] += PHYSICS_WORLD.gravity[2] * phys->gravityScale * dt;
 
-  bool inside = (dist2 < 1e-10f);
+    vec3 pos;
-  if (!inside && dist2 >= sr * sr) return false;
+    entityPositionGetPosition(id, posComp, pos);
-
+    pos[0] += phys->velocity[0] * dt;
-  if (!inside) {
+    pos[1] += phys->velocity[1] * dt;
-    float_t dist = sqrtf(dist2);
+    pos[2] += phys->velocity[2] * dt;
-    *outDepth = sr - dist;
+    entityPositionSetPosition(id, posComp, pos);
-    glm_vec3_scale(diff, 1.0f / dist, outNormal);
-  } else {
-    /* Sphere center is inside the AABB — find nearest face. */
-    float_t faces[6] = {
-      (ac[0] + ah[0]) - sc[0],
-      sc[0] - (ac[0] - ah[0]),
-      (ac[1] + ah[1]) - sc[1],
-      sc[1] - (ac[1] - ah[1]),
-      (ac[2] + ah[2]) - sc[2],
-      sc[2] - (ac[2] - ah[2])
-    };
-    static const float_t normals[6][3] = {
-      {1,0,0},{-1,0,0},{0,1,0},{0,-1,0},{0,0,1},{0,0,-1}
-    };
-    int mi = 0;
-    for (int k = 1; k < 6; k++) {
-      if (faces[k] < faces[mi]) mi = k;
-    }
-    *outDepth = sr + faces[mi];
-    outNormal[0] = normals[mi][0];
-    outNormal[1] = normals[mi][1];
-    outNormal[2] = normals[mi][2];
-  }
-  return true;
-}
-
-/* outNormal: plane normal (from plane toward A) */
-static bool sphereVsPlane(
-  const vec3 sc, const float_t sr,
-  const vec3 pn, const float_t pd,
-  vec3 outNormal, float_t *outDepth
-) {
-  float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)sc) - pd;
-  *outDepth = sr - signedDist;
-  if (*outDepth <= 0.0f) return false;
-  glm_vec3_copy((float_t *)pn, outNormal);
-  return true;
-}
-
-static bool aabbVsPlane(
-  const vec3 ac, const vec3 ah,
-  const vec3 pn, const float_t pd,
-  vec3 outNormal, float_t *outDepth
-) {
-  float_t proj = fabsf(pn[0] * ah[0])
-               + fabsf(pn[1] * ah[1])
-               + fabsf(pn[2] * ah[2]);
-  float_t signedDist = glm_vec3_dot((float_t *)pn, (float_t *)ac) - pd;
-  *outDepth = proj - signedDist;
-  if (*outDepth <= 0.0f) return false;
-  glm_vec3_copy((float_t *)pn, outNormal);
-  return true;
-}
-
-static void closestPointOnSegment(
-  const vec3 a, const vec3 b, const vec3 p, vec3 out
-) {
-  vec3 ab, ap;
-  glm_vec3_sub((float_t *)b, (float_t *)a, ab);
-  glm_vec3_sub((float_t *)p, (float_t *)a, ap);
-  float_t denom = glm_vec3_dot(ab, ab);
-  float_t t = (denom > 1e-10f)
-    ? glm_clamp(glm_vec3_dot(ap, ab) / denom, 0.0f, 1.0f)
-    : 0.0f;
-  glm_vec3_lerp((float_t *)a, (float_t *)b, t, out);
-}
-
-static void closestPointsBetweenSegments(
-  const vec3 a1, const vec3 b1,
-  const vec3 a2, const vec3 b2,
-  vec3 outP1, vec3 outP2
-) {
-  vec3 d1, d2, r;
-  glm_vec3_sub((float_t *)b1, (float_t *)a1, d1);
-  glm_vec3_sub((float_t *)b2, (float_t *)a2, d2);
-  glm_vec3_sub((float_t *)a1, (float_t *)a2, r);
-
-  float_t a  = glm_vec3_dot(d1, d1);
-  float_t e  = glm_vec3_dot(d2, d2);
-  float_t f  = glm_vec3_dot(d2, r);
-  float_t s, t;
-
-  if (a <= 1e-10f && e <= 1e-10f) {
-    glm_vec3_copy((float_t *)a1, outP1);
-    glm_vec3_copy((float_t *)a2, outP2);
-    return;
-  }
-  if (a <= 1e-10f) {
-    t = 0.0f;
-    s = glm_clamp(f / e, 0.0f, 1.0f);
-  } else {
-    float_t c = glm_vec3_dot(d1, r);
-    if (e <= 1e-10f) {
-      s = 0.0f;
-      t = glm_clamp(-c / a, 0.0f, 1.0f);
-    } else {
-      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)
-        : 0.0f;
-      s = glm_clamp((b * t + f) / e, 0.0f, 1.0f);
-      t = glm_clamp((b * s - c) / a, 0.0f, 1.0f);
-    }
-  }
-  glm_vec3_lerp((float_t *)a1, (float_t *)b1, t, outP1);
-  glm_vec3_lerp((float_t *)a2, (float_t *)b2, s, outP2);
-}
-
-/* capsule axis: (cc.x, cc.y ± halfHeight, cc.z), oriented along Y. */
-
-/* outNormal: from sphere toward capsule */
-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
-) {
-  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);
-  return sphereVsSphere(closest, cr, sc, sr, outNormal, outDepth);
-}
-
-/* outNormal: from AABB toward capsule */
-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
-) {
-  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);
-  return sphereVsAabb(closest, cr, ac, ah, outNormal, outDepth);
-}
-
-/* outNormal: from plane toward capsule */
-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
-) {
-  vec3 capA = { cc[0], cc[1] - chh, cc[2] };
-  vec3 capB = { cc[0], cc[1] + chh, cc[2] };
-  float_t da = glm_vec3_dot((float_t *)pn, capA) - pd;
-  float_t db = glm_vec3_dot((float_t *)pn, capB) - pd;
-  float_t minDist = (da < db) ? da : db;
-  *outDepth = cr - minDist;
-  if (*outDepth <= 0.0f) return false;
-  glm_vec3_copy((float_t *)pn, outNormal);
-  return true;
-}
-
-/* outNormal: from capsule-B toward capsule-A */
-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
-) {
-  vec3 a1 = { c1[0], c1[1] - hh1, c1[2] };
-  vec3 b1 = { c1[0], c1[1] + hh1, c1[2] };
-  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);
-  return sphereVsSphere(p1, r1, p2, r2, outNormal, outDepth);
-}
-
-/* ===========================================================
- * Dispatch: tests two bodies and returns the push-out vector
- * for body A (outNormal points from B toward A).
- * =========================================================== */
-static bool physicsTestOverlapBodies(
-  const physicsbody_t *a,
-  const physicsbody_t *b,
-  vec3 outNormal,
-  float_t *outDepth
-) {
-  physicshapetype_t ta = a->shape.type;
-  physicshapetype_t tb = b->shape.type;
-
-  /* Plane is always the reference surface; treat as B. */
-  if (tb == PHYSICS_SHAPE_PLANE) {
-    const float_t *pn = b->shape.data.plane.normal;
-    const float_t  pd = b->shape.data.plane.distance;
-    switch (ta) {
-      case PHYSICS_SHAPE_CUBE:
-        return aabbVsPlane(a->position, a->shape.data.cube.halfExtents, pn, pd, outNormal, outDepth);
-      case PHYSICS_SHAPE_SPHERE:
-        return sphereVsPlane(a->position, a->shape.data.sphere.radius, pn, pd, outNormal, outDepth);
-      case PHYSICS_SHAPE_CAPSULE:
-        return capsuleVsPlane(a->position, a->shape.data.capsule.radius, a->shape.data.capsule.halfHeight, pn, pd, outNormal, outDepth);
-      default: return false;
-    }
   }
 
-  /* If A is a plane, swap roles and negate. */
+  /* Phase 2: dynamic vs static/kinematic — push dynamic fully. */
-  if (ta == PHYSICS_SHAPE_PLANE) {
+  for(entityid_t i = 0; i < physCount; i++) {
-    vec3 tmp; float_t d;
+    entityid_t id = physEnts[i];
-    if (!physicsTestOverlapBodies(b, a, tmp, &d)) return false;
+    componentid_t posComp = entityGetComponent(id, COMPONENT_TYPE_POSITION);
-    glm_vec3_scale(tmp, -1.0f, outNormal);
+    if(posComp == 0xFF) continue;
-    *outDepth = d;
-    return true;
-  }
 
-  switch (ta) {
+    entityphysics_t *phys = entityPhysicsGet(id, physComps[i]);
-    case PHYSICS_SHAPE_CUBE: {
+    if(phys->type != PHYSICS_BODY_DYNAMIC) continue;
-      const float_t *ac = a->position, *ah = a->shape.data.cube.halfExtents;
-      switch (tb) {
-        case PHYSICS_SHAPE_CUBE:
-          return aabbVsAabb(ac, ah, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
-        case PHYSICS_SHAPE_SPHERE: {
-          /* A=cube B=sphere: want normal from B toward A; sphereVsAabb gives from A(AABB) toward B(sphere) → negate. */
-          vec3 tmp; float_t d;
-          if (!sphereVsAabb(b->position, b->shape.data.sphere.radius, ac, ah, tmp, &d)) return false;
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
-        }
-        case PHYSICS_SHAPE_CAPSULE: {
-          /* A=cube B=capsule: capsuleVsAabb(capsule=B, aabb=A) → normal from A(AABB) toward B(cap) → negate. */
-          vec3 tmp; float_t d;
-          if (!capsuleVsAabb(b->position, b->shape.data.capsule.radius, b->shape.data.capsule.halfHeight, ac, ah, tmp, &d)) return false;
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
-        }
-        default: return false;
-      }
-    }
 
-    case PHYSICS_SHAPE_SPHERE: {
+    vec3 pos;
-      const float_t sr = a->shape.data.sphere.radius;
+    entityPositionGetPosition(id, posComp, pos);
-      switch (tb) {
-        case PHYSICS_SHAPE_CUBE:
-          /* sphereVsAabb(sphere=A, aabb=B): normal from AABB(B) toward sphere(A) ✓ */
-          return sphereVsAabb(a->position, sr, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
-        case PHYSICS_SHAPE_SPHERE:
-          return sphereVsSphere(a->position, sr, b->position, b->shape.data.sphere.radius, outNormal, outDepth);
-        case PHYSICS_SHAPE_CAPSULE: {
-          /* A=sphere B=capsule: capsuleVsSphere(cap=B, sphere=A) → normal from A(sphere) toward B(cap) → negate. */
-          vec3 tmp; float_t d;
-          if (!capsuleVsSphere(b->position, b->shape.data.capsule.radius, b->shape.data.capsule.halfHeight, a->position, sr, tmp, &d)) return false;
-          glm_vec3_scale(tmp, -1.0f, outNormal); *outDepth = d; return true;
-        }
-        default: return false;
-      }
-    }
 
-    case PHYSICS_SHAPE_CAPSULE: {
+    for(entityid_t j = 0; j < physCount; j++) {
-      const float_t cr = a->shape.data.capsule.radius;
+      if(i == j) continue;
-      const float_t chh = a->shape.data.capsule.halfHeight;
+      entityid_t otherId = physEnts[j];
-      switch (tb) {
+      componentid_t otherPosComp = entityGetComponent(otherId, COMPONENT_TYPE_POSITION);
-        case PHYSICS_SHAPE_CUBE:
+      if(otherPosComp == 0xFF) continue;
-          /* capsuleVsAabb(cap=A, aabb=B): normal from AABB(B) toward cap(A) ✓ */
-          return capsuleVsAabb(a->position, cr, chh, b->position, b->shape.data.cube.halfExtents, outNormal, outDepth);
-        case PHYSICS_SHAPE_SPHERE:
-          /* capsuleVsSphere(cap=A, sphere=B): normal from sphere(B) toward cap(A) ✓ */
-          return capsuleVsSphere(a->position, cr, chh, b->position, b->shape.data.sphere.radius, outNormal, outDepth);
-        case PHYSICS_SHAPE_CAPSULE:
-          return capsuleVsCapsule(a->position, cr, chh, b->position, b->shape.data.capsule.radius, b->shape.data.capsule.halfHeight, outNormal, outDepth);
-        default: return false;
-      }
-    }
 
-    default: return false;
+      entityphysics_t *otherPhys = entityPhysicsGet(otherId, physComps[j]);
-  }
+      if(otherPhys->type == PHYSICS_BODY_DYNAMIC) continue;
-}
 
-/* ===========================================================
+      vec3 otherPos;
- * Public API
+      entityPositionGetPosition(otherId, otherPosComp, otherPos);
- * =========================================================== */
-
-void physicsWorldInit(physicsworld_t *world) {
-  assertNotNull(world, "World cannot be NULL");
-  memoryZero(world, sizeof(physicsworld_t));
-  world->gravity[0] =  0.0f;
-  world->gravity[1] = -9.81f;
-  world->gravity[2] =  0.0f;
-}
-
-physicsbody_t *physicsWorldAddBody(physicsworld_t *world) {
-  assertNotNull(world, "World cannot be NULL");
-  for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
-    physicsbody_t *b = &world->bodies[i];
-    if (b->active) continue;
-
-    memoryZero(b, sizeof(physicsbody_t));
-    b->active       = true;
-    b->type         = PHYSICS_BODY_DYNAMIC;
-    b->gravityScale = 1.0f;
-    b->shape.type   = PHYSICS_SHAPE_CUBE;
-    b->shape.data.cube.halfExtents[0] = 0.5f;
-    b->shape.data.cube.halfExtents[1] = 0.5f;
-    b->shape.data.cube.halfExtents[2] = 0.5f;
-    return b;
-  }
-  return NULL;
-}
-
-void physicsWorldRemoveBody(physicsworld_t *world, physicsbody_t *body) {
-  assertNotNull(world, "World cannot be NULL");
-  assertNotNull(body, "Body cannot be NULL");
-  body->active = false;
-}
-
-void physicsWorldStep(physicsworld_t *world, float_t dt) {
-  assertNotNull(world, "World cannot be NULL");
-
-  /* 1. Reset ground flags and integrate dynamic bodies. */
-  for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
-    physicsbody_t *b = &world->bodies[i];
-    if (!b->active || b->type != PHYSICS_BODY_DYNAMIC) continue;
-
-    b->onGround = false;
-
-    b->velocity[0] += world->gravity[0] * b->gravityScale * dt;
-    b->velocity[1] += world->gravity[1] * b->gravityScale * dt;
-    b->velocity[2] += world->gravity[2] * b->gravityScale * dt;
-
-    b->position[0] += b->velocity[0] * dt;
-    b->position[1] += b->velocity[1] * dt;
-    b->position[2] += b->velocity[2] * dt;
-  }
-
-  /* 2. Resolve dynamic vs static / kinematic (push dynamic fully). */
-  for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
-    physicsbody_t *a = &world->bodies[i];
-    if (!a->active || a->type != PHYSICS_BODY_DYNAMIC) continue;
-
-    for (int32_t j = 0; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
-      if (i == j) continue;
-      physicsbody_t *b = &world->bodies[j];
-      if (!b->active || b->type == PHYSICS_BODY_DYNAMIC) continue;
 
       vec3 normal; float_t depth;
-      if (!physicsTestOverlapBodies(a, b, normal, &depth)) continue;
+      if(!physicsTestShapeVsShape(
+        pos, phys->shape, otherPos, otherPhys->shape, normal, &depth
+      )) continue;
 
-      a->position[0] += normal[0] * depth;
+      pos[0] += normal[0] * depth;
-      a->position[1] += normal[1] * depth;
+      pos[1] += normal[1] * depth;
-      a->position[2] += normal[2] * depth;
+      pos[2] += normal[2] * depth;
+      entityPositionSetPosition(id, posComp, pos);
 
-      /* Cancel velocity into the surface. */
+      float_t vn = glm_vec3_dot(phys->velocity, normal);
-      float_t vn = glm_vec3_dot(a->velocity, normal);
+      if(vn < 0.0f) {
-      if (vn < 0.0f) {
+        phys->velocity[0] -= vn * normal[0];
-        a->velocity[0] -= vn * normal[0];
+        phys->velocity[1] -= vn * normal[1];
-        a->velocity[1] -= vn * normal[1];
+        phys->velocity[2] -= vn * normal[2];
-        a->velocity[2] -= vn * normal[2];
       }
 
-      if (normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
+      if(normal[1] > PHYSICS_GROUND_THRESHOLD) phys->onGround = true;
     }
   }
 
-  /* 3. Resolve dynamic vs dynamic (each pair once, elastic equal-mass). */
+  /* Phase 3: dynamic vs dynamic — push each half-way, exchange velocities. */
-  for (int32_t i = 0; i < PHYSICS_WORLD_BODY_COUNT_MAX; i++) {
+  for(entityid_t i = 0; i < physCount; i++) {
-    physicsbody_t *a = &world->bodies[i];
+    entityid_t idA = physEnts[i];
-    if (!a->active || a->type != PHYSICS_BODY_DYNAMIC) continue;
+    componentid_t posCompA = entityGetComponent(idA, COMPONENT_TYPE_POSITION);
+    if(posCompA == 0xFF) continue;
 
-    for (int32_t j = i + 1; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
+    entityphysics_t *physA = entityPhysicsGet(idA, physComps[i]);
-      physicsbody_t *b = &world->bodies[j];
+    if(physA->type != PHYSICS_BODY_DYNAMIC) continue;
-      if (!b->active || b->type != PHYSICS_BODY_DYNAMIC) continue;
+
+    vec3 posA;
+    entityPositionGetPosition(idA, posCompA, posA);
+
+    for(entityid_t j = i + 1; j < physCount; j++) {
+      entityid_t idB = physEnts[j];
+      componentid_t posCompB = entityGetComponent(idB, COMPONENT_TYPE_POSITION);
+      if(posCompB == 0xFF) continue;
+
+      entityphysics_t *physB = entityPhysicsGet(idB, physComps[j]);
+      if(physB->type != PHYSICS_BODY_DYNAMIC) continue;
+
+      vec3 posB;
+      entityPositionGetPosition(idB, posCompB, posB);
 
       vec3 normal; float_t depth;
-      if (!physicsTestOverlapBodies(a, b, normal, &depth)) continue;
+      if(!physicsTestShapeVsShape(
+        posA, physA->shape, posB, physB->shape, normal, &depth
+      )) continue;
 
-      /* Push both half-way. */
+      posA[0] += normal[0] * depth * 0.5f;
-      a->position[0] += normal[0] * depth * 0.5f;
+      posA[1] += normal[1] * depth * 0.5f;
-      a->position[1] += normal[1] * depth * 0.5f;
+      posA[2] += normal[2] * depth * 0.5f;
-      a->position[2] += normal[2] * depth * 0.5f;
+      entityPositionSetPosition(idA, posCompA, posA);
-      b->position[0] -= normal[0] * depth * 0.5f;
-      b->position[1] -= normal[1] * depth * 0.5f;
-      b->position[2] -= normal[2] * depth * 0.5f;
 
-      /* Exchange velocity components along normal (elastic equal-mass). */
+      posB[0] -= normal[0] * depth * 0.5f;
-      float_t v_rel = glm_vec3_dot(a->velocity, normal)
+      posB[1] -= normal[1] * depth * 0.5f;
-                    - glm_vec3_dot(b->velocity, normal);
+      posB[2] -= normal[2] * depth * 0.5f;
-      if (v_rel < 0.0f) {
+      entityPositionSetPosition(idB, posCompB, posB);
-        a->velocity[0] -= v_rel * normal[0];
+
-        a->velocity[1] -= v_rel * normal[1];
+      float_t v_rel = glm_vec3_dot(physA->velocity, normal)
-        a->velocity[2] -= v_rel * normal[2];
+                    - glm_vec3_dot(physB->velocity, normal);
-        b->velocity[0] += v_rel * normal[0];
+      if(v_rel < 0.0f) {
-        b->velocity[1] += v_rel * normal[1];
+        physA->velocity[0] -= v_rel * normal[0];
-        b->velocity[2] += v_rel * normal[2];
+        physA->velocity[1] -= v_rel * normal[1];
+        physA->velocity[2] -= v_rel * normal[2];
+        physB->velocity[0] += v_rel * normal[0];
+        physB->velocity[1] += v_rel * normal[1];
+        physB->velocity[2] += v_rel * normal[2];
       }
 
-      if ( normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
+      if( normal[1] > PHYSICS_GROUND_THRESHOLD) physA->onGround = true;
-      if (-normal[1] > PHYSICS_GROUND_THRESHOLD) b->onGround = true;
+      if(-normal[1] > PHYSICS_GROUND_THRESHOLD) physB->onGround = true;
     }
   }
 }
-
-void physicsWorldMoveBody(
-  physicsworld_t *world,
-  physicsbody_t *body,
-  const vec3 motion
-) {
-  assertNotNull(world, "World cannot be NULL");
-  assertNotNull(body, "Body cannot be NULL");
-
-  body->onGround    = false;
-  body->position[0] += motion[0];
-  body->position[1] += motion[1];
-  body->position[2] += motion[2];
-
-  for (int32_t j = 0; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
-    physicsbody_t *b = &world->bodies[j];
-    if (!b->active || b == body) continue;
-    if (b->type == PHYSICS_BODY_KINEMATIC) continue;
-
-    vec3 normal; float_t depth;
-    if (!physicsTestOverlapBodies(body, b, normal, &depth)) continue;
-
-    body->position[0] += normal[0] * depth;
-    body->position[1] += normal[1] * depth;
-    body->position[2] += normal[2] * depth;
-
-    if (normal[1] > PHYSICS_GROUND_THRESHOLD) body->onGround = true;
-  }
-}

src/dusk/physics/physicsworld.h

@@ -6,39 +6,28 @@
  */
 
 #pragma once
-#include "physicsbody.h"
+#include "physics/physicsshape.h"
+#include "physics/physicsbodytype.h"
 
-#define PHYSICS_WORLD_BODY_COUNT_MAX 64
+#define PHYSICS_GROUND_THRESHOLD 0.707f
 
 typedef struct {
-  physicsbody_t bodies[PHYSICS_WORLD_BODY_COUNT_MAX];
   vec3 gravity;
 } physicsworld_t;
 
-/**
+extern physicsworld_t PHYSICS_WORLD;
- * Initializes the physics world with default gravity (0, -9.81, 0).
- */
-void physicsWorldInit(physicsworld_t *world);
 
 /**
- * Allocates a body slot from the world and returns a pointer to it.
+ * Initializes the physics world.
- * Defaults: DYNAMIC, unit CUBE half-extents, gravityScale=1.
- * Returns NULL if the world is full.
  */
-physicsbody_t *physicsWorldAddBody(physicsworld_t *world);
+void physicsWorldInit(void);
 
 /**
- * Releases a body slot back to the world.
+ * Steps the physics simulation forward.
+ * 
+ * @param dt The time delta in seconds since the last step.
  */
-void physicsWorldRemoveBody(physicsworld_t *world, physicsbody_t *body);
+void physicsWorldStep(const float_t dt);
-
-/**
- * Steps the simulation by dt seconds:
- *   1. Integrates DYNAMIC bodies (gravity + velocity).
- *   2. Resolves DYNAMIC vs STATIC/KINEMATIC collisions.
- *   3. Resolves DYNAMIC vs DYNAMIC collisions (each pair once).
- */
-void physicsWorldStep(physicsworld_t *world, float_t dt);
 
 /**
  * Moves a KINEMATIC body by motion and immediately resolves overlaps against
@@ -48,8 +37,8 @@ void physicsWorldStep(physicsworld_t *world, float_t dt);
  * @param body   The kinematic body to move (must be PHYSICS_BODY_KINEMATIC).
  * @param motion World-space displacement for this frame.
  */
-void physicsWorldMoveBody(
+// void physicsWorldMoveBody(
-  physicsworld_t *world,
+//   physicsworld_t *world,
-  physicsbody_t *body,
+//   physicsbody_t *body,
-  const vec3 motion
+//   const vec3 motion
-);
+// );