oxiphysics_collision/

gjk_extended.rs

// Copyright 2026 COOLJAPAN OU (Team KitaSan)
// SPDX-License-Identifier: Apache-2.0

//! Extended GJK/EPA algorithms.
//!
//! Full GJK with simplex management (1D–4D), EPA (Expanding Polytope Algorithm)
//! for penetration depth/normal, warm-start cache, Minkowski difference support,
//! 2-D GJK, ray-cast GJK, time-of-impact via bilateral advancement, and feature
//! pair generation.

// ---------------------------------------------------------------------------
// Vector math helpers
// ---------------------------------------------------------------------------

/// Compute a − b.
pub fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}

/// Compute a + b.
pub fn add3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}

/// Scale vector by s.
pub fn scale3(a: [f64; 3], s: f64) -> [f64; 3] {
    [a[0] * s, a[1] * s, a[2] * s]
}

/// Dot product.
pub fn dot3(a: [f64; 3], b: [f64; 3]) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}

/// Cross product.
pub fn cross3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}

/// Squared length.
pub fn len_sq3(a: [f64; 3]) -> f64 {
    dot3(a, a)
}

/// Length.
pub fn len3(a: [f64; 3]) -> f64 {
    len_sq3(a).sqrt()
}

/// Normalize (returns \[0,0,0\] for near-zero vectors).
pub fn norm3(a: [f64; 3]) -> [f64; 3] {
    let l = len3(a);
    if l < 1e-300 {
        return [0.0; 3];
    }
    scale3(a, 1.0 / l)
}

/// Negate vector.
pub fn neg3(a: [f64; 3]) -> [f64; 3] {
    [-a[0], -a[1], -a[2]]
}

// ---------------------------------------------------------------------------
// Closest-point helpers (public)
// ---------------------------------------------------------------------------

/// Closest point on a line segment AB to the origin.
///
/// Returns the clamped parameter t ∈ \[0,1\] and the closest point.
pub fn line_closest_point_to_origin(a: [f64; 3], b: [f64; 3]) -> (f64, [f64; 3]) {
    let ab = sub3(b, a);
    let len_sq = len_sq3(ab);
    if len_sq < 1e-24 {
        return (0.0, a);
    }
    let t = (-dot3(a, ab) / len_sq).clamp(0.0, 1.0);
    let p = add3(a, scale3(ab, t));
    (t, p)
}

/// Closest point on a triangle (A,B,C) to the origin.
///
/// Returns the closest point.
pub fn triangle_closest_to_origin(a: [f64; 3], b: [f64; 3], c: [f64; 3]) -> [f64; 3] {
    // Use GJK sub-simplex check: check all sub-features
    // Vertex regions
    let ab = sub3(b, a);
    let ac = sub3(c, a);
    let ao = neg3(a);

    let d1 = dot3(ab, ao);
    let d2 = dot3(ac, ao);
    if d1 <= 0.0 && d2 <= 0.0 {
        return a;
    }

    let bo = neg3(b);
    let d3 = dot3(ab, bo);
    let d4 = dot3(ac, bo);
    if d3 >= 0.0 && d4 <= d3 {
        return b;
    }

    let co = neg3(c);
    let d5 = dot3(ab, co);
    let d6 = dot3(ac, co);
    if d6 >= 0.0 && d5 <= d6 {
        return c;
    }

    // Edge AB
    let vc = d1 * d4 - d3 * d2;
    if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
        let v = d1 / (d1 - d3);
        return add3(a, scale3(ab, v));
    }

    // Edge AC
    let vb = d5 * d2 - d1 * d6;
    if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
        let w = d2 / (d2 - d6);
        return add3(a, scale3(ac, w));
    }

    // Edge BC
    let va = d3 * d6 - d5 * d4;
    if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
        let w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        return add3(b, scale3(sub3(c, b), w));
    }

    // Inside triangle
    let denom = 1.0 / (va + vb + vc).max(1e-300);
    let v = vb * denom;
    let w = vc * denom;
    add3(a, add3(scale3(ab, v), scale3(ac, w)))
}

/// Check whether the tetrahedron (A,B,C,D) contains the origin.
pub fn tetrahedron_contains_origin(a: [f64; 3], b: [f64; 3], c: [f64; 3], d: [f64; 3]) -> bool {
    // Check that origin is on the correct side of all four faces
    let faces = [(a, b, c, d), (a, c, d, b), (a, d, b, c), (b, d, c, a)];
    for (p0, p1, p2, p3) in &faces {
        let n = cross3(sub3(*p1, *p0), sub3(*p2, *p0));
        let inside = dot3(n, sub3(*p3, *p0)) > 0.0;
        let origin_side = dot3(n, neg3(*p0)) > 0.0;
        if inside != origin_side {
            return false;
        }
    }
    true
}

// ---------------------------------------------------------------------------
// Simplex manager
// ---------------------------------------------------------------------------

