Phyiscs engine first pass

src/dusk/CMakeLists.txt

@@ -69,6 +69,7 @@ add_subdirectory(error)
 add_subdirectory(event)
 add_subdirectory(input)
 add_subdirectory(locale)
+add_subdirectory(physics)
 add_subdirectory(scene)
 add_subdirectory(script)
 add_subdirectory(time)

src/dusk/engine/engine.c

@@ -18,19 +18,16 @@
 #include "assert/assert.h"
 #include "entity/entitymanager.h"
 #include "game/game.h"
-
+#include "physics/physicsmanager.h"
-#include "display/mesh/quad.h"
+#include "display/mesh/cube.h"
-#include "display/mesh/capsule.h"
+#include "display/mesh/plane.h"
-#include "asset/loader/display/assetmeshloader.h"
 
 engine_t ENGINE;
-entityid_t ent1;
-componentid_t ent1Pos;
-componentid_t ent1Mesh;
-componentid_t ent1Mat;
 
-mesh_t loadedMesh;
+/* Physics demo entities */
-meshvertex_t *loadedVertices;
+static entityid_t    phFloorEnt, phBoxEnt;
+static componentid_t phFloorPos, phFloorMesh, phFloorMat;
+static componentid_t phBoxPos,   phBoxMesh,   phBoxMat,   phBoxPhys;
 
 errorret_t engineInit(const int32_t argc, const char_t **argv) {
   memoryZero(&ENGINE, sizeof(engine_t));
@@ -48,46 +45,69 @@ errorret_t engineInit(const int32_t argc, const char_t **argv) {
   errorChain(uiInit());
   errorChain(sceneInit());
   entityManagerInit();
+  physicsManagerInit();
   errorChain(gameInit());
 
-  // FOF
+  /* ---- Camera ---- */
   entityid_t cam = entityManagerAdd();
   componentid_t camPos = entityAddComponent(cam, COMPONENT_TYPE_POSITION);
-  float_t distance = 1.5f;
+  float_t distance = 6.0f;
-  float_t up = distance / 2.0f;
   entityPositionLookAt(
-    cam,
+    cam, camPos,
-    camPos,
-    (vec3){ 0.0f, up, 0.0f },
     (vec3){ 0.0f, 1.0f, 0.0f },
-    (vec3){ distance, distance + up, distance }
+    (vec3){ 0.0f, 1.0f, 0.0f },
+    (vec3){ distance, distance, distance }
   );
   componentid_t camCam = entityAddComponent(cam, COMPONENT_TYPE_CAMERA);
-  entityCameraSetZFar(cam, camCam, distance * 5.0f);
+  entityCameraSetZFar(cam, camCam, distance * 6.0f);
 
-  ent1 = entityManagerAdd();
+  /* ---- Static floor (visual + physics) ---- */
-  ent1Pos = entityAddComponent(ent1, COMPONENT_TYPE_POSITION);
+  phFloorEnt  = entityManagerAdd();
-  ent1Mesh = entityAddComponent(ent1, COMPONENT_TYPE_MESH);
+  phFloorPos  = entityAddComponent(phFloorEnt, COMPONENT_TYPE_POSITION);
-  ent1Mat = entityAddComponent(ent1, COMPONENT_TYPE_MATERIAL);
+  phFloorMesh = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MESH);
+  phFloorMat  = entityAddComponent(phFloorEnt, COMPONENT_TYPE_MATERIAL);
 
-  errorChain(assetMeshLoad(
+  /* Scale the unit XZ plane to 10×10, centred on origin */
-    "test/Mei.stl",
+  entityPositionSetPosition(phFloorEnt, phFloorPos, (vec3){ -5.0f, 0.0f, -5.0f });
-    &loadedMesh,
+  entityPositionSetScale(phFloorEnt, phFloorPos, (vec3){ 10.0f, 1.0f, 10.0f });
-    &loadedVertices,
+  entityMeshSetMesh(phFloorEnt, phFloorMesh, &PLANE_MESH_SIMPLE);
-    MESH_INPUT_AXIS_Y_UP
+  entityMaterialGetShaderMaterial(phFloorEnt, phFloorMat)->unlit.color = COLOR_GREEN;
-  ));
-  entityMeshSetMesh(ent1, ent1Mesh, &loadedMesh);
 
-  vec3 min, max;
+  /* No PHYSICS component for the floor — we add the body manually so it never
-  meshGetBounds(&loadedMesh, min, max);
+   * gets disposed by the entity system before we're done with it. */
-  printf("Mesh bounds: min(%f, %f, %f), max(%f, %f, %f)\n", min[0], min[1], min[2], max[0], max[1], max[2]);
+  physicsbody_t *floorBody = physicsWorldAddBody(&PHYSICS_WORLD);
+  floorBody->type                   = PHYSICS_BODY_STATIC;
+  floorBody->shape.type             = PHYSICS_SHAPE_PLANE;
+  floorBody->shape.data.plane.normal[0]   = 0.0f;
+  floorBody->shape.data.plane.normal[1]   = 1.0f;
+  floorBody->shape.data.plane.normal[2]   = 0.0f;
+  floorBody->shape.data.plane.distance    = 0.0f;
 
-  shadermaterial_t *mat = entityMaterialGetShaderMaterial(ent1, ent1Mat);
+  /* ---- Dynamic box ---- */
-  mat->unlit.color = COLOR_WHITE;
+  phBoxEnt  = entityManagerAdd();
+  phBoxPos  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_POSITION);
+  phBoxMesh = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MESH);
+  phBoxMat  = entityAddComponent(phBoxEnt, COMPONENT_TYPE_MATERIAL);
+  phBoxPhys = entityAddComponent(phBoxEnt, COMPONENT_TYPE_PHYSICS);
 
