feat(physics): add narrow phase collision detection (sphere-sphere, sphere-box, box-box)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

crates/voltex_physics/src/lib.rs

@@ -1,5 +1,6 @@
 pub mod collider;
 pub mod contact;
+pub mod narrow;
 
 pub use collider::Collider;
 pub use contact::ContactPoint;

crates/voltex_physics/src/narrow.rs

@@ -0,0 +1,236 @@
+use voltex_math::Vec3;
+
+/// Returns (normal A→B, depth, point_on_a, point_on_b) or None if no collision.
+pub fn sphere_vs_sphere(
+    pos_a: Vec3, radius_a: f32,
+    pos_b: Vec3, radius_b: f32,
+) -> Option<(Vec3, f32, Vec3, Vec3)> {
+    let diff = pos_b - pos_a;
+    let dist_sq = diff.length_squared();
+    let sum_r = radius_a + radius_b;
+
+    if dist_sq > sum_r * sum_r {
+        return None;
+    }
+
+    let dist = dist_sq.sqrt();
+    let normal = if dist > 1e-8 {
+        diff * (1.0 / dist)
+    } else {
+        Vec3::Y
+    };
+
+    let depth = sum_r - dist;
+    let point_on_a = pos_a + normal * radius_a;
+    let point_on_b = pos_b - normal * radius_b;
+
+    Some((normal, depth, point_on_a, point_on_b))
+}
+
+pub fn sphere_vs_box(
+    sphere_pos: Vec3, radius: f32,
+    box_pos: Vec3, half_extents: Vec3,
+) -> Option<(Vec3, f32, Vec3, Vec3)> {
+    let bmin = box_pos - half_extents;
+    let bmax = box_pos + half_extents;
+
+    let closest = Vec3::new(
+        sphere_pos.x.clamp(bmin.x, bmax.x),
+        sphere_pos.y.clamp(bmin.y, bmax.y),
+        sphere_pos.z.clamp(bmin.z, bmax.z),
+    );
+
+    let diff = sphere_pos - closest;
+    let dist_sq = diff.length_squared();
+
+    // Sphere center outside box
+    if dist_sq > 1e-8 {
+        let dist = dist_sq.sqrt();
+        if dist > radius {
+            return None;
+        }
+        let normal = diff * (-1.0 / dist); // sphere→box direction
+        let depth = radius - dist;
+        let point_on_a = sphere_pos + normal * radius; // sphere surface toward box
+        let point_on_b = closest;
+        return Some((normal, depth, point_on_a, point_on_b));
+    }
+
+    // Sphere center inside box — find nearest face
+    let dx_min = sphere_pos.x - bmin.x;
+    let dx_max = bmax.x - sphere_pos.x;
+    let dy_min = sphere_pos.y - bmin.y;
+    let dy_max = bmax.y - sphere_pos.y;
+    let dz_min = sphere_pos.z - bmin.z;
+    let dz_max = bmax.z - sphere_pos.z;
+
+    let mut min_dist = dx_min;
+    let mut normal = Vec3::new(-1.0, 0.0, 0.0);
+    let mut closest_face = Vec3::new(bmin.x, sphere_pos.y, sphere_pos.z);
+
+    if dx_max < min_dist {
+        min_dist = dx_max;
+        normal = Vec3::new(1.0, 0.0, 0.0);
+        closest_face = Vec3::new(bmax.x, sphere_pos.y, sphere_pos.z);
+    }
+    if dy_min < min_dist {
+        min_dist = dy_min;
+        normal = Vec3::new(0.0, -1.0, 0.0);
+        closest_face = Vec3::new(sphere_pos.x, bmin.y, sphere_pos.z);
+    }
+    if dy_max < min_dist {
+        min_dist = dy_max;
+        normal = Vec3::new(0.0, 1.0, 0.0);
+        closest_face = Vec3::new(sphere_pos.x, bmax.y, sphere_pos.z);
+    }
+    if dz_min < min_dist {
+        min_dist = dz_min;
+        normal = Vec3::new(0.0, 0.0, -1.0);
+        closest_face = Vec3::new(sphere_pos.x, sphere_pos.y, bmin.z);
+    }
+    if dz_max < min_dist {
+        min_dist = dz_max;
+        normal = Vec3::new(0.0, 0.0, 1.0);
+        closest_face = Vec3::new(sphere_pos.x, sphere_pos.y, bmax.z);
+    }
+
+    let depth = min_dist + radius;
+    let point_on_a = sphere_pos + normal * radius;
+    let point_on_b = closest_face;
+
+    Some((normal, depth, point_on_a, point_on_b))
+}
+
+pub fn box_vs_box(
+    pos_a: Vec3, half_a: Vec3,
+    pos_b: Vec3, half_b: Vec3,
+) -> Option<(Vec3, f32, Vec3, Vec3)> {
+    let diff = pos_b - pos_a;
+
+    let overlap_x = (half_a.x + half_b.x) - diff.x.abs();
+    if overlap_x < 0.0 { return None; }
+
+    let overlap_y = (half_a.y + half_b.y) - diff.y.abs();
+    if overlap_y < 0.0 { return None; }
+
+    let overlap_z = (half_a.z + half_b.z) - diff.z.abs();
+    if overlap_z < 0.0 { return None; }
+
+    let (normal, depth) = if overlap_x <= overlap_y && overlap_x <= overlap_z {
+        let sign = if diff.x >= 0.0 { 1.0 } else { -1.0 };
+        (Vec3::new(sign, 0.0, 0.0), overlap_x)
+    } else if overlap_y <= overlap_z {
+        let sign = if diff.y >= 0.0 { 1.0 } else { -1.0 };
+        (Vec3::new(0.0, sign, 0.0), overlap_y)
+    } else {
+        let sign = if diff.z >= 0.0 { 1.0 } else { -1.0 };
+        (Vec3::new(0.0, 0.0, sign), overlap_z)
+    };
+
+    let point_on_a = pos_a + Vec3::new(
+        normal.x * half_a.x,
+        normal.y * half_a.y,
+        normal.z * half_a.z,
+    );
+    let point_on_b = pos_b - Vec3::new(
+        normal.x * half_b.x,
+        normal.y * half_b.y,
+        normal.z * half_b.z,
+    );
+
+    Some((normal, depth, point_on_a, point_on_b))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn approx(a: f32, b: f32) -> bool { (a - b).abs() < 1e-5 }
+    fn approx_vec(a: Vec3, b: Vec3) -> bool { approx(a.x, b.x) && approx(a.y, b.y) && approx(a.z, b.z) }
+
+    // sphere_vs_sphere tests
+    #[test]
+    fn test_sphere_sphere_separated() {
+        let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(5.0, 0.0, 0.0), 1.0);
+        assert!(r.is_none());
+    }
+
+    #[test]
+    fn test_sphere_sphere_overlapping() {
+        let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(1.5, 0.0, 0.0), 1.0);
+        let (normal, depth, pa, pb) = r.unwrap();
+        assert!(approx_vec(normal, Vec3::X));
+        assert!(approx(depth, 0.5));
+        assert!(approx_vec(pa, Vec3::new(1.0, 0.0, 0.0)));
+        assert!(approx_vec(pb, Vec3::new(0.5, 0.0, 0.0)));
+    }
+
+    #[test]
+    fn test_sphere_sphere_touching() {
+        let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::new(2.0, 0.0, 0.0), 1.0);
+        let (normal, depth, _pa, _pb) = r.unwrap();
+        assert!(approx_vec(normal, Vec3::X));
+        assert!(approx(depth, 0.0));
+    }
+
+    #[test]
+    fn test_sphere_sphere_coincident() {
+        let r = sphere_vs_sphere(Vec3::ZERO, 1.0, Vec3::ZERO, 1.0);
+        let (_normal, depth, _pa, _pb) = r.unwrap();
+        assert!(approx(depth, 2.0));
+    }
+
+    // sphere_vs_box tests
+    #[test]
+    fn test_sphere_box_separated() {
+        let r = sphere_vs_box(Vec3::new(5.0, 0.0, 0.0), 1.0, Vec3::ZERO, Vec3::ONE);
+        assert!(r.is_none());
+    }
+
+    #[test]
+    fn test_sphere_box_face_overlap() {
+        let r = sphere_vs_box(Vec3::new(1.5, 0.0, 0.0), 1.0, Vec3::ZERO, Vec3::ONE);
+        let (normal, depth, _pa, pb) = r.unwrap();
+        assert!(approx(normal.x, -1.0));
+        assert!(approx(depth, 0.5));
+        assert!(approx(pb.x, 1.0));
+    }
+
+    #[test]
+    fn test_sphere_box_center_inside() {
+        let r = sphere_vs_box(Vec3::ZERO, 0.5, Vec3::ZERO, Vec3::ONE);
+        assert!(r.is_some());
+        let (_normal, depth, _pa, _pb) = r.unwrap();
+        assert!(depth > 0.0);
+    }
+
+    // box_vs_box tests
+    #[test]
+    fn test_box_box_separated() {
+        let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(5.0, 0.0, 0.0), Vec3::ONE);
+        assert!(r.is_none());
+    }
+
+    #[test]
+    fn test_box_box_overlapping() {
+        let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(1.5, 0.0, 0.0), Vec3::ONE);
+        let (normal, depth, _pa, _pb) = r.unwrap();
+        assert!(approx_vec(normal, Vec3::X));
+        assert!(approx(depth, 0.5));
+    }
+
+    #[test]
+    fn test_box_box_touching() {
+        let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(2.0, 0.0, 0.0), Vec3::ONE);
+        let (_normal, depth, _pa, _pb) = r.unwrap();
+        assert!(approx(depth, 0.0));
+    }
+
+    #[test]
+    fn test_box_box_y_axis() {
+        let r = box_vs_box(Vec3::ZERO, Vec3::ONE, Vec3::new(0.0, 1.5, 0.0), Vec3::ONE);
+        let (normal, depth, _pa, _pb) = r.unwrap();
+        assert!(approx_vec(normal, Vec3::Y));
+        assert!(approx(depth, 0.5));
+    }
+}