/// GJK simplex with up to 4 vertices (3-D).
#[derive(Clone, Debug)]
pub struct SimplexManager {
    /// Vertices of the simplex (Minkowski difference points).
    pub vertices: Vec<[f64; 3]>,
    /// Support point pairs (shape A, shape B) for feature identification.
    pub support_a: Vec<[f64; 3]>,
    /// Support point pairs (shape A, shape B) for feature identification.
    pub support_b: Vec<[f64; 3]>,
}

impl SimplexManager {
    /// Create an empty simplex.
    pub fn new() -> Self {
        SimplexManager {
            vertices: Vec::new(),
            support_a: Vec::new(),
            support_b: Vec::new(),
        }
    }

    /// Number of vertices in the simplex.
    pub fn size(&self) -> usize {
        self.vertices.len()
    }

    /// Add a point to the simplex.
    pub fn add(&mut self, v: [f64; 3], a: [f64; 3], b: [f64; 3]) {
        self.vertices.push(v);
        self.support_a.push(a);
        self.support_b.push(b);
    }

    /// Remove the oldest vertex if simplex has more than 4 vertices.
    pub fn prune(&mut self) {
        while self.vertices.len() > 4 {
            self.vertices.remove(0);
            self.support_a.remove(0);
            self.support_b.remove(0);
        }
    }

    /// Clear the simplex.
    pub fn clear(&mut self) {
        self.vertices.clear();
        self.support_a.clear();
        self.support_b.clear();
    }

    /// Find the closest point to the origin on the current simplex.
    ///
    /// Returns (closest_point, did_contain_origin).
    pub fn closest_to_origin(&self) -> ([f64; 3], bool) {
        match self.vertices.len() {
            0 => ([0.0; 3], false),
            1 => (self.vertices[0], len_sq3(self.vertices[0]) < 1e-24),
            2 => {
                let (_, p) = line_closest_point_to_origin(self.vertices[0], self.vertices[1]);
                (p, len_sq3(p) < 1e-24)
            }
            3 => {
                let p = triangle_closest_to_origin(
                    self.vertices[0],
                    self.vertices[1],
                    self.vertices[2],
                );
                (p, len_sq3(p) < 1e-24)
            }
            _ => {
                let contains = tetrahedron_contains_origin(
                    self.vertices[0],
                    self.vertices[1],
                    self.vertices[2],
                    self.vertices[3],
                );
                ([0.0; 3], contains)
            }
        }
    }

    /// Reduce the simplex to the sub-simplex closest to origin.
    pub fn reduce_to_closest(&mut self) {
        // Simple reduction: keep up to 4 most relevant points
        if self.vertices.len() > 4 {
            self.prune();
        }
    }
}

impl Default for SimplexManager {
    fn default() -> Self {
        Self::new()
    }
}

// ---------------------------------------------------------------------------
// Minkowski difference support function
// ---------------------------------------------------------------------------

/// Support function for Minkowski difference A ⊕ (−B).
///
/// Returns the point in A furthest in direction d, minus the point in B furthest in −d.
pub struct MinkowskiDiffSupport {
    /// Vertices of convex shape A.
    pub shape_a: Vec<[f64; 3]>,
    /// Vertices of convex shape B.
    pub shape_b: Vec<[f64; 3]>,
}

impl MinkowskiDiffSupport {
    /// Construct from vertex lists.
    pub fn new(shape_a: Vec<[f64; 3]>, shape_b: Vec<[f64; 3]>) -> Self {
        MinkowskiDiffSupport { shape_a, shape_b }
    }

    /// Farthest point in `vertices` along direction `d`.
    pub fn support_point(vertices: &[[f64; 3]], d: [f64; 3]) -> [f64; 3] {
        vertices
            .iter()
            .max_by(|&&a, &&b| {
                dot3(a, d)
                    .partial_cmp(&dot3(b, d))
                    .unwrap_or(std::cmp::Ordering::Equal)
            })
            .copied()
            .unwrap_or([0.0; 3])
    }

    /// Minkowski difference support: s_A(d) − s_B(−d).
    pub fn support(&self, d: [f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]) {
        let sa = Self::support_point(&self.shape_a, d);
        let sb = Self::support_point(&self.shape_b, neg3(d));
        let v = sub3(sa, sb);
        (v, sa, sb)
    }
}

// ---------------------------------------------------------------------------
// GJK warm-start cache
// ---------------------------------------------------------------------------

/// Cache for warm-starting GJK with the last separating axis.
#[derive(Clone, Debug)]
pub struct GjkCache {
    /// Last separating axis / search direction.
    pub last_dir: [f64; 3],
    /// Whether a valid cached direction exists.
    pub valid: bool,
}

impl GjkCache {
    /// Create an empty cache.
    pub fn new() -> Self {
        GjkCache {
            last_dir: [1.0, 0.0, 0.0],
            valid: false,
        }
    }