-  // EOF
+  entityMeshSetMesh(phBoxEnt, phBoxMesh, &CUBE_MESH_SIMPLE);
+  entityMaterialGetShaderMaterial(phBoxEnt, phBoxMat)->unlit.color = COLOR_RED;
 
-  // Run the init script.
+  /* Start the box 4 units above the floor; default shape is a unit AABB */
+  physicsbody_t *boxBody = entityPhysicsGetBody(phBoxEnt, phBoxPhys);
+  boxBody->position[0] = 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;
   errorChain(scriptContextInit(&ctx));
   errorChain(scriptContextExecFile(&ctx, "init.lua"));
@@ -100,17 +120,21 @@ errorret_t engineUpdate(void) {
   timeUpdate();
   inputUpdate();
 
-  vec3 rotation;
-  entityPositionGetRotation(ent1, ent1Pos, rotation);
-  #if DUSK_TIME_DYNAMIC
-    rotation[1] += 1.0f * TIME.dynamicDelta;
-  #else
-    rotation[1] += 1.0f * TIME.delta;
-  #endif
-  entityPositionSetRotation(ent1, ent1Pos, rotation);
-
   uiUpdate();
   errorChain(sceneUpdate());
+
+  /* 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());
 
@@ -124,9 +148,6 @@ void engineExit(void) {
 }
 
 errorret_t engineDispose(void) {
-  errorChain(meshDispose(&loadedMesh));
-  memoryFree(loadedVertices);
-
   sceneDispose();
   errorChain(gameDispose());
   entityManagerDispose();

src/dusk/entity/component/CMakeLists.txt

@@ -4,3 +4,4 @@
 # https://opensource.org/licenses/MIT
 
 add_subdirectory(display)
+add_subdirectory(physics)

src/dusk/entity/component/physics/CMakeLists.txt

@@ -0,0 +1,10 @@
+# Copyright (c) 2026 Dominic Masters
+# 
+# This software is released under the MIT License.
+# https://opensource.org/licenses/MIT
+
+# Sources
+target_sources(${DUSK_LIBRARY_TARGET_NAME}
+  PUBLIC
+    entityphysics.c
+)

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

@@ -0,0 +1,74 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#include "entityphysics.h"
+#include "entity/entitymanager.h"
+#include "entity/component/display/entityposition.h"
+#include "physics/physicsmanager.h"
+#include "assert/assert.h"
+
+void entityPhysicsInit(
+  const entityid_t entityId,
+  const componentid_t componentId
+) {
+  entityphysics_t *phys = componentGetData(
+    entityId, componentId, COMPONENT_TYPE_PHYSICS
+  );
+
+  phys->body = physicsWorldAddBody(&PHYSICS_WORLD);
+  assertNotNull(phys->body, "Physics world body limit reached");
+}
+
+void entityPhysicsSyncPosition(
+  const entityid_t entityId,
+  const componentid_t componentId
+) {
+  entityphysics_t *phys = componentGetData(
+    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(
+  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
+) {
+  entityphysics_t *phys = componentGetData(
+    entityId, componentId, COMPONENT_TYPE_PHYSICS
+  );
+  assertNotNull(phys->body, "Physics body is NULL");
+  physicsWorldMoveBody(&PHYSICS_WORLD, phys->body, motion);
+}
+
+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

@@ -0,0 +1,60 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+#include "entity/entitybase.h"
+#include "physics/physicsbody.h"
+
+typedef struct {
+  /** Pointer into PHYSICS_WORLD.bodies[]. Allocated on component init. */
+  physicsbody_t *body;
+} entityphysics_t;
+
+/**
+ * Initializes the physics component: allocates a body in PHYSICS_WORLD.
+ * Asserts if the world body limit is reached.
+ */
+void entityPhysicsInit(
+  const entityid_t entityId,
+  const componentid_t componentId
+);
+
+/**
+ * Copies the physics body's position back into the entity's POSITION
+ * component (if present). Call this after physicsManagerStep each frame.
+ */
+void entityPhysicsSyncPosition(
+  const entityid_t entityId,
+  const componentid_t componentId
+);
+
+/**
+ * Returns the raw physics body for direct manipulation.
+ */
+physicsbody_t *entityPhysicsGetBody(
+  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
+);
+
+/**
+ * Releases the body slot back to PHYSICS_WORLD. Called automatically when
+ * the component is disposed via the component system.
+ */
+void entityPhysicsDispose(
+  const entityid_t entityId,
+  const componentid_t componentId
+);

