1 files changed, 144 insertions, 0 deletions
diff --git a/src/3d/physics.cc b/src/3d/physics.cc
new file mode 100644
index 0000000..351dd06
--- /dev/null
+++ b/src/3d/physics.cc
@@ -0,0 +1,144 @@
+// This file is part of the 64k demo project.
+// It implements a lightweight SDF-based physics engine.
+
+#include "3d/physics.h"
+#include "3d/bvh.h"
+#include "3d/sdf_cpu.h"
+#include <algorithm>
+
+namespace {
+// Helper to get world AABB (copied from bvh.cc or shared)
+AABB get_world_aabb(const Object3D& obj) {
+  BoundingVolume local = obj.get_local_bounds();
+  mat4 model = obj.get_model_matrix();
+
+  vec3 corners[8] = {
+      {local.min.x, local.min.y, local.min.z},
+      {local.max.x, local.min.y, local.min.z},
+      {local.min.x, local.max.y, local.min.z},
+      {local.max.x, local.max.y, local.min.z},
+      {local.min.x, local.min.y, local.max.z},
+      {local.max.x, local.min.y, local.max.z},
+      {local.min.x, local.max.y, local.max.z},
+      {local.max.x, local.max.y, local.max.z},
+  };
+
+  AABB world;
+  for (int i = 0; i < 8; ++i) {
+    vec4 p = model * vec4(corners[i].x, corners[i].y, corners[i].z, 1.0f);
+    world.expand(p.xyz());
+  }
+  return world;
+}
+} // namespace
+
+float PhysicsSystem::sample_sdf(const Object3D& obj, vec3 world_p) {
+  mat4 inv_model = obj.get_model_matrix().inverse();
+  vec4 local_p4 = inv_model * vec4(world_p.x, world_p.y, world_p.z, 1.0f);
+  vec3 q = local_p4.xyz();
+
+  float d = 1000.0f;
+  if (obj.type == ObjectType::SPHERE) {
+    d = q.len() - 1.0f;
+  } else if (obj.type == ObjectType::BOX || obj.type == ObjectType::CUBE) {
+    d = sdf::sdBox(q, vec3(1.0f, 1.0f, 1.0f));
+  } else if (obj.type == ObjectType::TORUS) {
+    d = sdf::sdTorus(q, vec2(1.0f, 0.4f));
+  } else if (obj.type == ObjectType::PLANE) {
+    d = sdf::sdPlane(q, vec3(0.0f, 1.0f, 0.0f), 0.0f);
+  }
+
+  // Extract scale from model matrix (assuming orthogonal with uniform or
+  // non-uniform scale)
+  mat4 model = obj.get_model_matrix();
+  float sx = vec3(model.m[0], model.m[1], model.m[2]).len();
+  float sy = vec3(model.m[4], model.m[5], model.m[6]).len();
+  float sz = vec3(model.m[8], model.m[9], model.m[10]).len();
+  float s = std::min(sx, std::min(sy, sz));
+
+  return d * s;
+}
+
+void PhysicsSystem::resolve_collision(Object3D& a, Object3D& b) {
+  if (a.is_static && b.is_static)
+    return;
+
+  // Probe points for 'a' (center and corners)
+  BoundingVolume local = a.get_local_bounds();
+  mat4 model_a = a.get_model_matrix();
+  vec3 probes[9] = {
+      {0, 0, 0}, // Center
+      {local.min.x, local.min.y, local.min.z},
+      {local.max.x, local.min.y, local.min.z},
+      {local.min.x, local.max.y, local.min.z},
+      {local.max.x, local.max.y, local.min.z},
+      {local.min.x, local.min.y, local.max.z},
+      {local.max.x, local.min.y, local.max.z},
+      {local.min.x, local.max.y, local.max.z},
+      {local.max.x, local.max.y, local.max.z},
+  };
+
+  for (int i = 0; i < 9; ++i) {
+    vec3 world_probe =
+        (model_a * vec4(probes[i].x, probes[i].y, probes[i].z, 1.0f)).xyz();
+    float d = sample_sdf(b, world_probe);
+
+    if (d < 0.0f) {
+      // Collision detected!
+      float penetration = -d;
+
+      // Calculate normal via gradient of b's SDF
+      auto b_sdf = [this, &b](vec3 p) { return sample_sdf(b, p); };
+      vec3 normal = sdf::calc_normal(world_probe, b_sdf);
+
+      // Resolution
+      if (!a.is_static) {
+        // Positional correction
+        a.position += normal * penetration;
+
+        // Velocity response
+        float v_dot_n = vec3::dot(a.velocity, normal);
+        if (v_dot_n < 0) {
+          a.velocity -= normal * (1.0f + a.restitution) * v_dot_n;
+        }
+      }
+    }
+  }
+}
+
+void PhysicsSystem::update(Scene& scene, float dt) {
+  if (dt <= 0)
+    return;
+
+  // 1. Integration
+  for (auto& obj : scene.objects) {
+    if (obj.is_static)
+      continue;
+    obj.velocity += gravity * dt;
+    obj.position += obj.velocity * dt;
+  }
+
+  // 2. Broad Phase
+  BVH bvh;
+  BVHBuilder::build(bvh, scene.objects);
+
+  // 3. Narrow Phase & Resolution
+  // We do multiple iterations for better stability? Just 1 for now.
+  for (int iter = 0; iter < 2; ++iter) {
+    for (int i = 0; i < (int)scene.objects.size(); ++i) {
+      Object3D& a = scene.objects[i];
+      if (a.is_static)
+        continue;
+
+      AABB query_box = get_world_aabb(a);
+      std::vector<int> candidates;
+      bvh.query(query_box, candidates);
+
+      for (int cand_idx : candidates) {
+        if (cand_idx == i)
+          continue;
+        resolve_collision(a, scene.objects[cand_idx]);
+      }
+    }
+  }
+}