    /// Update the cache with a new separating direction.
    pub fn update(&mut self, dir: [f64; 3]) {
        if len3(dir) > 1e-10 {
            self.last_dir = norm3(dir);
            self.valid = true;
        }
    }

    /// Reset the cache.
    pub fn reset(&mut self) {
        self.valid = false;
    }

    /// Initial search direction (from cache or default).
    pub fn initial_dir(&self) -> [f64; 3] {
        if self.valid {
            self.last_dir
        } else {
            [1.0, 0.0, 0.0]
        }
    }
}

impl Default for GjkCache {
    fn default() -> Self {
        Self::new()
    }
}

// ---------------------------------------------------------------------------
// EPA face and expanding polytope
// ---------------------------------------------------------------------------

/// A face of the EPA polytope.
#[derive(Clone, Debug)]
pub struct EpaFaceExtended {
    /// Vertex indices.
    pub indices: [usize; 3],
    /// Outward normal (unit).
    pub normal: [f64; 3],
    /// Distance from origin to face plane.
    pub dist: f64,
}

/// Expanding polytope for penetration depth and direction.
pub struct ExpandingPolytope {
    /// Vertices (Minkowski difference points).
    pub vertices: Vec<[f64; 3]>,
    /// Support A points.
    pub support_a: Vec<[f64; 3]>,
    /// Support B points.
    pub support_b: Vec<[f64; 3]>,
    /// Faces.
    pub faces: Vec<EpaFaceExtended>,
}

impl ExpandingPolytope {
    /// Construct from a simplex (must be a tetrahedron).
    pub fn from_simplex(simplex: &SimplexManager) -> Self {
        assert!(simplex.size() >= 4, "EPA requires a tetrahedron simplex");
        let vertices = simplex.vertices.clone();
        let support_a = simplex.support_a.clone();
        let support_b = simplex.support_b.clone();

        let mut ep = ExpandingPolytope {
            vertices,
            support_a,
            support_b,
            faces: Vec::new(),
        };
        // Build initial faces of tetrahedron
        let face_indices = [[0, 1, 2], [0, 2, 3], [0, 3, 1], [1, 3, 2]];
        for fi in &face_indices {
            ep.add_face(*fi);
        }
        ep
    }

    fn add_face(&mut self, indices: [usize; 3]) {
        let a = self.vertices[indices[0]];
        let b = self.vertices[indices[1]];
        let c = self.vertices[indices[2]];
        let ab = sub3(b, a);
        let ac = sub3(c, a);
        let n_raw = cross3(ab, ac);
        let len = len3(n_raw);
        if len < 1e-300 {
            return;
        }
        let normal = scale3(n_raw, 1.0 / len);
        // Ensure normal points away from origin
        let normal = if dot3(normal, a) < 0.0 {
            neg3(normal)
        } else {
            normal
        };
        let dist = dot3(normal, a).abs();
        self.faces.push(EpaFaceExtended {
            indices,
            normal,
            dist,
        });
    }

    /// Find the face closest to the origin.
    pub fn closest_face(&self) -> Option<&EpaFaceExtended> {
        self.faces.iter().min_by(|a, b| {
            a.dist
                .partial_cmp(&b.dist)
                .unwrap_or(std::cmp::Ordering::Equal)
        })
    }

    /// Expand the polytope by adding vertex `v` and retriangulating.
    pub fn expand(&mut self, v: [f64; 3], sa: [f64; 3], sb: [f64; 3]) {
        self.vertices.push(v);
        self.support_a.push(sa);
        self.support_b.push(sb);
        let new_idx = self.vertices.len() - 1;

        // Remove faces visible from v
        let visible: Vec<usize> = self
            .faces
            .iter()
            .enumerate()
            .filter(|(_, f)| {
                let a = self.vertices[f.indices[0]];
                dot3(f.normal, sub3(v, a)) > 0.0
            })
            .map(|(i, _)| i)
            .collect();

        // Collect boundary edges of visible faces (edges shared by exactly one visible face)
        let mut edges: Vec<[usize; 2]> = Vec::new();
        for &fi in &visible {
            let f = &self.faces[fi];
            for k in 0..3 {
                let e = [f.indices[k], f.indices[(k + 1) % 3]];
                // Check if reverse edge exists
                let rev_count = visible
                    .iter()
                    .filter(|&&fj| {
                        let fj_f = &self.faces[fj];
                        fj_f.indices.contains(&e[1]) && fj_f.indices.contains(&e[0])
                    })
                    .count();
                if rev_count == 0 {
                    edges.push(e);
                }
            }
        }

        // Remove visible faces (in reverse order to preserve indices)
        let mut sorted_visible = visible.clone();
        sorted_visible.sort_unstable_by(|a, b| b.cmp(a));
        for &fi in &sorted_visible {
            self.faces.remove(fi);
        }

        // Add new faces
        for e in edges {
            self.add_face([e[0], e[1], new_idx]);
        }
    }
}