src/dusk/entity/componentlist.h

@@ -9,8 +9,10 @@
 #include "entity/component/display/entitycamera.h"
 #include "entity/component/display/entitymesh.h"
 #include "entity/component/display/entitymaterial.h"
+#include "entity/component/physics/entityphysics.h"
 
 X(POSITION, entityposition_t, position, entityPositionInit, NULL)
 X(CAMERA, entitycamera_t, camera, entityCameraInit, NULL)
 X(MESH, entitymesh_t, mesh, entityMeshInit, NULL)
 X(MATERIAL, entitymaterial_t, material, entityMaterialInit, NULL)
+X(PHYSICS, entityphysics_t, physics, entityPhysicsInit, entityPhysicsDispose)

src/dusk/physics/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Copyright (c) 2026 Dominic Masters
+# 
+# This software is released under the MIT License.
+# https://opensource.org/licenses/MIT
+
+# Sources
+target_sources(${DUSK_LIBRARY_TARGET_NAME}
+  PUBLIC
+    physicsmanager.c
+    physicsbody.c
+    physicsworld.c
+)

src/dusk/physics/physicsbody.c

@@ -0,0 +1,33 @@
+/**
+ * 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

@@ -0,0 +1,57 @@
+/**
+ * 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

@@ -0,0 +1,18 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+
+typedef enum {
+  /** Never moves. Acts as an immovable collision surface. */
+  PHYSICS_BODY_STATIC,
+  /** 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. */
+  PHYSICS_BODY_KINEMATIC
+} physicsbodytype_t;

src/dusk/physics/physicsmanager.c

@@ -0,0 +1,23 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#include "physicsmanager.h"
+#include "time/time.h"
+
+physicsworld_t PHYSICS_WORLD;
+
+void physicsManagerInit(void) {
+  physicsWorldInit(&PHYSICS_WORLD);
+}
+
+void physicsManagerUpdate() {
+  #if DUSK_TIME_DYNAMIC
+    if(TIME.dynamicUpdate) return; // Don't update on dynamic updates.
+  #endif
+
+  physicsWorldStep(&PHYSICS_WORLD, TIME.delta);
+}

src/dusk/physics/physicsmanager.h

@@ -0,0 +1,21 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+#include "physicsworld.h"
+
+extern physicsworld_t PHYSICS_WORLD;
+
+/**
+ * Initializes the global physics world with default gravity (0, -9.81, 0).
+ */
+void physicsManagerInit(void);
+
+/**
+ * Advances the physics simulation.
+ */
+void physicsManagerUpdate();

src/dusk/physics/physicsshape.h

@@ -0,0 +1,46 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+#include "dusk.h"
+
+typedef enum {
+  PHYSICS_SHAPE_CUBE,
+  PHYSICS_SHAPE_SPHERE,
+  PHYSICS_SHAPE_CAPSULE,
+  PHYSICS_SHAPE_PLANE
+} physicshapetype_t;
+
+typedef struct {
+  vec3 halfExtents;
+} physicsshapecube_t;
+
+typedef struct {
+  float_t radius;
+} physicsshapesphere_t;
+
+typedef struct {
+  float_t radius;
+  float_t halfHeight;
+} physicsshapecapsule_t;
+
+typedef struct {
+  vec3 normal;
+  float_t distance;
+} physicsshapeplane_t;
+
+typedef union {
+  physicsshapecube_t cube;
+  physicsshapesphere_t sphere;
+  physicsshapecapsule_t capsule;
+  physicsshapeplane_t plane;
+} physicsshapedata_t;
+
+typedef struct {
+  physicshapetype_t type;
+  physicsshapedata_t data;
+} physicsshape_t;

