feat(physics): add GJK/EPA narrow phase for convex shape collision

Implement GJK (Gilbert-Johnson-Keerthi) overlap detection and EPA
(Expanding Polytope Algorithm) penetration resolution for arbitrary
convex collider pairs. Used as the fallback narrow phase for any
combination involving Capsule, while Sphere/Box specialized routines
are preserved for performance.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

crates/voltex_physics/src/collision.rs

@@ -6,6 +6,7 @@ use crate::collider::Collider;
 use crate::contact::ContactPoint;
 use crate::bvh::BvhTree;
 use crate::narrow;
+use crate::gjk;
 
 pub fn detect_collisions(world: &World) -> Vec<ContactPoint> {
     // 1. Gather entities with Transform + Collider
@@ -54,6 +55,10 @@ pub fn detect_collisions(world: &World) -> Vec<ContactPoint> {
             (Collider::Box { half_extents: ha }, Collider::Box { half_extents: hb }) => {
                 narrow::box_vs_box(pos_a, *ha, pos_b, *hb)
             }
+            // Any combination involving Capsule uses GJK/EPA
+            (Collider::Capsule { .. }, _) | (_, Collider::Capsule { .. }) => {
+                gjk::gjk_epa(&col_a, pos_a, &col_b, pos_b)
+            }
         };
 
         if let Some((normal, depth, point_on_a, point_on_b)) = result {

crates/voltex_physics/src/gjk.rs

@@ -0,0 +1,521 @@
+use voltex_math::Vec3;
+use crate::collider::Collider;
+
+/// Returns the farthest point on a convex collider in a given direction.
+fn support(collider: &Collider, position: Vec3, direction: Vec3) -> Vec3 {
+    match collider {
+        Collider::Sphere { radius } => {
+            let len = direction.length();
+            if len < 1e-10 {
+                return position;
+            }
+            position + direction * (*radius / len)
+        }
+        Collider::Box { half_extents } => {
+            Vec3::new(
+                position.x + if direction.x >= 0.0 { half_extents.x } else { -half_extents.x },
+                position.y + if direction.y >= 0.0 { half_extents.y } else { -half_extents.y },
+                position.z + if direction.z >= 0.0 { half_extents.z } else { -half_extents.z },
+            )
+        }
+        Collider::Capsule { radius, half_height } => {
+            let base = if direction.y >= 0.0 {
+                Vec3::new(position.x, position.y + *half_height, position.z)
+            } else {
+                Vec3::new(position.x, position.y - *half_height, position.z)
+            };
+            let len = direction.length();
+            if len < 1e-10 {
+                return base;
+            }
+            base + direction * (*radius / len)
+        }
+    }
+}
+
+/// Minkowski difference support: support_A(dir) - support_B(-dir)
+fn support_minkowski(a: &Collider, pos_a: Vec3, b: &Collider, pos_b: Vec3, dir: Vec3) -> Vec3 {
+    support(a, pos_a, dir) - support(b, pos_b, -dir)
+}
+
+/// GJK: returns Some(simplex) if shapes overlap, None if separated.
+pub fn gjk(a: &Collider, pos_a: Vec3, b: &Collider, pos_b: Vec3) -> Option<Vec<Vec3>> {
+    let mut dir = pos_b - pos_a;
+    if dir.length_squared() < 1e-10 {
+        dir = Vec3::X;
+    }
+
+    let mut simplex: Vec<Vec3> = Vec::new();
+    let s = support_minkowski(a, pos_a, b, pos_b, dir);
+    simplex.push(s);
+    dir = -s;
+
+    for _ in 0..64 {
+        if dir.length_squared() < 1e-20 {
+            return build_tetrahedron(&simplex, a, pos_a, b, pos_b);
+        }
+
+        let new_point = support_minkowski(a, pos_a, b, pos_b, dir);
+
+        // If the new point didn't pass the origin, no intersection
+        if new_point.dot(dir) < 0.0 {
+            return None;
+        }
+
+        simplex.push(new_point);
+
+        if process_simplex(&mut simplex, &mut dir) {
+            return build_tetrahedron(&simplex, a, pos_a, b, pos_b);
+        }
+    }
+
+    if simplex.len() >= 4 {
+        Some(simplex)
+    } else {
+        None
+    }
+}
+
+fn build_tetrahedron(
+    simplex: &[Vec3],
+    a: &Collider, pos_a: Vec3,
+    b: &Collider, pos_b: Vec3,
+) -> Option<Vec<Vec3>> {
+    let mut pts: Vec<Vec3> = simplex.to_vec();
+    if pts.len() >= 4 {
+        return Some(pts[..4].to_vec());
+    }
+    let dirs = [Vec3::X, Vec3::Y, Vec3::Z, -Vec3::X, -Vec3::Y, -Vec3::Z];
+    for &d in &dirs {
+        if pts.len() >= 4 { break; }
+        let p = support_minkowski(a, pos_a, b, pos_b, d);
+        if !pts.iter().any(|s| (*s - p).length_squared() < 1e-10) {
+            pts.push(p);
+        }
+    }
+    if pts.len() >= 4 {
+        Some(pts[..4].to_vec())
+    } else {
+        Some(pts)
+    }
+}
+
+fn process_simplex(simplex: &mut Vec<Vec3>, dir: &mut Vec3) -> bool {
+    match simplex.len() {
+        2 => process_line(simplex, dir),
+        3 => process_triangle(simplex, dir),
+        4 => process_tetrahedron(simplex, dir),
+        _ => false,
+    }
+}
+
+// Simplex is [B, A] where A is the most recently added point
+fn process_line(simplex: &mut Vec<Vec3>, dir: &mut Vec3) -> bool {
+    let a = simplex[1];
+    let b = simplex[0];
+    let ab = b - a;
+    let ao = -a;
+
+    if ab.dot(ao) > 0.0 {
+        // Origin is in the region of the line AB
+        // Direction perpendicular to AB toward origin
+        let t = ab.cross(ao).cross(ab);
+        if t.length_squared() < 1e-20 {
+            // Origin is on the line — use any perpendicular
+            *dir = perpendicular_to(ab);
+        } else {
+            *dir = t;
+        }
+    } else {
+        // Origin is behind A, closest to A alone
+        *simplex = vec![a];
+        *dir = ao;
+    }
+    false
+}
+
+// Simplex is [C, B, A] where A is the most recently added
+fn process_triangle(simplex: &mut Vec<Vec3>, dir: &mut Vec3) -> bool {
+    let a = simplex[2];
+    let b = simplex[1];
+    let c = simplex[0];
+
+    let ab = b - a;
+    let ac = c - a;
+    let ao = -a;
+    let abc = ab.cross(ac); // triangle normal
+
+    // Check if origin is outside edge AB (on the side away from C)
+    let ab_normal = ab.cross(abc); // perpendicular to AB, pointing away from triangle interior
+    if ab_normal.dot(ao) > 0.0 {
+        // Origin is outside AB edge
+        if ab.dot(ao) > 0.0 {
+            *simplex = vec![b, a];
+            *dir = ab.cross(ao).cross(ab);
+            if dir.length_squared() < 1e-20 {
+                *dir = perpendicular_to(ab);
+            }
+        } else {
+            *simplex = vec![a];
+            *dir = ao;
+        }
+        return false;
+    }
+
+    // Check if origin is outside edge AC (on the side away from B)
+    let ac_normal = abc.cross(ac); // perpendicular to AC, pointing away from triangle interior
+    if ac_normal.dot(ao) > 0.0 {
+        // Origin is outside AC edge
+        if ac.dot(ao) > 0.0 {
+            *simplex = vec![c, a];
+            *dir = ac.cross(ao).cross(ac);
+            if dir.length_squared() < 1e-20 {
+                *dir = perpendicular_to(ac);
+            }
+        } else {
+            *simplex = vec![a];
+            *dir = ao;
+        }
+        return false;
+    }
+
+    // Origin is within the triangle prism — check above or below
+    if abc.dot(ao) > 0.0 {
+        // Origin is above the triangle
+        *dir = abc;
+    } else {
+        // Origin is below the triangle — flip winding
+        *simplex = vec![b, c, a];
+        *dir = -abc;
+    }
+    false
+}
+
+// Simplex is [D, C, B, A] where A is the most recently added
+fn process_tetrahedron(simplex: &mut Vec<Vec3>, dir: &mut Vec3) -> bool {
+    let a = simplex[3];
+    let b = simplex[2];
+    let c = simplex[1];
+    let d = simplex[0];
+
+    let ab = b - a;
+    let ac = c - a;
+    let ad = d - a;
+    let ao = -a;
+
+    // Face normals, oriented outward from the tetrahedron
+    let abc = ab.cross(ac);
+    let acd = ac.cross(ad);
+    let adb = ad.cross(ab);
+
+    // Ensure normals point outward: ABC normal should point away from D
+    let abc_out = if abc.dot(ad) < 0.0 { abc } else { -abc };
+    let acd_out = if acd.dot(ab) < 0.0 { acd } else { -acd };
+    let adb_out = if adb.dot(ac) < 0.0 { adb } else { -adb };
+
+    if abc_out.dot(ao) > 0.0 {
+        // Origin above face ABC — reduce to triangle ABC
+        *simplex = vec![c, b, a];
+        *dir = abc_out;
+        return false;
+    }
+    if acd_out.dot(ao) > 0.0 {
+        // Origin above face ACD — reduce to triangle ACD
+        *simplex = vec![d, c, a];
+        *dir = acd_out;
+        return false;
+    }
+    if adb_out.dot(ao) > 0.0 {
+        // Origin above face ADB — reduce to triangle ADB
+        *simplex = vec![b, d, a];
+        *dir = adb_out;
+        return false;
+    }
+
+    // Origin is inside the tetrahedron
+    true
+}
+
+fn perpendicular_to(v: Vec3) -> Vec3 {
+    let candidate = if v.x.abs() < 0.9 { Vec3::X } else { Vec3::Y };
+    v.cross(candidate)
+}
+
+// ==================== EPA ====================
+
+const EPA_MAX_ITER: usize = 64;
+const EPA_TOLERANCE: f32 = 0.0001;
+
+#[derive(Clone)]
+struct EpaFace {
+    indices: [usize; 3],
+    normal: Vec3,
+    distance: f32,
+}
+
+/// EPA: given a simplex containing the origin, find penetration info.
+/// Returns (normal, depth, point_on_a, point_on_b).
+pub fn epa(
+    a: &Collider, pos_a: Vec3,
+    b: &Collider, pos_b: Vec3,
+    simplex: &[Vec3],
+) -> (Vec3, f32, Vec3, Vec3) {
+    let mut polytope: Vec<Vec3> = simplex.to_vec();
+
+    if polytope.len() < 4 {
+        let diff = pos_b - pos_a;
+        let len = diff.length();
+        let normal = if len > 1e-8 { diff * (1.0 / len) } else { Vec3::Y };
+        return (normal, 0.0, pos_a, pos_b);
+    }
+
+    // Build initial 4 faces of tetrahedron
+    let mut faces: Vec<EpaFace> = Vec::new();
+    let face_indices: [[usize; 3]; 4] = [
+        [0, 1, 2],
+        [0, 3, 1],
+        [0, 2, 3],
+        [1, 3, 2],
+    ];
+
+    for indices in &face_indices {
+        let mut face = make_face(&polytope, *indices);
+        // Ensure outward-pointing normal (away from origin)
+        if face.distance < 0.0 {
+            face.indices.swap(0, 1);
+            face.normal = -face.normal;
+            face.distance = -face.distance;
+        }
+        if face.normal.length_squared() > 1e-10 {
+            faces.push(face);
+        }
+    }
+
+    let mut closest_face = EpaFace {
+        indices: [0, 0, 0],
+        normal: Vec3::Y,
+        distance: f32::MAX,
+    };
+
+    for _ in 0..EPA_MAX_ITER {
+        if faces.is_empty() {
+            break;
+        }
+
+        // Find closest face
+        let mut min_dist = f32::MAX;
+        let mut min_idx = 0;
+        for (i, face) in faces.iter().enumerate() {
+            if face.distance < min_dist {
+                min_dist = face.distance;
+                min_idx = i;
+            }
+        }
+        closest_face = faces[min_idx].clone();
+
+        let new_point = support_minkowski(a, pos_a, b, pos_b, closest_face.normal);
+        let new_dist = new_point.dot(closest_face.normal);
+
+        if new_dist - min_dist < EPA_TOLERANCE {
+            break;
+        }
+
+        // Add new point
+        let new_idx = polytope.len();
+        polytope.push(new_point);
+
+        // Find and remove visible faces, collect horizon edges
+        let mut edges: Vec<[usize; 2]> = Vec::new();
+        faces.retain(|face| {
+            let v = polytope[face.indices[0]];
+            if face.normal.dot(new_point - v) > 0.0 {
+                for i in 0..3 {
+                    let edge = [face.indices[i], face.indices[(i + 1) % 3]];
+                    let rev_idx = edges.iter().position(|e| e[0] == edge[1] && e[1] == edge[0]);
+                    if let Some(idx) = rev_idx {
+                        edges.remove(idx);
+                    } else {
+                        edges.push(edge);
+                    }
+                }
+                false
+            } else {
+                true
+            }
+        });
+
+        // Create new faces from horizon edges
+        for edge in &edges {
+            let mut face = make_face(&polytope, [edge[0], edge[1], new_idx]);
+            if face.distance < 0.0 {
+                face.indices.swap(0, 1);
+                face.normal = -face.normal;
+                face.distance = -face.distance;
+            }
+            if face.normal.length_squared() > 1e-10 {
+                faces.push(face);
+            }
+        }
+    }
+
+    let normal = closest_face.normal;
+    let depth = closest_face.distance.max(0.0);
+
+    let point_on_a = support(a, pos_a, normal);
+    let point_on_b = support(b, pos_b, -normal);
+
+    (normal, depth, point_on_a, point_on_b)
+}
+
+fn make_face(polytope: &[Vec3], indices: [usize; 3]) -> EpaFace {
+    let a = polytope[indices[0]];
+    let b = polytope[indices[1]];
+    let c = polytope[indices[2]];
+
+    let ab = b - a;
+    let ac = c - a;
+    let mut normal = ab.cross(ac);
+    let len = normal.length();
+    if len > 1e-10 {
+        normal = normal * (1.0 / len);
+    } else {
+        normal = Vec3::Y;
+    }
+
+    let distance = normal.dot(a);
+
+    EpaFace {
+        indices,
+        normal,
+        distance,
+    }
+}
+
+/// Combined GJK + EPA: returns (normal, depth, point_on_a, point_on_b) or None.
+pub fn gjk_epa(
+    a: &Collider, pos_a: Vec3,
+    b: &Collider, pos_b: Vec3,
+) -> Option<(Vec3, f32, Vec3, Vec3)> {
+    let simplex = gjk(a, pos_a, b, pos_b)?;
+    Some(epa(a, pos_a, b, pos_b, &simplex))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn approx(a: f32, b: f32, tol: f32) -> bool {
+        (a - b).abs() < tol
+    }
+
+    #[test]
+    fn test_gjk_spheres_overlapping() {
+        let a = Collider::Sphere { radius: 1.0 };
+        let b = Collider::Sphere { radius: 1.0 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(1.5, 0.0, 0.0);
+
+        let result = gjk_epa(&a, pos_a, &b, pos_b);
+        assert!(result.is_some());
+        let (normal, depth, _pa, _pb) = result.unwrap();
+        assert!(normal.x.abs() > 0.5);
+        assert!(approx(depth, 0.5, 0.1));
+    }
+
+    #[test]
+    fn test_gjk_spheres_separated() {
+        let a = Collider::Sphere { radius: 1.0 };
+        let b = Collider::Sphere { radius: 1.0 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(5.0, 0.0, 0.0);
+
+        let result = gjk(&a, pos_a, &b, pos_b);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_gjk_capsule_vs_sphere_overlap() {
+        let a = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let b = Collider::Sphere { radius: 1.0 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(1.0, 0.0, 0.0);
+
+        let result = gjk_epa(&a, pos_a, &b, pos_b);
+        assert!(result.is_some());
+        let (_normal, depth, _pa, _pb) = result.unwrap();
+        assert!(depth > 0.0);
+    }
+
+    #[test]
+    fn test_gjk_capsule_vs_sphere_separated() {
+        let a = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let b = Collider::Sphere { radius: 0.5 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(5.0, 0.0, 0.0);
+
+        let result = gjk(&a, pos_a, &b, pos_b);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_gjk_capsule_vs_box_overlap() {
+        let a = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let b = Collider::Box { half_extents: Vec3::ONE };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(1.0, 0.0, 0.0);
+
+        let result = gjk_epa(&a, pos_a, &b, pos_b);
+        assert!(result.is_some());
+        let (_normal, depth, _pa, _pb) = result.unwrap();
+        assert!(depth > 0.0);
+    }
+
+    #[test]
+    fn test_gjk_capsule_vs_capsule_overlap() {
+        let a = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let b = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(0.5, 0.0, 0.0);
+
+        let result = gjk_epa(&a, pos_a, &b, pos_b);
+        assert!(result.is_some());
+        let (_normal, depth, _pa, _pb) = result.unwrap();
+        assert!(depth > 0.0);
+    }
+
+    #[test]
+    fn test_gjk_capsule_vs_capsule_separated() {
+        let a = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let b = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let pos_a = Vec3::ZERO;
+        let pos_b = Vec3::new(5.0, 0.0, 0.0);
+
+        let result = gjk(&a, pos_a, &b, pos_b);
+        assert!(result.is_none());
+    }
+
+    #[test]
+    fn test_support_sphere() {
+        let c = Collider::Sphere { radius: 2.0 };
+        let p = support(&c, Vec3::ZERO, Vec3::X);
+        assert!(approx(p.x, 2.0, 1e-5));
+        assert!(approx(p.y, 0.0, 1e-5));
+        assert!(approx(p.z, 0.0, 1e-5));
+    }
+
+    #[test]
+    fn test_support_box() {
+        let c = Collider::Box { half_extents: Vec3::new(1.0, 2.0, 3.0) };
+        let p = support(&c, Vec3::ZERO, Vec3::new(1.0, -1.0, 1.0));
+        assert!(approx(p.x, 1.0, 1e-5));
+        assert!(approx(p.y, -2.0, 1e-5));
+        assert!(approx(p.z, 3.0, 1e-5));
+    }
+
+    #[test]
+    fn test_support_capsule() {
+        let c = Collider::Capsule { radius: 0.5, half_height: 1.0 };
+        let p = support(&c, Vec3::ZERO, Vec3::Y);
+        assert!(approx(p.y, 1.5, 1e-5));
+    }
+}

crates/voltex_physics/src/lib.rs

@@ -3,6 +3,7 @@ pub mod ray;
 pub mod collider;
 pub mod contact;
 pub mod narrow;
+pub mod gjk;
 pub mod collision;
 pub mod rigid_body;
 pub mod integrator;