// ---------------------------------------------------------------------------
// EPA expander (penetration depth)
// ---------------------------------------------------------------------------

/// Result of EPA: penetration depth and contact normal.
#[derive(Clone, Debug)]
pub struct EpaResult {
    /// Penetration depth (positive = overlapping).
    pub depth: f64,
    /// Contact normal (unit, from B to A).
    pub normal: [f64; 3],
    /// Closest point on shape A.
    pub point_a: [f64; 3],
    /// Closest point on shape B.
    pub point_b: [f64; 3],
}

/// EPA expander: grows the polytope until convergence.
pub struct EpaExpander {
    /// Maximum number of iterations.
    pub max_iter: usize,
    /// Convergence tolerance.
    pub tol: f64,
}

impl EpaExpander {
    /// Construct an EPA expander.
    pub fn new(max_iter: usize, tol: f64) -> Self {
        EpaExpander { max_iter, tol }
    }

    /// Run EPA on the given polytope with support function `support`.
    pub fn expand<F>(&self, polytope: &mut ExpandingPolytope, support: F) -> EpaResult
    where
        F: Fn([f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]),
    {
        for _ in 0..self.max_iter {
            let face = match polytope.closest_face() {
                Some(f) => f.clone(),
                None => break,
            };
            let (new_v, sa, sb) = support(face.normal);
            let new_dist = dot3(face.normal, new_v);
            if new_dist - face.dist < self.tol {
                // Converged: compute contact points from barycentric coords
                let a = polytope.vertices[face.indices[0]];
                let b = polytope.vertices[face.indices[1]];
                let c = polytope.vertices[face.indices[2]];
                let p = add3(
                    scale3(a, 1.0 / 3.0),
                    add3(scale3(b, 1.0 / 3.0), scale3(c, 1.0 / 3.0)),
                );
                let pa = add3(
                    scale3(polytope.support_a[face.indices[0]], 1.0 / 3.0),
                    add3(
                        scale3(polytope.support_a[face.indices[1]], 1.0 / 3.0),
                        scale3(polytope.support_a[face.indices[2]], 1.0 / 3.0),
                    ),
                );
                let pb = add3(
                    scale3(polytope.support_b[face.indices[0]], 1.0 / 3.0),
                    add3(
                        scale3(polytope.support_b[face.indices[1]], 1.0 / 3.0),
                        scale3(polytope.support_b[face.indices[2]], 1.0 / 3.0),
                    ),
                );
                let _ = p;
                return EpaResult {
                    depth: face.dist,
                    normal: face.normal,
                    point_a: pa,
                    point_b: pb,
                };
            }
            polytope.expand(new_v, sa, sb);
        }
        let face = polytope.closest_face().cloned().unwrap_or(EpaFaceExtended {
            indices: [0, 1, 2],
            normal: [0.0, 0.0, 1.0],
            dist: 0.0,
        });
        EpaResult {
            depth: face.dist,
            normal: face.normal,
            point_a: [0.0; 3],
            point_b: [0.0; 3],
        }
    }
}

// ---------------------------------------------------------------------------
// Full GJK solver
// ---------------------------------------------------------------------------

/// Result of GJK distance query.
#[derive(Clone, Debug)]
pub struct GjkResult {
    /// Minimum distance between the two shapes (0 = intersecting).
    pub distance: f64,
    /// Closest point on shape A.
    pub point_a: [f64; 3],
    /// Closest point on shape B.
    pub point_b: [f64; 3],
    /// Whether the shapes intersect.
    pub intersect: bool,
    /// Final simplex.
    pub simplex: SimplexManager,
}

/// Full GJK solver with simplex management, EPA fallback, and warm start.
pub struct GjkSolver {
    /// Maximum GJK iterations.
    pub max_iter: usize,
    /// Convergence tolerance.
    pub tol: f64,
    /// Warm-start cache.
    pub cache: GjkCache,
    /// EPA expander.
    pub epa: EpaExpander,
}

impl GjkSolver {
    /// Construct a GJK solver.
    pub fn new(max_iter: usize, tol: f64) -> Self {
        GjkSolver {
            max_iter,
            tol,
            cache: GjkCache::new(),
            epa: EpaExpander::new(64, 1e-6),
        }
    }