src/dusk/physics/physicsworld.c

@@ -0,0 +1,514 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#include "physicsworld.h"
+#include "assert/assert.h"
+#include "util/memory.h"
+
+/* ---- Threshold for "close enough to ground" (cos ~45°) ---- */
+#define PHYSICS_GROUND_THRESHOLD 0.707f
+
+/* ===========================================================
+ * 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 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. */
+  if (ta == PHYSICS_SHAPE_PLANE) {
+    vec3 tmp; float_t d;
+    if (!physicsTestOverlapBodies(b, a, 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 = 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: {
+      const float_t sr = a->shape.data.sphere.radius;
+      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: {
+      const float_t cr = a->shape.data.capsule.radius;
+      const float_t chh = a->shape.data.capsule.halfHeight;
+      switch (tb) {
+        case PHYSICS_SHAPE_CUBE:
+          /* 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;
+  }
+}
+
+/* ===========================================================
+ * Public API
+ * =========================================================== */
+
+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;
+
+      a->position[0] += normal[0] * depth;
+      a->position[1] += normal[1] * depth;
+      a->position[2] += normal[2] * depth;
+
+      /* Cancel velocity into the surface. */
+      float_t vn = glm_vec3_dot(a->velocity, normal);
+      if (vn < 0.0f) {
+        a->velocity[0] -= vn * normal[0];
+        a->velocity[1] -= vn * normal[1];
+        a->velocity[2] -= vn * normal[2];
+      }
+
+      if (normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
+    }
+  }
+
+  /* 3. Resolve dynamic vs dynamic (each pair once, elastic equal-mass). */
+  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 = i + 1; j < PHYSICS_WORLD_BODY_COUNT_MAX; j++) {
+      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;
+
+      /* Push both half-way. */
+      a->position[0] += normal[0] * depth * 0.5f;
+      a->position[1] += normal[1] * depth * 0.5f;
+      a->position[2] += normal[2] * depth * 0.5f;
+      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). */
+      float_t v_rel = glm_vec3_dot(a->velocity, normal)
+                    - glm_vec3_dot(b->velocity, normal);
+      if (v_rel < 0.0f) {
+        a->velocity[0] -= v_rel * normal[0];
+        a->velocity[1] -= v_rel * normal[1];
+        a->velocity[2] -= v_rel * normal[2];
+        b->velocity[0] += v_rel * normal[0];
+        b->velocity[1] += v_rel * normal[1];
+        b->velocity[2] += v_rel * normal[2];
+      }
+
+      if ( normal[1] > PHYSICS_GROUND_THRESHOLD) a->onGround = true;
+      if (-normal[1] > PHYSICS_GROUND_THRESHOLD) b->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

@@ -0,0 +1,55 @@
+/**
+ * Copyright (c) 2026 Dominic Masters
+ *
+ * This software is released under the MIT License.
+ * https://opensource.org/licenses/MIT
+ */
+
+#pragma once
+#include "physicsbody.h"
+
+#define PHYSICS_WORLD_BODY_COUNT_MAX 64
+
+typedef struct {
+  physicsbody_t bodies[PHYSICS_WORLD_BODY_COUNT_MAX];
+  vec3 gravity;
+} physicsworld_t;
+
+/**
+ * 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.
+ * Defaults: DYNAMIC, unit CUBE half-extents, gravityScale=1.
+ * Returns NULL if the world is full.
+ */
+physicsbody_t *physicsWorldAddBody(physicsworld_t *world);
+
+/**
+ * Releases a body slot back to the world.
+ */
+void physicsWorldRemoveBody(physicsworld_t *world, physicsbody_t *body);
+
+/**
+ * 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
+ * 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
+);

src/duskgl/display/shader/shadergl.c

@@ -229,6 +229,7 @@ errorret_t shaderSetTextureGL(
     if(texture == NULL) {
       glDisable(GL_TEXTURE_2D);
       errorChain(errorGLCheck());
+
       errorOk();
     }
 
@@ -312,6 +313,14 @@ errorret_t shaderSetColorGL(
     // glTexEnvi(GL_TEXTURE_ENV, GL_OPERAND1_RGB, GL_SRC_COLOR);
     // errorChain(errorGLCheck());
 
+    glColor4f(
+      (float_t)color.r / 255.0f,
+      (float_t)color.g / 255.0f,
+      (float_t)color.b / 255.0f,
+      (float_t)color.a / 255.0f
+    );
+    errorChain(errorGLCheck());
+
   #else
     GLint location;
     errorChain(shaderParamGetLocationGL(shader, name, &location));