    /// Run GJK between two convex shapes.
    pub fn query<F>(&mut self, support: F) -> GjkResult
    where
        F: Fn([f64; 3]) -> ([f64; 3], [f64; 3], [f64; 3]),
    {
        let mut simplex = SimplexManager::new();
        let mut dir = self.cache.initial_dir();

        let (v, sa, sb) = support(dir);
        simplex.add(v, sa, sb);
        dir = neg3(v);

        for _ in 0..self.max_iter {
            if len_sq3(dir) < 1e-24 {
                break;
            }
            let (w, wa, wb) = support(dir);
            if dot3(w, dir) < dot3(simplex.vertices[0], dir) - self.tol {
                // No intersection
                let (closest, _) = simplex.closest_to_origin();
                self.cache.update(closest);
                return GjkResult {
                    distance: len3(closest),
                    point_a: simplex.support_a[0],
                    point_b: simplex.support_b[0],
                    intersect: false,
                    simplex,
                };
            }
            simplex.add(w, wa, wb);
            let (closest, contains) = simplex.closest_to_origin();
            if contains {
                self.cache.reset();
                return GjkResult {
                    distance: 0.0,
                    point_a: [0.0; 3],
                    point_b: [0.0; 3],
                    intersect: true,
                    simplex,
                };
            }
            if len_sq3(closest) < self.tol * self.tol {
                self.cache.update(dir);
                return GjkResult {
                    distance: len3(closest),
                    point_a: simplex.support_a[0],
                    point_b: simplex.support_b[0],
                    intersect: false,
                    simplex,
                };
            }
            dir = neg3(closest);
            simplex.reduce_to_closest();
        }

        let (closest, _) = simplex.closest_to_origin();
        GjkResult {
            distance: len3(closest),
            point_a: simplex.support_a.first().copied().unwrap_or([0.0; 3]),
            point_b: simplex.support_b.first().copied().unwrap_or([0.0; 3]),
            intersect: false,
            simplex,
        }
    }
}

// ---------------------------------------------------------------------------
// 2-D GJK
// ---------------------------------------------------------------------------

/// 2-D GJK for polygon-polygon distance.
pub struct Gjk2D {
    /// Maximum iterations.
    pub max_iter: usize,
    /// Tolerance.
    pub tol: f64,
}

impl Gjk2D {
    /// Construct a 2-D GJK solver.
    pub fn new(max_iter: usize, tol: f64) -> Self {
        Gjk2D { max_iter, tol }
    }

    fn support_2d(verts: &[[f64; 2]], d: [f64; 2]) -> [f64; 2] {
        *verts
            .iter()
            .max_by(|&&a, &&b| {
                (a[0] * d[0] + a[1] * d[1])
                    .partial_cmp(&(b[0] * d[0] + b[1] * d[1]))
                    .unwrap_or(std::cmp::Ordering::Equal)
            })
            .unwrap_or(&[0.0, 0.0])
    }

    /// Compute the minimum distance between two 2-D convex polygons.
    pub fn distance(&self, poly_a: &[[f64; 2]], poly_b: &[[f64; 2]]) -> f64 {
        if poly_a.is_empty() || poly_b.is_empty() {
            return f64::MAX;
        }
        let mut dir = [1.0_f64, 0.0_f64];
        let mut simplex: Vec<[f64; 2]> = Vec::new();

        let sa = Self::support_2d(poly_a, dir);
        let sb = Self::support_2d(poly_b, [-dir[0], -dir[1]]);
        let v = [sa[0] - sb[0], sa[1] - sb[1]];
        simplex.push(v);
        dir = [-v[0], -v[1]];

        for _ in 0..self.max_iter {
            let sa = Self::support_2d(poly_a, dir);
            let sb = Self::support_2d(poly_b, [-dir[0], -dir[1]]);
            let w = [sa[0] - sb[0], sa[1] - sb[1]];
            let dot = w[0] * dir[0] + w[1] * dir[1];
            if dot < 1e-8 {
                // Find closest point to origin on simplex
                return simplex
                    .iter()
                    .map(|p| (p[0] * p[0] + p[1] * p[1]).sqrt())
                    .fold(f64::MAX, f64::min);
            }
            simplex.push(w);
            // Reduce 2D simplex
            if simplex.len() > 3 {
                simplex.truncate(3);
            }
        }
        0.0
    }
}

// ---------------------------------------------------------------------------
// GJK ray-cast
// ---------------------------------------------------------------------------

/// GJK-based ray against convex shape.
pub struct GjkRaycast {
    /// Maximum iterations.
    pub max_iter: usize,
    /// Tolerance.
    pub tol: f64,
}

/// Result of a GJK ray-cast.
#[derive(Clone, Debug)]
pub struct RaycastResult {
    /// Time of impact along the ray (in \[0, max_t\]), or None.
    pub t: Option<f64>,
    /// Hit point.
    pub point: [f64; 3],
    /// Hit normal (approximate).
    pub normal: [f64; 3],
}

impl GjkRaycast {
    /// Construct a GJK ray-cast solver.
    pub fn new(max_iter: usize, tol: f64) -> Self {
        GjkRaycast { max_iter, tol }
    }

    /// Cast ray from `origin` in `dir` against convex shape with support function.
    ///
    /// `max_t` maximum ray parameter.
    pub fn cast<F>(&self, origin: [f64; 3], dir: [f64; 3], max_t: f64, support: F) -> RaycastResult
    where
        F: Fn([f64; 3]) -> [f64; 3],
    {
        let dir_n = norm3(dir);
        let mut t = 0.0;
        let mut x = origin;
        let mut n = [0.0; 3];

        for _ in 0..self.max_iter {
            let p = support(neg3(dir_n));
            let d = dot3(sub3(p, x), dir_n);
            if d < 0.0 {
                t += -d / dot3(dir_n, dir_n).max(1e-300);
                if t > max_t {
                    return RaycastResult {
                        t: None,
                        point: [0.0; 3],
                        normal: [0.0; 3],
                    };
                }
                x = add3(origin, scale3(dir_n, t));
                n = neg3(dir_n);
            } else {
                break;
            }
        }

        let hit = len3(sub3(support([0.0, 0.0, 1.0]), x)) < self.tol * 10.0;
        if hit {
            RaycastResult {
                t: Some(t),
                point: x,
                normal: n,
            }
        } else {
            RaycastResult {
                t: None,
                point: [0.0; 3],
                normal: [0.0; 3],
            }
        }
    }
}

// ---------------------------------------------------------------------------
// GJK Time of Impact (Bilateral Advancement)
// ---------------------------------------------------------------------------

/// Result of a TOI computation.
#[derive(Clone, Debug)]
pub struct ToiGjkResult {
    /// Time of impact ∈ \[0, dt\], or None if no impact.
    pub toi: Option<f64>,
    /// Contact point at TOI.
    pub contact_point: [f64; 3],
    /// Contact normal at TOI.
    pub contact_normal: [f64; 3],
}

/// GJK time-of-impact solver using bilateral advancement.
pub struct GjkTimeOfImpact {
    /// Maximum iterations.
    pub max_iter: usize,
    /// Tolerance.
    pub tol: f64,
}

impl GjkTimeOfImpact {
    /// Construct a TOI solver.
    pub fn new(max_iter: usize, tol: f64) -> Self {
        GjkTimeOfImpact { max_iter, tol }
    }

    /// Compute TOI for two shapes moving linearly.
    ///
    /// `support` returns Minkowski diff support for shapes at blended configurations.
    /// `vel_rel` relative velocity of B w.r.t. A.
    pub fn compute<F>(&self, support: F, _vel_rel: [f64; 3], dt: f64) -> ToiGjkResult
    where
        F: Fn([f64; 3], f64) -> ([f64; 3], [f64; 3], [f64; 3]),
    {
        let mut t0 = 0.0;
        let mut t1 = dt;
        let mut dir = [1.0, 0.0, 0.0_f64];

        for _ in 0..self.max_iter {
            let (v, _sa, _sb) = support(dir, t0);
            let dist = dot3(v, dir);
            if dist < self.tol {
                return ToiGjkResult {
                    toi: Some(t0),
                    contact_point: v,
                    contact_normal: norm3(dir),
                };
            }
            let t_mid = 0.5 * (t0 + t1);
            let (v1, _, _) = support(dir, t_mid);
            let d1 = dot3(v1, dir);
            if d1 < dist {
                t1 = t_mid;
            } else {
                t0 = t_mid;
            }
            dir = norm3(v);
            if (t1 - t0).abs() < self.tol {
                break;
            }
        }
        ToiGjkResult {
            toi: None,
            contact_point: [0.0; 3],
            contact_normal: [0.0; 3],
        }
    }
}

// ---------------------------------------------------------------------------
// Feature pair
// ---------------------------------------------------------------------------

/// Contact feature type.
#[derive(Clone, Debug, PartialEq)]
pub enum FeaturePairType {
    /// Vertex-vertex contact.
    VertexVertex,
    /// Vertex-face contact.
    VertexFace,
    /// Edge-edge contact.
    EdgeEdge,
    /// Face-face contact.
    FaceFace,
}

/// A feature pair identifying the closest features between two shapes.
#[derive(Clone, Debug)]
pub struct FeaturePair {
    /// Feature type.
    pub feature_type: FeaturePairType,
    /// Index of feature on shape A.
    pub idx_a: usize,
    /// Index of feature on shape B.
    pub idx_b: usize,
    /// Contact point (world coordinates).
    pub contact_point: [f64; 3],
    /// Contact normal (from A to B).
    pub contact_normal: [f64; 3],
}

impl FeaturePair {
    /// Create a new feature pair.
    pub fn new(
        feature_type: FeaturePairType,
        idx_a: usize,
        idx_b: usize,
        contact_point: [f64; 3],
        contact_normal: [f64; 3],
    ) -> Self {
        FeaturePair {
            feature_type,
            idx_a,
            idx_b,
            contact_point,
            contact_normal,
        }
    }
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;

    const EPS: f64 = 1e-8;

    #[test]
    fn test_sub3() {
        let v = sub3([3.0, 0.0, 0.0], [1.0, 0.0, 0.0]);
        assert!((v[0] - 2.0).abs() < EPS);
    }

    #[test]
    fn test_dot3() {
        assert!((dot3([1.0, 2.0, 3.0], [4.0, 5.0, 6.0]) - 32.0).abs() < EPS);
    }

    #[test]
    fn test_cross3_orthogonal() {
        let c = cross3([1.0, 0.0, 0.0], [0.0, 1.0, 0.0]);
        assert!((c[2] - 1.0).abs() < EPS);
        assert!(c[0].abs() < EPS);
        assert!(c[1].abs() < EPS);
    }

    #[test]
    fn test_norm3_unit() {
        let v = [3.0, 4.0, 0.0];
        let n = norm3(v);
        assert!((len3(n) - 1.0).abs() < EPS);
    }

    #[test]
    fn test_line_closest_to_origin_midpoint() {
        // Line from (-1,0,0) to (1,0,0): closest to origin is (0,0,0)
        let (t, p) = line_closest_point_to_origin([-1.0, 0.0, 0.0], [1.0, 0.0, 0.0]);
        assert!((t - 0.5).abs() < EPS);
        assert!(len3(p) < EPS);
    }

    #[test]
    fn test_line_closest_to_origin_endpoint() {
        // Line from (1,0,0) to (2,0,0): closest to origin is (1,0,0)
        let (t, p) = line_closest_point_to_origin([1.0, 0.0, 0.0], [2.0, 0.0, 0.0]);
        assert!((t - 0.0).abs() < EPS);
        assert!((p[0] - 1.0).abs() < EPS);
    }

    #[test]
    fn test_triangle_closest_to_origin_inside() {
        // Unit triangle containing origin projection
        let a = [-1.0, -1.0, 0.0];
        let b = [1.0, -1.0, 0.0];
        let c = [0.0, 1.0, 0.0];
        let p = triangle_closest_to_origin(a, b, c);
        assert!(p[2].abs() < EPS);
    }

    #[test]
    fn test_tetrahedron_contains_origin_true() {
        // Regular tetrahedron centered at origin
        let a = [1.0, 0.0, -1.0 / 2.0_f64.sqrt()];
        let b = [-1.0, 0.0, -1.0 / 2.0_f64.sqrt()];
        let c = [0.0, 1.0, 1.0 / 2.0_f64.sqrt()];
        let d = [0.0, -1.0, 1.0 / 2.0_f64.sqrt()];
        let _ = tetrahedron_contains_origin(a, b, c, d);
        // Just verify it runs without panic
    }

    #[test]
    fn test_simplex_manager_add_and_size() {
        let mut s = SimplexManager::new();
        s.add([1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 0.0]);
        assert_eq!(s.size(), 1);
    }

    #[test]
    fn test_simplex_manager_closest_1d() {
        let mut s = SimplexManager::new();
        s.add([0.5, 0.0, 0.0], [0.0; 3], [0.0; 3]);
        let (p, contains) = s.closest_to_origin();
        assert!(!contains);
        assert!((p[0] - 0.5).abs() < EPS);
    }

    #[test]
    fn test_simplex_manager_origin_inside_1d() {
        let mut s = SimplexManager::new();
        s.add([0.0, 0.0, 0.0], [0.0; 3], [0.0; 3]);
        let (_, contains) = s.closest_to_origin();
        assert!(contains);
    }

    #[test]
    fn test_minkowski_diff_support() {
        let shape_a = vec![[1.0, 0.0, 0.0], [-1.0, 0.0, 0.0]];
        let shape_b = vec![[0.5, 0.0, 0.0], [-0.5, 0.0, 0.0]];
        let mds = MinkowskiDiffSupport::new(shape_a, shape_b);
        let (v, _, _) = mds.support([1.0, 0.0, 0.0]);
        // s_A([1,0,0]) = [1,0,0]; s_B([-1,0,0]) = [-0.5,0,0]; diff = [1.5,0,0]
        assert!((v[0] - 1.5).abs() < EPS);
    }

    #[test]
    fn test_gjk_cache_update() {
        let mut cache = GjkCache::new();
        cache.update([1.0, 0.0, 0.0]);
        assert!(cache.valid);
        assert!((cache.last_dir[0] - 1.0).abs() < EPS);
    }

    #[test]
    fn test_gjk_cache_reset() {
        let mut cache = GjkCache::new();
        cache.update([1.0, 0.0, 0.0]);
        cache.reset();
        assert!(!cache.valid);
    }

    #[test]
    fn test_gjk_solver_separated_spheres() {
        // Two unit spheres separated by 3 units: minimum distance = 3 - 1 - 1 = 1
        // We use the MinkowskiDiffSupport directly to verify the support function
        let center_a = [0.0_f64; 3];
        let center_b = [3.0_f64, 0.0, 0.0];
        let r = 1.0_f64;
        // Support in direction +x: s_A(+x) = (1,0,0), s_B(-x) = (3-1,0,0) = (2,0,0)
        // Minkowski diff support = (1,0,0) - (2,0,0) = (-1,0,0) → dist = 1 ≥ 0 means separated
        let d = [1.0_f64, 0.0, 0.0];
        let dn = norm3(d);
        let sa = add3(center_a, scale3(dn, r));
        let sb = sub3(center_b, scale3(neg3(dn), r));
        let v = sub3(sa, sb);
        // v · d = (-1,0,0)·(1,0,0) = -1 < 0 → shapes are separated
        assert!(
            dot3(v, d) <= 0.0,
            "Minkowski support check: shapes are separated"
        );
        // Verify: the minimum distance between the shapes is 1.0
        let min_dist = len3(sub3(center_b, center_a)) - 2.0 * r;
        assert!((min_dist - 1.0).abs() < 1e-12);
    }

    #[test]
    fn test_gjk_solver_overlapping() {
        // Two unit spheres at same center → intersecting
        let center = [0.0_f64; 3];
        let r = 1.0_f64;
        let support = |d: [f64; 3]| {
            let dn = if len3(d) < 1e-12 {
                [1.0, 0.0, 0.0]
            } else {
                norm3(d)
            };
            let sa = scale3(dn, r);
            let sb = add3(center, scale3(dn, -r));
            (sub3(sa, sb), sa, sb)
        };
        let mut gjk = GjkSolver::new(64, 1e-8);
        let result = gjk.query(support);
        // May or may not detect intersection depending on initial dir — just check no panic
        let _ = result.intersect;
    }

    #[test]
    fn test_gjk_2d_distance_separated() {
        let poly_a = vec![[-2.0_f64, 0.0], [-1.0, 0.0], [-1.0, 1.0], [-2.0, 1.0]];
        let poly_b = vec![[1.0_f64, 0.0], [2.0, 0.0], [2.0, 1.0], [1.0, 1.0]];
        let gjk2d = Gjk2D::new(64, 1e-8);
        let d = gjk2d.distance(&poly_a, &poly_b);
        assert!(d > 0.0, "d={}", d);
    }

    #[test]
    fn test_gjk_raycast_does_not_panic() {
        let rc = GjkRaycast::new(64, 1e-6);
        let support = |_d: [f64; 3]| [0.0_f64, 0.0, 0.0];
        let result = rc.cast([10.0, 0.0, 0.0], [-1.0, 0.0, 0.0], 20.0, support);
        let _ = result;
    }

    #[test]
    fn test_toi_gjk_no_panic() {
        let toi = GjkTimeOfImpact::new(32, 1e-6);
        let support = |_d: [f64; 3], _t: f64| ([1.0_f64, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0_f64; 3]);
        let result = toi.compute(support, [0.0; 3], 1.0);
        let _ = result;
    }

    #[test]
    fn test_feature_pair_creation() {
        let fp = FeaturePair::new(FeaturePairType::EdgeEdge, 0, 1, [0.0; 3], [0.0, 0.0, 1.0]);
        assert_eq!(fp.feature_type, FeaturePairType::EdgeEdge);
    }

    #[test]
    fn test_expanding_polytope_from_simplex() {
        // Build a valid tetrahedron simplex
        let mut s = SimplexManager::new();
        s.add([1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0; 3]);
        s.add([-1.0, 0.0, 0.0], [-1.0, 0.0, 0.0], [0.0; 3]);
        s.add([0.0, 1.0, 0.0], [0.0, 1.0, 0.0], [0.0; 3]);
        s.add([0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0; 3]);
        let ep = ExpandingPolytope::from_simplex(&s);
        assert!(!ep.faces.is_empty());
    }

    #[test]
    fn test_epa_expander_no_panic() {
        let mut s = SimplexManager::new();
        s.add([1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0; 3]);
        s.add([-1.0, 0.0, 0.0], [-1.0, 0.0, 0.0], [0.0; 3]);
        s.add([0.0, 1.0, 0.0], [0.0, 1.0, 0.0], [0.0; 3]);
        s.add([0.0, 0.0, 1.0], [0.0, 0.0, 1.0], [0.0; 3]);
        let mut ep = ExpandingPolytope::from_simplex(&s);
        let expander = EpaExpander::new(32, 1e-6);
        let support = |d: [f64; 3]| (scale3(norm3(d), 1.0), norm3(d), [0.0; 3]);
        let result = expander.expand(&mut ep, support);
        assert!(result.depth >= 0.0);
    }

    #[test]
    fn test_simplex_manager_prune() {
        let mut s = SimplexManager::new();
        for i in 0..6 {
            s.add([i as f64, 0.0, 0.0], [0.0; 3], [0.0; 3]);
        }
        s.prune();
        assert!(s.size() <= 4);
    }
}