Skip to main content

alice_physics/
collider.rs

1//! Collision Detection using GJK and EPA
2//!
3//! Deterministic collision detection using fixed-point math.
4//!
5//! # Algorithms
6//!
7//! - **GJK (Gilbert-Johnson-Keerthi)**: Determines if two convex shapes intersect
8//! - **EPA (Expanding Polytope Algorithm)**: Computes penetration depth and normal
9
10use crate::math::{Fix128, Vec3Fix};
11use crate::metric::MetricWeights;
12
13#[cfg(not(feature = "std"))]
14use alloc::vec::Vec;
15
16// ============================================================================
17// Collision Result
18// ============================================================================
19
20/// Result of a collision detection query
21#[derive(Clone, Copy, Debug, PartialEq, Eq)]
22pub struct CollisionResult {
23    /// Whether the shapes are colliding
24    pub colliding: bool,
25    /// Penetration depth (if colliding)
26    pub depth: Fix128,
27    /// Collision normal, pointing from B toward A (the same contract as
28    /// [`Contact::normal`]): translating A by `depth · normal` separates the
29    /// shapes. For the plane queries A is the sphere / box and B the plane.
30    pub normal: Vec3Fix,
31    /// Contact point on shape A
32    pub point_a: Vec3Fix,
33    /// Contact point on shape B
34    pub point_b: Vec3Fix,
35}
36
37impl CollisionResult {
38    /// No collision
39    pub const NONE: Self = Self {
40        colliding: false,
41        depth: Fix128::ZERO,
42        normal: Vec3Fix::ZERO,
43        point_a: Vec3Fix::ZERO,
44        point_b: Vec3Fix::ZERO,
45    };
46
47    /// Create a new collision result
48    #[must_use]
49    pub const fn new(depth: Fix128, normal: Vec3Fix, point_a: Vec3Fix, point_b: Vec3Fix) -> Self {
50        Self {
51            colliding: true,
52            depth,
53            normal,
54            point_a,
55            point_b,
56        }
57    }
58}
59
60// ============================================================================
61// Collider Shapes
62// ============================================================================
63
64/// Support function trait for GJK
65pub trait Support {
66    /// Returns the point on the shape furthest in the given direction
67    fn support(&self, direction: Vec3Fix) -> Vec3Fix;
68}
69
70/// Sphere collider
71#[derive(Clone, Copy, Debug, PartialEq, Eq)]
72pub struct Sphere {
73    /// Center position
74    pub center: Vec3Fix,
75    /// Sphere radius
76    pub radius: Fix128,
77}
78
79impl Sphere {
80    /// Create a new sphere from center and radius
81    #[must_use]
82    pub const fn new(center: Vec3Fix, radius: Fix128) -> Self {
83        Self { center, radius }
84    }
85}
86
87impl Support for Sphere {
88    #[inline(always)]
89    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
90        // a tiny direction would normalize to zero (AUD-A-S3W3-018)
91        let dir_norm = direction.rescaled_direction().normalize();
92        self.center + dir_norm * self.radius
93    }
94}
95
96/// Axis-Aligned Bounding Box
97#[derive(Clone, Copy, Debug, PartialEq, Eq)]
98pub struct AABB {
99    /// Minimum corner
100    pub min: Vec3Fix,
101    /// Maximum corner
102    pub max: Vec3Fix,
103}
104
105/// `√(a² + b² + c²)` without forming the squares at their own scale: the
106/// components are divided by the largest magnitude first, so a component below
107/// `2^-32` is not lost to `Fix128` underflow and one above `~3e9` does not
108/// overflow. Exact for a single non-zero component.
109#[must_use]
110pub(crate) fn hypot3(a: Fix128, b: Fix128, c: Fix128) -> Fix128 {
111    let (aa, ab, ac) = (a.abs(), b.abs(), c.abs());
112    let mut m = aa;
113    if ab > m {
114        m = ab;
115    }
116    if ac > m {
117        m = ac;
118    }
119    if m.is_zero() {
120        return Fix128::ZERO;
121    }
122    let (x, y, z) = (aa / m, ab / m, ac / m);
123    m * (x * x + y * y + z * z).sqrt()
124}
125
126impl AABB {
127    /// Create a new AABB from min and max corners
128    #[must_use]
129    pub const fn new(min: Vec3Fix, max: Vec3Fix) -> Self {
130        Self { min, max }
131    }
132
133    /// Create AABB from center and half-extents
134    #[must_use]
135    pub fn from_center_half(center: Vec3Fix, half: Vec3Fix) -> Self {
136        Self {
137            min: center - half,
138            max: center + half,
139        }
140    }
141
142    /// The tight box of the ball `{x : g(x − center) ≤ radius}` in the
143    /// metric `metric`.
144    ///
145    /// The half-width is [`MetricWeights::axis_extent`], which is exact:
146    /// a cube-metric ball of radius `r` gives exactly `[−r, r]³`, and an
147    /// octahedral one the same box with the ball inscribed in it. For the
148    /// Euclidean metric this is `from_center_half(center, (r, r, r))`.
149    #[must_use]
150    pub fn from_metric_ball(center: Vec3Fix, radius: Fix128, metric: MetricWeights) -> Self {
151        let e = metric.axis_extent(radius);
152        Self::from_center_half(center, Vec3Fix::new(e, e, e))
153    }
154
155    /// Check if two AABBs intersect (broad phase)
156    #[inline]
157    #[must_use]
158    pub fn intersects(&self, other: &Self) -> bool {
159        self.min.x <= other.max.x
160            && self.max.x >= other.min.x
161            && self.min.y <= other.max.y
162            && self.max.y >= other.min.y
163            && self.min.z <= other.max.z
164            && self.max.z >= other.min.z
165    }
166
167    /// Compute union of two AABBs
168    #[must_use]
169    pub fn union(&self, other: &Self) -> Self {
170        Self {
171            min: Vec3Fix::new(
172                if self.min.x < other.min.x {
173                    self.min.x
174                } else {
175                    other.min.x
176                },
177                if self.min.y < other.min.y {
178                    self.min.y
179                } else {
180                    other.min.y
181                },
182                if self.min.z < other.min.z {
183                    self.min.z
184                } else {
185                    other.min.z
186                },
187            ),
188            max: Vec3Fix::new(
189                if self.max.x > other.max.x {
190                    self.max.x
191                } else {
192                    other.max.x
193                },
194                if self.max.y > other.max.y {
195                    self.max.y
196                } else {
197                    other.max.y
198                },
199                if self.max.z > other.max.z {
200                    self.max.z
201                } else {
202                    other.max.z
203                },
204            ),
205        }
206    }
207
208    /// Surface area (for BVH heuristics)
209    #[must_use]
210    pub fn surface_area(&self) -> Fix128 {
211        let d = self.max - self.min;
212        let two = Fix128::from_int(2);
213        two * (d.x * d.y + d.y * d.z + d.z * d.x)
214    }
215}
216
217impl Support for AABB {
218    #[inline(always)]
219    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
220        Vec3Fix::new(
221            if direction.x >= Fix128::ZERO {
222                self.max.x
223            } else {
224                self.min.x
225            },
226            if direction.y >= Fix128::ZERO {
227                self.max.y
228            } else {
229                self.min.y
230            },
231            if direction.z >= Fix128::ZERO {
232                self.max.z
233            } else {
234                self.min.z
235            },
236        )
237    }
238}
239
240/// Convex hull (array of vertices)
241#[derive(Clone, Debug)]
242pub struct ConvexHull {
243    /// Hull vertices
244    pub vertices: Vec<Vec3Fix>,
245}
246
247impl ConvexHull {
248    /// Create a new convex hull from vertices.
249    ///
250    /// # Panics
251    ///
252    /// Panics if `vertices` is empty, since `support()` requires at least one vertex.
253    #[must_use]
254    pub fn new(vertices: Vec<Vec3Fix>) -> Self {
255        assert!(
256            !vertices.is_empty(),
257            "ConvexHull requires at least one vertex"
258        );
259        Self { vertices }
260    }
261}
262
263impl Support for ConvexHull {
264    #[inline(always)]
265    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
266        let mut best = self.vertices[0];
267        let mut best_dot = best.dot(direction);
268
269        for &v in &self.vertices[1..] {
270            let d = v.dot(direction);
271            if d > best_dot {
272                best = v;
273                best_dot = d;
274            }
275        }
276
277        best
278    }
279}
280
281/// Capsule (line segment with radius)
282#[derive(Clone, Copy, Debug, PartialEq, Eq)]
283pub struct Capsule {
284    /// Start point of the capsule segment
285    pub a: Vec3Fix,
286    /// End point of the capsule segment
287    pub b: Vec3Fix,
288    /// Capsule radius
289    pub radius: Fix128,
290}
291
292impl Capsule {
293    /// Create a new capsule from two endpoints and a radius
294    #[must_use]
295    pub const fn new(a: Vec3Fix, b: Vec3Fix, radius: Fix128) -> Self {
296        Self { a, b, radius }
297    }
298}
299
300impl Support for Capsule {
301    #[inline(always)]
302    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
303        let direction = direction.rescaled_direction();
304        let da = self.a.dot(direction);
305        let db = self.b.dot(direction);
306        let base = if da > db { self.a } else { self.b };
307        base + direction.normalize() * self.radius
308    }
309}
310
311/// Uniformly scaled shape wrapper
312///
313/// Wraps any `Support`-implementing shape with a uniform scale factor. The
314/// scale is about the world origin, not about the inner shape's centre: a
315/// sphere centred at `c` with radius `r` scaled by `s > 0` is the sphere
316/// centred at `s·c` with radius `s·r`.
317///
318/// The support in `d` is `s · inner.support(d)` for `s >= 0`. A negative
319/// scale mirrors the shape through the origin, whose furthest point in `d`
320/// is `s · inner.support(-d)`.
321#[derive(Clone, Copy, Debug, PartialEq, Eq)]
322pub struct ScaledShape<S> {
323    /// Inner shape
324    pub shape: S,
325    /// Uniform scale factor
326    pub scale: Fix128,
327}
328
329impl<S> ScaledShape<S> {
330    /// Create a new scaled shape
331    pub const fn new(shape: S, scale: Fix128) -> Self {
332        Self { shape, scale }
333    }
334}
335
336impl<S: Support> Support for ScaledShape<S> {
337    #[inline(always)]
338    fn support(&self, direction: Vec3Fix) -> Vec3Fix {
339        // s·x maximises <s·x, d> = <x, s·d>: for s < 0 that is the inner
340        // point furthest in -d (AUD-A-S3W3-017)
341        if self.scale.is_negative() {
342            self.shape.support(-direction) * self.scale
343        } else {
344            self.shape.support(direction) * self.scale
345        }
346    }
347}
348
349// ============================================================================
350// GJK Algorithm
351// ============================================================================
352
353/// Minkowski difference support function
354#[inline(always)]
355fn minkowski_support<A: Support, B: Support>(a: &A, b: &B, direction: Vec3Fix) -> Vec3Fix {
356    a.support(direction) - b.support(-direction)
357}
358
359/// Simplex for GJK (up to 4 points in 3D)
360#[derive(Clone, Debug)]
361struct Simplex {
362    points: [Vec3Fix; 4],
363    size: usize,
364}
365
366impl Simplex {
367    const fn new() -> Self {
368        Self {
369            points: [Vec3Fix::ZERO; 4],
370            size: 0,
371        }
372    }
373
374    fn push(&mut self, point: Vec3Fix) {
375        // Shift existing points
376        for i in (1..4).rev() {
377            self.points[i] = self.points[i - 1];
378        }
379        self.points[0] = point;
380        self.size = (self.size + 1).min(4);
381    }
382
383    fn set(&mut self, points: &[Vec3Fix]) {
384        for (i, &p) in points.iter().enumerate().take(4) {
385            self.points[i] = p;
386        }
387        self.size = points.len().min(4);
388    }
389}
390
391/// GJK collision result
392#[derive(Clone, Copy, Debug, PartialEq, Eq)]
393pub struct GjkResult {
394    /// Whether the two shapes are colliding
395    pub colliding: bool,
396    /// Closest point on the Minkowski difference boundary
397    pub closest_point: Vec3Fix,
398}
399
400/// GJK algorithm for collision detection
401///
402/// Returns true if the two shapes are intersecting.
403/// Deterministic: fixed iteration count.
404pub fn gjk<A: Support, B: Support>(a: &A, b: &B) -> GjkResult {
405    gjk_core(a, b).0
406}
407
408/// The GJK loop, also returning the simplex it stopped on: when the shapes
409/// collide, a tetrahedron around (or, for touching shapes, on) the origin that
410/// [`epa`] can expand — or a lower-dimensional simplex when the origin lies on one.
411fn gjk_core<A: Support, B: Support>(a: &A, b: &B) -> (GjkResult, Simplex) {
412    const MAX_ITERATIONS: usize = 64;
413
414    // Initial direction
415    let mut direction = Vec3Fix::UNIT_X;
416
417    // Get initial support point
418    let mut simplex = Simplex::new();
419    simplex.push(minkowski_support(a, b, direction));
420
421    // Direction toward origin
422    direction = -simplex.points[0];
423
424    for _ in 0..MAX_ITERATIONS {
425        if direction.length_squared().is_zero() {
426            // Origin is on the simplex
427            return (
428                GjkResult {
429                    colliding: true,
430                    closest_point: Vec3Fix::ZERO,
431                },
432                simplex,
433            );
434        }
435
436        let new_point = minkowski_support(a, b, direction);
437
438        // Check if we passed the origin
439        if new_point.dot(direction) < Fix128::ZERO {
440            return (
441                GjkResult {
442                    colliding: false,
443                    closest_point: new_point,
444                },
445                simplex,
446            );
447        }
448
449        simplex.push(new_point);
450
451        // Update simplex and direction
452        if do_simplex(&mut simplex, &mut direction) {
453            return (
454                GjkResult {
455                    colliding: true,
456                    closest_point: Vec3Fix::ZERO,
457                },
458                simplex,
459            );
460        }
461    }
462
463    // Assume no collision if max iterations reached
464    let closest_point = simplex.points[0];
465    (
466        GjkResult {
467            colliding: false,
468            closest_point,
469        },
470        simplex,
471    )
472}
473
474/// Whether `p` lies off the line / plane / space spanned by `points` (so adding
475/// it raises the dimension of the simplex).
476fn raises_dimension(points: &[Vec3Fix], p: Vec3Fix) -> bool {
477    match points.len() {
478        1 => p != points[0],
479        2 => {
480            (points[1] - points[0])
481                .cross(p - points[0])
482                .length_squared()
483                > Fix128::ZERO
484        }
485        3 => {
486            (points[1] - points[0])
487                .cross(points[2] - points[0])
488                .dot(p - points[0])
489                != Fix128::ZERO
490        }
491        _ => false,
492    }
493}
494
495/// The contact of two overlapping convex shapes: GJK to decide they overlap, then
496/// EPA on the simplex GJK ended with for the penetration depth and normal.
497///
498/// `None` when the shapes do not overlap, or when they only touch and the
499/// Minkowski difference is too flat to expand (the origin on its boundary with no
500/// room for a tetrahedron). The [`Contact::normal`] points from `b` to `a`, as
501/// everywhere in the crate: translating `a` by `depth · normal` separates them.
502///
503/// EPA is exact when the faces that meet at the answer are flat (boxes,
504/// cylinder caps) and approximates a curved Minkowski sum by a polytope otherwise
505/// (when the fixed iteration count runs out on such a shape, the depth is the
506/// smallest support distance seen, exact for spheres; see [`epa`]).
507///
508/// GJK may end on a point, a segment or a triangle that contains the origin; EPA
509/// needs a tetrahedron, so the simplex is completed with Minkowski-difference
510/// points in fixed directions until it spans space.
511#[must_use]
512pub fn contact<A: Support, B: Support>(a: &A, b: &B) -> Option<Contact> {
513    let (result, simplex) = gjk_core(a, b);
514    if !result.colliding {
515        return None;
516    }
517    let mut points: Vec<Vec3Fix> = simplex.points[..simplex.size].to_vec();
518    if points.is_empty() {
519        points.push(minkowski_support(a, b, Vec3Fix::UNIT_X));
520    }
521    // Fixed order, so the same pair always completes the same way.
522    let directions = [
523        Vec3Fix::UNIT_X,
524        -Vec3Fix::UNIT_X,
525        Vec3Fix::UNIT_Y,
526        -Vec3Fix::UNIT_Y,
527        Vec3Fix::UNIT_Z,
528        -Vec3Fix::UNIT_Z,
529        Vec3Fix::new(Fix128::ONE, Fix128::ONE, Fix128::ONE),
530        Vec3Fix::new(-Fix128::ONE, Fix128::ONE, -Fix128::ONE),
531    ];
532    for direction in directions {
533        if points.len() >= 4 {
534            break;
535        }
536        let p = minkowski_support(a, b, direction);
537        if raises_dimension(&points, p) {
538            points.push(p);
539        }
540    }
541    if points.len() < 4 {
542        return None;
543    }
544    epa(a, b, &points)
545}
546
547/// Process simplex and update direction toward origin
548fn do_simplex(simplex: &mut Simplex, direction: &mut Vec3Fix) -> bool {
549    match simplex.size {
550        2 => do_simplex_line(simplex, direction),
551        3 => do_simplex_triangle(simplex, direction),
552        4 => do_simplex_tetrahedron(simplex, direction),
553        _ => false,
554    }
555}
556
557fn do_simplex_line(simplex: &mut Simplex, direction: &mut Vec3Fix) -> bool {
558    let a = simplex.points[0];
559    let b = simplex.points[1];
560    let ab = b - a;
561    let ao = -a;
562
563    if ab.dot(ao) > Fix128::ZERO {
564        // Origin is between A and B
565        let mut new_dir = ab.cross(ao).cross(ab);
566        // If ab and ao are parallel, the triple cross product is zero.
567        // Pick a perpendicular fallback direction.
568        if new_dir.length_squared().is_zero() {
569            // Choose an axis least aligned with ab for the perpendicular
570            let abs_x = ab.x.abs();
571            let abs_y = ab.y.abs();
572            let abs_z = ab.z.abs();
573            let perp = if abs_x <= abs_y && abs_x <= abs_z {
574                Vec3Fix::UNIT_X
575            } else if abs_y <= abs_z {
576                Vec3Fix::UNIT_Y
577            } else {
578                Vec3Fix::UNIT_Z
579            };
580            new_dir = ab.cross(perp);
581        }
582        *direction = new_dir;
583    } else {
584        // Origin is beyond A
585        simplex.set(&[a]);
586        *direction = ao;
587    }
588
589    false
590}
591
592fn do_simplex_triangle(simplex: &mut Simplex, direction: &mut Vec3Fix) -> bool {
593    let a = simplex.points[0];
594    let b = simplex.points[1];
595    let c = simplex.points[2];
596
597    let ab = b - a;
598    let ac = c - a;
599    let ao = -a;
600
601    let abc = ab.cross(ac);
602
603    if abc.cross(ac).dot(ao) > Fix128::ZERO {
604        if ac.dot(ao) > Fix128::ZERO {
605            simplex.set(&[a, c]);
606            *direction = ac.cross(ao).cross(ac);
607        } else {
608            simplex.set(&[a, b]);
609            return do_simplex_line(simplex, direction);
610        }
611    } else if ab.cross(abc).dot(ao) > Fix128::ZERO {
612        simplex.set(&[a, b]);
613        return do_simplex_line(simplex, direction);
614    } else if abc.dot(ao) > Fix128::ZERO {
615        *direction = abc;
616    } else {
617        simplex.set(&[a, c, b]);
618        *direction = -abc;
619    }
620
621    false
622}
623
624fn do_simplex_tetrahedron(simplex: &mut Simplex, direction: &mut Vec3Fix) -> bool {
625    let a = simplex.points[0];
626    let b = simplex.points[1];
627    let c = simplex.points[2];
628    let d = simplex.points[3];
629
630    let ab = b - a;
631    let ac = c - a;
632    let ad = d - a;
633    let ao = -a;
634
635    let abc = ab.cross(ac);
636    let acd = ac.cross(ad);
637    let adb = ad.cross(ab);
638
639    if abc.dot(ao) > Fix128::ZERO {
640        simplex.set(&[a, b, c]);
641        return do_simplex_triangle(simplex, direction);
642    }
643
644    if acd.dot(ao) > Fix128::ZERO {
645        simplex.set(&[a, c, d]);
646        return do_simplex_triangle(simplex, direction);
647    }
648
649    if adb.dot(ao) > Fix128::ZERO {
650        simplex.set(&[a, d, b]);
651        return do_simplex_triangle(simplex, direction);
652    }
653
654    // Origin is inside the tetrahedron
655    true
656}
657
658// ============================================================================
659// EPA Algorithm (Expanding Polytope Algorithm)
660// ============================================================================
661
662/// Contact information from EPA
663#[derive(Clone, Copy, Debug, PartialEq, Eq)]
664pub struct Contact {
665    /// Penetration depth
666    pub depth: Fix128,
667    /// Contact normal (pointing from B to A)
668    pub normal: Vec3Fix,
669    /// Contact point on A
670    pub point_a: Vec3Fix,
671    /// Contact point on B
672    pub point_b: Vec3Fix,
673}
674
675/// EPA face (triangle)
676#[derive(Clone, Copy, Debug, PartialEq)]
677struct EpaFace {
678    indices: [usize; 3],
679    normal: Vec3Fix,
680    distance: Fix128,
681}
682
683/// EPA algorithm for penetration depth
684///
685/// Call this after GJK returns a collision; `initial_simplex` must be a
686/// tetrahedron of Minkowski-difference points that encloses the origin.
687/// Deterministic: fixed iteration count. When the budget runs out before the
688/// nearest face stops moving (curved shapes), the smallest support distance
689/// seen, an upper bound that is exact for spheres, is returned with its
690/// direction rather than `None`.
691///
692/// The returned [`Contact::normal`] points from B to A (the crate-wide
693/// contact contract): translating A by `depth · normal` separates the
694/// shapes. `point_a` is the deepest point of A inside B and `point_b` the
695/// deepest point of B inside A. Before 1.2.0 the normal was the outward
696/// face normal of A⊖B, i.e. A→B, the opposite of the documented sign, and
697/// faces through the origin (touching shapes) kept their vertex winding,
698/// so a touching pair could report the far side of the polytope.
699pub fn epa<A: Support, B: Support>(a: &A, b: &B, initial_simplex: &[Vec3Fix]) -> Option<Contact> {
700    const MAX_ITERATIONS: usize = 64;
701    const EPSILON: Fix128 = Fix128 {
702        hi: 0,
703        lo: 0x0001000000000000,
704    }; // Small threshold
705
706    if initial_simplex.len() < 4 {
707        return None;
708    }
709
710    let mut vertices: Vec<Vec3Fix> = initial_simplex.to_vec();
711    let mut faces: Vec<EpaFace> = Vec::with_capacity(64);
712
713    // Interior reference point: the centroid of the initial tetrahedron
714    // stays inside the polytope as it grows (convex), so every face normal
715    // can be oriented away from it — independent of vertex winding and of
716    // whether the origin lies exactly on the face.
717    let interior =
718        (vertices[0] + vertices[1] + vertices[2] + vertices[3]) * Fix128::from_ratio(1, 4);
719
720    // Initialize with tetrahedron faces
721    add_face(&mut faces, &vertices, interior, 0, 1, 2);
722    add_face(&mut faces, &vertices, interior, 0, 3, 1);
723    add_face(&mut faces, &vertices, interior, 0, 2, 3);
724    add_face(&mut faces, &vertices, interior, 1, 3, 2);
725
726    // The smallest support distance `h(n) = support(n) · n` seen so far, with its
727    // direction: every `h(n)` bounds the penetration depth from above, as every
728    // face distance bounds it from below
729    let mut best_upper: Option<(Fix128, Vec3Fix)> = None;
730
731    for _ in 0..MAX_ITERATIONS {
732        // Find face closest to origin
733        let (_closest_idx, closest_face) = faces
734            .iter()
735            .enumerate()
736            .min_by(|(_, a), (_, b)| a.distance.cmp(&b.distance))
737            .map(|(i, f)| (i, *f))?;
738
739        // Get support point in face normal direction
740        let support = minkowski_support(a, b, closest_face.normal);
741        let distance = support.dot(closest_face.normal);
742        if best_upper.is_none_or(|(h, _)| distance < h) {
743            best_upper = Some((distance, closest_face.normal));
744        }
745
746        // Check for convergence
747        if distance - closest_face.distance < EPSILON {
748            // Found the closest face: its outward normal is the A→B
749            // penetration direction, so the B→A contact normal is its negation
750            let outward = closest_face.normal;
751            let depth = closest_face.distance;
752
753            // Deepest points (simplified): A furthest along A→B, B along B→A
754            let point_a = a.support(outward);
755            let point_b = b.support(-outward);
756
757            return Some(Contact {
758                depth,
759                normal: -outward,
760                point_a,
761                point_b,
762            });
763        }
764
765        // Add new vertex
766        let new_idx = vertices.len();
767        vertices.push(support);
768
769        // Remove faces visible from new vertex and add new faces
770        let mut edges: Vec<(usize, usize)> = Vec::new();
771
772        faces.retain(|face| {
773            let v = vertices[face.indices[0]];
774            let to_new = support - v;
775
776            if face.normal.dot(to_new) > Fix128::ZERO {
777                // Face is visible, collect edges
778                for i in 0..3 {
779                    let edge = (face.indices[i], face.indices[(i + 1) % 3]);
780                    // Check if edge already exists (shared edge)
781                    if let Some(pos) = edges.iter().position(|&e| e == (edge.1, edge.0)) {
782                        edges.remove(pos);
783                    } else {
784                        edges.push(edge);
785                    }
786                }
787                false
788            } else {
789                true
790            }
791        });
792
793        // Add new faces from edges
794        for (i, j) in edges {
795            add_face(&mut faces, &vertices, interior, i, j, new_idx);
796        }
797    }
798
799    // The iteration budget ran out before the polytope's nearest face stopped
800    // moving — what a curved Minkowski sum (a sphere, a cylinder's rim, an
801    // ellipsoid) does at any fixed budget. The nearest face's distance is then
802    // a lower bound that can still be several percent short on a sphere (the
803    // polytope inscribed in it converges slowly); the smallest support
804    // distance seen is the upper bound, exact along any direction for a sphere
805    // and second order in the angle for smooth shapes, so it is the answer
806    let (depth, outward) = match best_upper {
807        Some(found) => found,
808        None => {
809            let nearest = faces.iter().min_by(|a, b| a.distance.cmp(&b.distance))?;
810            (nearest.distance, nearest.normal)
811        }
812    };
813    Some(Contact {
814        depth,
815        normal: -outward,
816        point_a: a.support(outward),
817        point_b: b.support(-outward),
818    })
819}
820
821fn add_face(
822    faces: &mut Vec<EpaFace>,
823    vertices: &[Vec3Fix],
824    interior: Vec3Fix,
825    i: usize,
826    j: usize,
827    k: usize,
828) {
829    let a = vertices[i];
830    let b = vertices[j];
831    let c = vertices[k];
832
833    let ab = b - a;
834    let ac = c - a;
835    let cross = ab.cross(ac);
836
837    // Skip degenerate faces where vertices are collinear or coincident
838    if cross.length_squared().is_zero() {
839        return;
840    }
841
842    let normal = cross.normalize();
843
844    // Orient the normal away from the polytope interior (not away from the
845    // origin: a face through the origin has distance 0 either way and the
846    // origin gives no orientation)
847    let normal = if (a - interior).dot(normal) < Fix128::ZERO {
848        -normal
849    } else {
850        normal
851    };
852    let distance = a.dot(normal);
853
854    faces.push(EpaFace {
855        indices: [i, j, k],
856        normal,
857        distance,
858    });
859}
860
861#[cfg(test)]
862mod tests {
863    fn p(x: i64, y: i64, z: i64) -> Vec3Fix {
864        Vec3Fix::new(
865            Fix128::from_int(x),
866            Fix128::from_int(y),
867            Fix128::from_int(z),
868        )
869    }
870
871    /// `raises_dimension` says whether a point leaves the line / plane / space the
872    /// simplex spans: a repeated point, a point on the line, a point in the plane do
873    /// not; one off them does.
874    #[test]
875    fn raises_dimension_is_true_only_off_the_span() {
876        // one point: another point raises, the same point does not
877        assert!(raises_dimension(&[p(1, 2, 3)], p(1, 2, 4)));
878        assert!(!raises_dimension(&[p(1, 2, 3)], p(1, 2, 3)));
879        // a segment along x: a point on the x line (even far away) does not, one off it does
880        let line = [p(0, 0, 0), p(1, 0, 0)];
881        assert!(!raises_dimension(&line, p(5, 0, 0)));
882        assert!(!raises_dimension(&line, p(-3, 0, 0)));
883        assert!(raises_dimension(&line, p(0, 1, 0)));
884        // a triangle in z = 0: a point in the plane does not, one above or below does
885        let plane = [p(0, 0, 0), p(1, 0, 0), p(0, 1, 0)];
886        assert!(!raises_dimension(&plane, p(3, 4, 0)));
887        assert!(raises_dimension(&plane, p(0, 0, 1)));
888        assert!(raises_dimension(&plane, p(0, 0, -2)));
889        // four points already span space
890        assert!(!raises_dimension(
891            &[p(0, 0, 0), p(1, 0, 0), p(0, 1, 0), p(0, 0, 1)],
892            p(2, 2, 2)
893        ));
894    }
895
896    use super::*;
897    #[cfg(not(feature = "std"))]
898    use alloc::vec;
899
900    #[test]
901    fn test_aabb_intersection() {
902        let a = AABB::new(Vec3Fix::from_int(0, 0, 0), Vec3Fix::from_int(2, 2, 2));
903        let b = AABB::new(Vec3Fix::from_int(1, 1, 1), Vec3Fix::from_int(3, 3, 3));
904        let c = AABB::new(Vec3Fix::from_int(5, 5, 5), Vec3Fix::from_int(6, 6, 6));
905
906        assert!(a.intersects(&b), "a and b should intersect");
907        assert!(!a.intersects(&c), "a and c should not intersect");
908    }
909
910    #[test]
911    fn test_sphere_support() {
912        let sphere = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
913
914        let support = sphere.support(Vec3Fix::UNIT_X);
915        assert_eq!(support.x.hi, 1);
916        assert!(support.y.is_zero());
917        assert!(support.z.is_zero());
918    }
919
920    #[test]
921    fn test_gjk_spheres_colliding() {
922        let a = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
923        let b = Sphere::new(
924            Vec3Fix::new(Fix128::ONE, Fix128::ZERO, Fix128::ZERO),
925            Fix128::ONE,
926        );
927
928        let result = gjk(&a, &b);
929        assert!(result.colliding, "Overlapping spheres should collide");
930    }
931
932    #[test]
933    fn test_gjk_spheres_not_colliding() {
934        let a = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
935        let b = Sphere::new(
936            Vec3Fix::new(Fix128::from_int(5), Fix128::ZERO, Fix128::ZERO),
937            Fix128::ONE,
938        );
939
940        let result = gjk(&a, &b);
941        assert!(!result.colliding, "Separated spheres should not collide");
942    }
943
944    #[test]
945    fn test_aabb_support() {
946        let aabb = AABB::new(Vec3Fix::from_int(-1, -1, -1), Vec3Fix::from_int(1, 1, 1));
947
948        let support = aabb.support(Vec3Fix::UNIT_X);
949        assert_eq!(support.x.hi, 1);
950    }
951
952    #[test]
953    fn test_scaled_shape() {
954        let sphere = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
955        let scaled = ScaledShape::new(sphere, Fix128::from_int(3));
956
957        let support = scaled.support(Vec3Fix::UNIT_X);
958        assert_eq!(support.x.hi, 3, "Scaled sphere support should be at 3");
959    }
960
961    #[test]
962    fn test_scaled_collision() {
963        let a = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
964        let b = Sphere::new(Vec3Fix::from_int(4, 0, 0), Fix128::ONE);
965
966        // Unscaled: distance=4, combined radius=2 → no collision
967        let result = gjk(&a, &b);
968        assert!(!result.colliding, "Unscaled spheres should not collide");
969
970        // Scaled: scale=3, effective radius=3, combined=3+1=4 → overlap at boundary
971        let scaled_a = ScaledShape::new(a, Fix128::from_int(3));
972        let result = gjk(&scaled_a, &b);
973        assert!(result.colliding, "Scaled sphere should collide");
974    }
975
976    // ---- mutation-score tests (2026-09-15) ----------------------------
977
978    fn fi(n: i64) -> Fix128 {
979        Fix128::from_int(n)
980    }
981
982    fn v3i(x: i64, y: i64, z: i64) -> Vec3Fix {
983        Vec3Fix::from_int(x, y, z)
984    }
985
986    fn near(a: Fix128, b: Fix128) -> bool {
987        (a - b).abs() < Fix128 { hi: 0, lo: 1 << 24 }
988    }
989
990    #[test]
991    fn aabb_from_center_half_union_and_surface_area_exact() {
992        let a = AABB::from_center_half(v3i(1, 2, 3), v3i(1, 2, 3));
993        assert_eq!(a.min, v3i(0, 0, 0));
994        assert_eq!(a.max, v3i(2, 4, 6));
995        // 2 (xy + yz + zx) = 2 (8 + 24 + 12) = 88
996        assert_eq!(a.surface_area(), fi(88));
997        let b = AABB::new(v3i(-5, 1, 2), v3i(1, 3, 9));
998        // union: min は成分ごとの小さい方、max は大きい方 (各軸で self / other が勝つ組合せを混ぜる)
999        let u = a.union(&b);
1000        assert_eq!(u.min, v3i(-5, 0, 0));
1001        assert_eq!(u.max, v3i(2, 4, 9));
1002        assert_eq!(b.union(&a), u, "対称");
1003        assert_eq!(a.union(&a), a);
1004        // surface_area の 3 項が独立に効く: 各軸を伸ばすと増分が閉形式
1005        let base = AABB::new(Vec3Fix::ZERO, v3i(1, 2, 3)); // 2(2+6+3) = 22
1006        assert_eq!(base.surface_area(), fi(22));
1007        assert_eq!(
1008            AABB::new(Vec3Fix::ZERO, v3i(2, 2, 3)).surface_area(),
1009            fi(32)
1010        ); // 2(4+6+6)
1011        assert_eq!(
1012            AABB::new(Vec3Fix::ZERO, v3i(1, 3, 3)).surface_area(),
1013            fi(30)
1014        ); // 2(3+9+3)
1015        assert_eq!(
1016            AABB::new(Vec3Fix::ZERO, v3i(1, 2, 4)).surface_area(),
1017            fi(28)
1018        ); // 2(2+8+4)
1019        assert_eq!(
1020            AABB::new(v3i(1, 1, 1), v3i(1, 1, 1)).surface_area(),
1021            Fix128::ZERO
1022        );
1023    }
1024
1025    #[test]
1026    fn aabb_intersects_boundary_semantics() {
1027        let a = AABB::new(Vec3Fix::ZERO, v3i(2, 2, 2));
1028        assert!(a.intersects(&AABB::new(v3i(1, 1, 1), v3i(3, 3, 3))));
1029        assert!(!a.intersects(&AABB::new(v3i(3, 0, 0), v3i(4, 2, 2))));
1030        // 各軸単独で離れている
1031        assert!(!a.intersects(&AABB::new(v3i(0, 3, 0), v3i(2, 4, 2))));
1032        assert!(!a.intersects(&AABB::new(v3i(0, 0, -5), v3i(2, 2, -3))));
1033        let touching = a.intersects(&AABB::new(v3i(2, 0, 0), v3i(3, 2, 2)));
1034        // 接触ちょうどの真偽は実装依存だが、少し重なれば必ず true、少し離れれば必ず false
1035        assert!(a.intersects(&AABB::new(
1036            Vec3Fix::new(fi(2) - Fix128::from_ratio(1, 8), Fix128::ZERO, Fix128::ZERO),
1037            v3i(3, 2, 2)
1038        )));
1039        assert!(!a.intersects(&AABB::new(
1040            Vec3Fix::new(fi(2) + Fix128::from_ratio(1, 8), Fix128::ZERO, Fix128::ZERO),
1041            v3i(3, 2, 2)
1042        )));
1043        let _ = touching;
1044    }
1045
1046    #[test]
1047    fn support_functions_return_extreme_points() {
1048        let aabb = AABB::new(v3i(-1, -2, -3), v3i(4, 5, 6));
1049        assert_eq!(aabb.support(v3i(1, 1, 1)), v3i(4, 5, 6));
1050        assert_eq!(aabb.support(v3i(-1, -1, -1)), v3i(-1, -2, -3));
1051        assert_eq!(aabb.support(v3i(1, -1, 1)), v3i(4, -2, 6));
1052        assert_eq!(aabb.support(v3i(-1, 1, -1)), v3i(-1, 5, -3));
1053        // 成分 0 は `>=` で max 側
1054        assert_eq!(aabb.support(Vec3Fix::ZERO), v3i(4, 5, 6));
1055        let sphere = Sphere::new(v3i(1, 1, 1), fi(2));
1056        assert_eq!(sphere.support(v3i(0, 5, 0)), v3i(1, 3, 1));
1057        assert_eq!(sphere.support(v3i(-3, 0, 0)), v3i(-1, 1, 1));
1058        let capsule = Capsule::new(v3i(0, -2, 0), v3i(0, 2, 0), Fix128::ONE);
1059        assert_eq!(capsule.support(v3i(0, 1, 0)), v3i(0, 3, 0));
1060        assert_eq!(capsule.support(v3i(0, -1, 0)), v3i(0, -3, 0));
1061        assert_eq!(
1062            capsule.support(v3i(1, 0, 0)),
1063            v3i(1, 2, 0),
1064            "da == db は b 側 (`>` false)"
1065        );
1066        assert_eq!(
1067            capsule.support(v3i(4, 3, 0)),
1068            Vec3Fix::new(
1069                Fix128::from_ratio(4, 5),
1070                fi(2) + Fix128::from_ratio(3, 5),
1071                Fix128::ZERO
1072            )
1073        );
1074        let hull = ConvexHull::new(vec![v3i(0, 0, 0), v3i(3, 0, 0), v3i(0, 4, 0), v3i(0, 0, 5)]);
1075        assert_eq!(hull.support(v3i(1, 0, 0)), v3i(3, 0, 0));
1076        assert_eq!(hull.support(v3i(0, 1, 0)), v3i(0, 4, 0));
1077        assert_eq!(hull.support(v3i(0, 0, 1)), v3i(0, 0, 5));
1078        assert_eq!(hull.support(v3i(-1, -1, -1)), v3i(0, 0, 0));
1079        // 同点 (dot が等しい) は最初の頂点 (`>` false)
1080        assert_eq!(hull.support(v3i(0, 0, -1)), v3i(0, 0, 0));
1081    }
1082
1083    #[test]
1084    fn gjk_box_box_and_sphere_box_configurations() {
1085        let a = AABB::new(v3i(-1, -1, -1), v3i(1, 1, 1));
1086        // 重なり (各軸)、接触前後、完全包含、離れている
1087        assert!(
1088            gjk(
1089                &a,
1090                &AABB::new(
1091                    Vec3Fix::new(Fix128::from_ratio(1, 2), fi(-1), fi(-1)),
1092                    v3i(3, 1, 1)
1093                )
1094            )
1095            .colliding
1096        );
1097        assert!(
1098            gjk(
1099                &a,
1100                &AABB::new(
1101                    Vec3Fix::new(fi(-1), Fix128::from_ratio(1, 2), fi(-1)),
1102                    v3i(1, 3, 1)
1103                )
1104            )
1105            .colliding
1106        );
1107        assert!(
1108            gjk(
1109                &a,
1110                &AABB::new(
1111                    Vec3Fix::new(fi(-1), fi(-1), Fix128::from_ratio(1, 2)),
1112                    v3i(1, 1, 3)
1113                )
1114            )
1115            .colliding
1116        );
1117        assert!(!gjk(&a, &AABB::new(v3i(2, -1, -1), v3i(4, 1, 1))).colliding);
1118        assert!(!gjk(&a, &AABB::new(v3i(-1, 2, -1), v3i(1, 4, 1))).colliding);
1119        assert!(!gjk(&a, &AABB::new(v3i(-1, -1, -4), v3i(1, 1, -2))).colliding);
1120        assert!(
1121            gjk(
1122                &a,
1123                &AABB::new(
1124                    Vec3Fix::new(
1125                        Fix128::from_ratio(-1, 4),
1126                        Fix128::from_ratio(-1, 4),
1127                        Fix128::from_ratio(-1, 4)
1128                    ),
1129                    Vec3Fix::new(
1130                        Fix128::from_ratio(1, 4),
1131                        Fix128::from_ratio(1, 4),
1132                        Fix128::from_ratio(1, 4)
1133                    )
1134                )
1135            )
1136            .colliding,
1137            "包含"
1138        );
1139        // 対角方向にずれた箱 (line / triangle / tetrahedron 全経路)
1140        assert!(
1141            gjk(
1142                &a,
1143                &AABB::new(
1144                    Vec3Fix::new(
1145                        Fix128::from_ratio(1, 2),
1146                        Fix128::from_ratio(1, 2),
1147                        Fix128::from_ratio(1, 2)
1148                    ),
1149                    v3i(3, 3, 3)
1150                )
1151            )
1152            .colliding
1153        );
1154        assert!(
1155            !gjk(
1156                &a,
1157                &AABB::new(
1158                    Vec3Fix::new(
1159                        Fix128::from_ratio(3, 2),
1160                        Fix128::from_ratio(3, 2),
1161                        Fix128::from_ratio(3, 2)
1162                    ),
1163                    v3i(3, 3, 3)
1164                )
1165            )
1166            .colliding
1167        );
1168        // 球 vs 箱: 角の外側 (距離 > r) と内側
1169        let corner_out = Sphere::new(v3i(2, 2, 2), Fix128::ONE); // 角 (1,1,1) まで √3 ≈ 1.73 > 1
1170        assert!(!gjk(&a, &corner_out).colliding);
1171        let corner_in = Sphere::new(v3i(2, 2, 2), fi(2));
1172        assert!(gjk(&a, &corner_in).colliding);
1173        let face = Sphere::new(
1174            Vec3Fix::new(fi(2) - Fix128::from_ratio(1, 2), Fix128::ZERO, Fix128::ZERO),
1175            Fix128::ONE,
1176        );
1177        assert!(gjk(&a, &face).colliding);
1178        let face_out = Sphere::new(
1179            Vec3Fix::new(fi(2) + Fix128::from_ratio(1, 2), Fix128::ZERO, Fix128::ZERO),
1180            Fix128::ONE,
1181        );
1182        assert!(!gjk(&a, &face_out).colliding);
1183        // capsule vs 箱
1184        let cap = Capsule::new(v3i(-3, 0, 0), v3i(3, 0, 0), Fix128::from_ratio(1, 2));
1185        assert!(gjk(&a, &cap).colliding);
1186        let cap_far = Capsule::new(v3i(-3, 3, 0), v3i(3, 3, 0), Fix128::from_ratio(1, 2));
1187        assert!(!gjk(&a, &cap_far).colliding);
1188        // 対称
1189        assert_eq!(gjk(&a, &corner_in).colliding, gjk(&corner_in, &a).colliding);
1190    }
1191
1192    /// 2 shape の Minkowski 差の 4 方向 support から EPA 用初期四面体を作る
1193    fn tetra<A: Support, B: Support>(a: &A, b: &B) -> [Vec3Fix; 4] {
1194        let dirs = [v3i(1, 1, 1), v3i(-1, -1, 1), v3i(-1, 1, -1), v3i(1, -1, -1)];
1195        let mut out = [Vec3Fix::ZERO; 4];
1196        for (i, d) in dirs.iter().enumerate() {
1197            out[i] = a.support(*d) - b.support(-*d);
1198        }
1199        out
1200    }
1201
1202    #[test]
1203    fn epa_reports_minimum_penetration_axis_and_depth() {
1204        let a = AABB::new(v3i(-1, -1, -1), v3i(1, 1, 1));
1205        // B が +x 側に 0.5 だけ重なる (y/z は 2 重なる) → 最小侵入 0.5、法線 ±x
1206        let b = AABB::new(
1207            Vec3Fix::new(Fix128::from_ratio(1, 2), fi(-1), fi(-1)),
1208            v3i(3, 1, 1),
1209        );
1210        let c = epa(&a, &b, &tetra(&a, &b)).expect("overlapping boxes");
1211        assert!(
1212            near(c.depth, Fix128::from_ratio(1, 2)),
1213            "depth {:?}",
1214            c.depth
1215        );
1216        assert!(
1217            near(c.normal.x.abs(), Fix128::ONE)
1218                && near(c.normal.y, Fix128::ZERO)
1219                && near(c.normal.z, Fix128::ZERO),
1220            "{:?}",
1221            c.normal
1222        );
1223        // +y 側に 0.25 重なる → depth 0.25、法線 ±y
1224        let by = AABB::new(
1225            Vec3Fix::new(fi(-1), Fix128::from_ratio(3, 4), fi(-1)),
1226            v3i(1, 3, 1),
1227        );
1228        let cy = epa(&a, &by, &tetra(&a, &by)).expect("overlapping boxes");
1229        assert!(
1230            near(cy.depth, Fix128::from_ratio(1, 4)),
1231            "depth {:?}",
1232            cy.depth
1233        );
1234        assert!(
1235            near(cy.normal.y.abs(), Fix128::ONE) && near(cy.normal.x, Fix128::ZERO),
1236            "{:?}",
1237            cy.normal
1238        );
1239        // -z 側に 0.125 重なる
1240        let bz = AABB::new(
1241            v3i(-1, -1, -3),
1242            Vec3Fix::new(fi(1), fi(1), fi(-1) + Fix128::from_ratio(1, 8)),
1243        );
1244        let cz = epa(&a, &bz, &tetra(&a, &bz)).expect("overlapping boxes");
1245        assert!(
1246            near(cz.depth, Fix128::from_ratio(1, 8)),
1247            "depth {:?}",
1248            cz.depth
1249        );
1250        assert!(near(cz.normal.z.abs(), Fix128::ONE), "{:?}", cz.normal);
1251        // 法線は単位長、point_a と point_b は法線方向に depth 離れている
1252        for cc in [c, cy, cz] {
1253            assert!(near(cc.normal.length(), Fix128::ONE));
1254            let sep = (cc.point_a - cc.point_b).dot(cc.normal).abs();
1255            assert!(near(sep, cc.depth), "sep {sep:?} vs depth {:?}", cc.depth);
1256        }
1257        // 四面体未満は None
1258        assert!(epa(&a, &b, &tetra(&a, &b)[..3]).is_none());
1259        assert!(epa(&a, &b, &[]).is_none());
1260    }
1261
1262    // ---- mutation-score tests, round 2 (2026-09-15) -------------------
1263    //
1264    // Each test below names the cargo-mutants mutant it kills (file line of
1265    // the mutated expression) and derives the expected value by hand in a
1266    // comment. The GJK simplex helpers are private, so they are driven
1267    // directly with hand-built simplices; every coordinate is a small
1268    // integer (or a dyadic fraction), so cross / dot products are exact in
1269    // Fix128 and the assertions use exact equality unless stated otherwise.
1270
1271    /// Build a simplex whose `points[i]` is `points[i]` (push in reverse).
1272    fn simplex_of(points: &[Vec3Fix]) -> Simplex {
1273        let mut s = Simplex::new();
1274        for &p in points.iter().rev() {
1275            s.push(p);
1276        }
1277        assert_eq!(s.size, points.len());
1278        s
1279    }
1280
1281    fn near_tol(a: Fix128, b: Fix128, tol: Fix128) -> bool {
1282        (a - b).abs() < tol
1283    }
1284
1285    /// Origin strictly inside a tetrahedron: every sub-tetrahedron obtained by
1286    /// replacing one vertex with the origin has the same signed volume sign.
1287    fn tetra_contains_origin(t: &[Vec3Fix; 4]) -> bool {
1288        fn vol(a: Vec3Fix, b: Vec3Fix, c: Vec3Fix, d: Vec3Fix) -> Fix128 {
1289            (b - a).cross(c - a).dot(d - a)
1290        }
1291        let o = Vec3Fix::ZERO;
1292        let v0 = vol(t[0], t[1], t[2], t[3]);
1293        let parts = [
1294            vol(o, t[1], t[2], t[3]),
1295            vol(t[0], o, t[2], t[3]),
1296            vol(t[0], t[1], o, t[3]),
1297            vol(t[0], t[1], t[2], o),
1298        ];
1299        !v0.is_zero()
1300            && parts
1301                .iter()
1302                .all(|v| !v.is_zero() && v.is_negative() == v0.is_negative())
1303    }
1304
1305    /// Kills `Simplex::set` → `()` (line 333): `set` must overwrite both the
1306    /// points and the size.
1307    #[test]
1308    fn simplex_set_overwrites_points_and_size() {
1309        let mut s = simplex_of(&[v3i(1, 1, 1), v3i(2, 2, 2), v3i(3, 3, 3)]);
1310        assert_eq!(s.size, 3);
1311        s.set(&[v3i(7, 8, 9), v3i(-1, -2, -3)]);
1312        assert_eq!(s.size, 2);
1313        assert_eq!(s.points[0], v3i(7, 8, 9));
1314        assert_eq!(s.points[1], v3i(-1, -2, -3));
1315        s.set(&[v3i(5, 5, 5)]);
1316        assert_eq!(s.size, 1);
1317        assert_eq!(s.points[0], v3i(5, 5, 5));
1318        // More than 4 points are truncated to 4.
1319        s.set(&[v3i(1, 0, 0); 6]);
1320        assert_eq!(s.size, 4);
1321    }
1322
1323    /// Kills `direction = -simplex.points[0]` → `simplex.points[0]` (line 364).
1324    ///
1325    /// A = sphere r=1 at origin, B = sphere r=1 at (5,0,0). A ⊖ B is a sphere
1326    /// of radius 2 centred at (-5,0,0); its closest point to the origin is
1327    /// (-3,0,0). Trace: first support (dir +x) = (1,0,0) - (4,0,0) = (-3,0,0);
1328    /// direction = (3,0,0); support(+x) = (-3,0,0) again, dot = -9 < 0 → not
1329    /// colliding with `closest_point = (-3,0,0)` (all steps exact: the only
1330    /// normalisation is of an axis-aligned vector). With the mutant the second
1331    /// direction is (-3,0,0), the walk goes through (-7,0,0) and the parallel
1332    /// fallback, and the returned point acquires a z component.
1333    #[test]
1334    fn gjk_closest_point_for_separated_spheres_is_exact() {
1335        let a = Sphere::new(Vec3Fix::ZERO, Fix128::ONE);
1336        let b = Sphere::new(v3i(5, 0, 0), Fix128::ONE);
1337        let r = gjk(&a, &b);
1338        assert!(!r.colliding);
1339        assert_eq!(r.closest_point, v3i(-3, 0, 0));
1340    }
1341
1342    /// Kills `new_point.dot(direction) < 0` → `<= 0` (line 378).
1343    ///
1344    /// Shapes that exactly touch put the origin on the boundary of A ⊖ B, so
1345    /// the support in the search direction has dot exactly 0. `AABB::intersects`
1346    /// treats touching as intersecting (`<=`), and GJK must agree.
1347    ///
1348    /// Box A = [-1,1]^3, sphere B centre (-2,1,1) r=1 touches A at the corner
1349    /// (-1,1,1): first support (dir +x) = (1,1,1) - (-3,1,1) = (4,0,0);
1350    /// direction = (-4,0,0); support = (-1,1,1) - (-1,1,1) = (0,0,0), dot = 0.
1351    /// Original: push, line case `ab·ao = 0` keeps only a = origin, direction
1352    /// = 0 → colliding. Mutant returns `colliding: false` at the dot == 0 step.
1353    /// Box B = [-3,-1]^3 touches A at the corner (-1,-1,-1) the same way:
1354    /// first support (4,2,2), second (-1,-1,-1) - (-1,-1,-1) = 0.
1355    #[test]
1356    fn gjk_touching_shapes_report_collision() {
1357        let a = AABB::new(v3i(-1, -1, -1), v3i(1, 1, 1));
1358        let sphere = Sphere::new(v3i(-2, 1, 1), Fix128::ONE);
1359        assert!(gjk(&a, &sphere).colliding, "sphere touching box corner");
1360        let corner = AABB::new(v3i(-3, -3, -3), v3i(-1, -1, -1));
1361        assert!(gjk(&a, &corner).colliding, "box touching box corner");
1362    }
1363
1364    /// Kills `ab.dot(ao) > 0` → `>= 0` (line 419).
1365    ///
1366    /// a = (2,0,0), b = (2,3,0): ab = (0,3,0), ao = (-2,0,0), ab·ao = 0 exactly.
1367    /// The documented side of the boundary is "origin beyond A": the simplex
1368    /// shrinks to [a] and the direction becomes ao = (-2,0,0). The mutant keeps
1369    /// both points and sets direction (ab × ao) × ab = (-18,0,0).
1370    #[test]
1371    fn simplex_line_origin_perpendicular_to_ab_keeps_only_a() {
1372        let a = v3i(2, 0, 0);
1373        let b = v3i(2, 3, 0);
1374        let mut s = simplex_of(&[a, b]);
1375        let mut dir = Vec3Fix::ZERO;
1376        assert!(!do_simplex_line(&mut s, &mut dir));
1377        assert_eq!(s.size, 1);
1378        assert_eq!(s.points[0], a);
1379        assert_eq!(dir, v3i(-2, 0, 0));
1380    }
1381
1382    /// Kills the parallel-fallback axis selection mutants (line 429:
1383    /// `&&` → `||`, `abs_x <= abs_y` → `>`, `abs_x <= abs_z` → `>`; line 431:
1384    /// `abs_y <= abs_z` → `>`).
1385    ///
1386    /// With a = -2·ab and b = -ab the origin lies on the line through a and b
1387    /// (ab ∥ ao, ab·ao = 2|ab|² > 0), so (ab × ao) × ab = 0 and the fallback
1388    /// picks the axis least aligned with ab; new_dir = ab × axis.
1389    ///   ab = (1,2,3): x smallest → UNIT_X → (1,2,3) × (1,0,0) = (0,3,-2)
1390    ///     (mutants `abs_x > abs_y` / `abs_x > abs_z` fall through to UNIT_Y
1391    ///     → (-3,0,1))
1392    ///   ab = (2,3,1): z smallest → UNIT_Z → (2,3,1) × (0,0,1) = (3,-2,0)
1393    ///     (`||` picks UNIT_X → (0,1,-3); `abs_y > abs_z` picks UNIT_Y →
1394    ///     (-1,0,2))
1395    ///   ab = (3,1,2): y smallest → UNIT_Y → (3,1,2) × (0,1,0) = (-2,0,3)
1396    ///     (`abs_y > abs_z` picks UNIT_Z → (1,-3,0))
1397    #[test]
1398    fn simplex_line_parallel_fallback_picks_least_aligned_axis() {
1399        let cases = [
1400            (v3i(1, 2, 3), v3i(0, 3, -2)),
1401            (v3i(2, 3, 1), v3i(3, -2, 0)),
1402            (v3i(3, 1, 2), v3i(-2, 0, 3)),
1403        ];
1404        for (ab, expected) in cases {
1405            let a = -ab - ab;
1406            let b = -ab;
1407            assert!(ab.cross(-a).length_squared().is_zero(), "ab ∥ ao");
1408            let mut s = simplex_of(&[a, b]);
1409            let mut dir = Vec3Fix::ZERO;
1410            assert!(!do_simplex_line(&mut s, &mut dir));
1411            assert_eq!(s.size, 2, "ab = {ab:?}");
1412            assert_eq!(s.points[0], a);
1413            assert_eq!(s.points[1], b);
1414            assert_eq!(dir, expected, "ab = {ab:?}");
1415        }
1416    }
1417
1418    /// Kills `ac = c - a` → `c + a` (line 454) and the `>` → `==` / `<`
1419    /// mutants of lines 459 and 460 (origin in the Voronoi region of edge AC).
1420    ///
1421    /// a = (1,-2,2), b = (4,-2,2), c = (1,3,2): ab = (3,0,0), ac = (0,5,0),
1422    /// ao = (-1,2,-2), abc = ab × ac = (0,0,15).
1423    /// (abc × ac)·ao = (-75,0,0)·ao = 75 > 0 and ac·ao = 10 > 0 → simplex
1424    /// becomes [a, c] and direction = (ac × ao) × ac = (-10,0,5) × (0,5,0)
1425    /// = (-25,0,-50) (∝ (-1,0,-2): from the edge at x=1, z=2 toward the origin).
1426    #[test]
1427    fn simplex_triangle_edge_ac_region() {
1428        let a = v3i(1, -2, 2);
1429        let b = v3i(4, -2, 2);
1430        let c = v3i(1, 3, 2);
1431        let mut s = simplex_of(&[a, b, c]);
1432        let mut dir = Vec3Fix::ZERO;
1433        assert!(!do_simplex_triangle(&mut s, &mut dir));
1434        assert_eq!(s.size, 2);
1435        assert_eq!(s.points[0], a);
1436        assert_eq!(s.points[1], c);
1437        assert_eq!(dir, v3i(-25, 0, -50));
1438    }
1439
1440    /// Kills the `>` → `>=` mutants of `do_simplex_triangle` (lines 459, 460,
1441    /// 467, 470) by putting the origin exactly on each decision plane.
1442    #[test]
1443    fn simplex_triangle_boundary_cases() {
1444        // Line 459: (abc × ac)·ao == 0. a = (0,-2,2), b = (3,-2,2), c = (0,3,2):
1445        // ab = (3,0,0), ac = (0,5,0), ao = (0,2,-2), abc = (0,0,15),
1446        // (abc × ac)·ao = (-75,0,0)·ao = 0; (ab × abc)·ao = (0,-45,0)·ao = -90;
1447        // abc·ao = -30 → last branch: simplex [a, c, b], direction -abc.
1448        // (`>=` enters the first branch and shrinks the simplex to [a, c].)
1449        let (a, b, c) = (v3i(0, -2, 2), v3i(3, -2, 2), v3i(0, 3, 2));
1450        let mut s = simplex_of(&[a, b, c]);
1451        let mut dir = Vec3Fix::ZERO;
1452        assert!(!do_simplex_triangle(&mut s, &mut dir));
1453        assert_eq!(s.size, 3);
1454        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, c, b]);
1455        assert_eq!(dir, v3i(0, 0, -15));
1456
1457        // Line 460: ac·ao == 0 with (abc × ac)·ao > 0. a = (1,0,2), b = (4,0,2),
1458        // c = (1,5,2): ab = (3,0,0), ac = (0,5,0), ao = (-1,0,-2),
1459        // (abc × ac)·ao = (-75,0,0)·ao = 75 > 0, ac·ao = 0 → simplex [a, b]
1460        // then line case: ab·ao = -3 ≤ 0 → [a], direction ao = (-1,0,-2).
1461        // (`>=` keeps [a, c] with size 2.)
1462        let (a, b, c) = (v3i(1, 0, 2), v3i(4, 0, 2), v3i(1, 5, 2));
1463        let mut s = simplex_of(&[a, b, c]);
1464        let mut dir = Vec3Fix::ZERO;
1465        assert!(!do_simplex_triangle(&mut s, &mut dir));
1466        assert_eq!(s.size, 1);
1467        assert_eq!(s.points[0], a);
1468        assert_eq!(dir, v3i(-1, 0, -2));
1469
1470        // Line 467: (ab × abc)·ao == 0. a = (-2,0,2), b = (3,0,2), c = (-2,4,2):
1471        // ab = (5,0,0), ac = (0,4,0), ao = (2,0,-2), abc = (0,0,20),
1472        // (abc × ac)·ao = (-80,0,0)·ao = -160; (ab × abc)·ao = (0,-100,0)·ao = 0;
1473        // abc·ao = -40 → [a, c, b], direction (0,0,-20).
1474        // (`>=` goes to the line case with [a, b] and direction (0,0,-50).)
1475        let (a, b, c) = (v3i(-2, 0, 2), v3i(3, 0, 2), v3i(-2, 4, 2));
1476        let mut s = simplex_of(&[a, b, c]);
1477        let mut dir = Vec3Fix::ZERO;
1478        assert!(!do_simplex_triangle(&mut s, &mut dir));
1479        assert_eq!(s.size, 3);
1480        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, c, b]);
1481        assert_eq!(dir, v3i(0, 0, -20));
1482
1483        // Line 470: abc·ao == 0 (origin in the triangle plane, inside the
1484        // triangle). a = (-2,-2,0), b = (4,-2,0), c = (-2,4,0): ab = (6,0,0),
1485        // ac = (0,6,0), ao = (2,2,0), abc = (0,0,36), (abc × ac)·ao = -432,
1486        // (ab × abc)·ao = -432, abc·ao = 0 → [a, c, b], direction (0,0,-36).
1487        // (`>=` keeps [a, b, c] and sets direction (0,0,36).)
1488        let (a, b, c) = (v3i(-2, -2, 0), v3i(4, -2, 0), v3i(-2, 4, 0));
1489        let mut s = simplex_of(&[a, b, c]);
1490        let mut dir = Vec3Fix::ZERO;
1491        assert!(!do_simplex_triangle(&mut s, &mut dir));
1492        assert_eq!(s.size, 3);
1493        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, c, b]);
1494        assert_eq!(dir, v3i(0, 0, -36));
1495    }
1496
1497    /// Kills line 467 `>` → `==`, line 470 `>` → `==` / `<`, and line 474
1498    /// `-abc` → `abc`.
1499    #[test]
1500    fn simplex_triangle_edge_ab_region_and_above_below_plane() {
1501        // Edge AB Voronoi region. a = (-2,1,2), b = (3,1,2), c = (-2,5,2):
1502        // ab = (5,0,0), ac = (0,4,0), ao = (2,-1,-2), abc = (0,0,20),
1503        // (abc × ac)·ao = (-80,0,0)·ao = -160; (ab × abc)·ao = (0,-100,0)·ao
1504        // = 100 > 0 → [a, b] then line case: ab·ao = 10 > 0,
1505        // (ab × ao) × ab = (0,10,-5) × (5,0,0) = (0,-25,-50).
1506        // (`==` falls through to the last branch: size 3, direction (0,0,-20).)
1507        let (a, b, c) = (v3i(-2, 1, 2), v3i(3, 1, 2), v3i(-2, 5, 2));
1508        let mut s = simplex_of(&[a, b, c]);
1509        let mut dir = Vec3Fix::ZERO;
1510        assert!(!do_simplex_triangle(&mut s, &mut dir));
1511        assert_eq!(s.size, 2);
1512        assert_eq!([s.points[0], s.points[1]], [a, b]);
1513        assert_eq!(dir, v3i(0, -25, -50));
1514
1515        // Origin above the triangle (abc·ao > 0). a = (-2,-2,-2), b = (4,-2,-2),
1516        // c = (-2,4,-2): abc = (6,0,0) × (0,6,0) = (0,0,36), ao = (2,2,2),
1517        // (abc × ac)·ao = (-216,0,0)·ao = -432, (ab × abc)·ao = (0,-216,0)·ao
1518        // = -432, abc·ao = 72 > 0 → simplex unchanged, direction abc.
1519        // (`==` / `<` take the last branch: [a, c, b], direction (0,0,-36).)
1520        let (a, b, c) = (v3i(-2, -2, -2), v3i(4, -2, -2), v3i(-2, 4, -2));
1521        let mut s = simplex_of(&[a, b, c]);
1522        let mut dir = Vec3Fix::ZERO;
1523        assert!(!do_simplex_triangle(&mut s, &mut dir));
1524        assert_eq!(s.size, 3);
1525        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, b, c]);
1526        assert_eq!(dir, v3i(0, 0, 36));
1527
1528        // Origin below the triangle (abc·ao < 0): same triangle lifted to z = 2,
1529        // ao = (2,2,-2), abc·ao = -72 → winding flipped to [a, c, b] and
1530        // direction = -abc = (0,0,-36). (Deleting the `-` yields (0,0,36).)
1531        let (a, b, c) = (v3i(-2, -2, 2), v3i(4, -2, 2), v3i(-2, 4, 2));
1532        let mut s = simplex_of(&[a, b, c]);
1533        let mut dir = Vec3Fix::ZERO;
1534        assert!(!do_simplex_triangle(&mut s, &mut dir));
1535        assert_eq!(s.size, 3);
1536        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, c, b]);
1537        assert_eq!(dir, v3i(0, 0, -36));
1538    }
1539
1540    /// Kills `do_simplex_tetrahedron` → `true` (line 481) and the `>` → `==` /
1541    /// `>=` mutants of lines 495 (face abc) and 505 (face adb).
1542    ///
1543    /// Base tetrahedron (apex a above a base triangle b, c, d, wound so that
1544    /// ab × ac, ac × ad, ad × ab all point outward):
1545    ///   a = (0,0,4), b = (-3,-3,0), c = (3,-3,0), d = (0,3,0)
1546    ///   abc = (0,-24,18), acd = (24,12,9), adb = (-24,12,9)
1547    /// The tetrahedron is translated so that the origin lands in the wanted
1548    /// region; translation does not change the face normals.
1549    #[test]
1550    fn simplex_tetrahedron_face_regions_and_containment() {
1551        let base = [v3i(0, 0, 4), v3i(-3, -3, 0), v3i(3, -3, 0), v3i(0, 3, 0)];
1552        let shifted = |t: Vec3Fix| [base[0] + t, base[1] + t, base[2] + t, base[3] + t];
1553
1554        // Origin outside face abc: shift by (0,4,0) → a = (0,4,4), ao = (0,-4,-4),
1555        // abc·ao = 96 - 72 = 24 > 0 → [a, b, c] handed to the triangle case,
1556        // which (both edge tests negative, abc·ao = 24 > 0) keeps the triangle
1557        // and sets direction abc = (0,-24,18). Return value false.
1558        // (`→ true` returns true; `==` skips to acd·ao = -84, adb·ao = -84 and
1559        // returns true.)
1560        let [a, b, c, d] = shifted(v3i(0, 4, 0));
1561        let mut s = simplex_of(&[a, b, c, d]);
1562        let mut dir = Vec3Fix::ZERO;
1563        assert!(!do_simplex_tetrahedron(&mut s, &mut dir));
1564        assert_eq!(s.size, 3);
1565        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, b, c]);
1566        assert_eq!(dir, v3i(0, -24, 18));
1567
1568        // Origin exactly on face abc: shift by (0,3,0) → a = (0,3,4),
1569        // ao = (0,-3,-4), abc·ao = 72 - 72 = 0, acd·ao = -72, adb·ao = -72
1570        // → origin counts as inside (true, simplex untouched).
1571        // (`>=` hands [a, b, c] to the triangle case and returns false.)
1572        let [a, b, c, d] = shifted(v3i(0, 3, 0));
1573        let mut s = simplex_of(&[a, b, c, d]);
1574        let mut dir = v3i(9, 9, 9);
1575        assert!(do_simplex_tetrahedron(&mut s, &mut dir));
1576        assert_eq!(s.size, 4);
1577        assert_eq!(dir, v3i(9, 9, 9));
1578
1579        // Origin outside face adb: shift by (3,0,0) → a = (3,0,4), ao = (-3,0,-4),
1580        // abc·ao = -72, acd·ao = -72 - 36 = -108, adb·ao = 72 - 36 = 36 > 0
1581        // → [a, d, b] handed to the triangle case: with b' = d, c' = b the
1582        // triangle normal is ab' × ac' = adb = (-24,12,9); (abc' × ac')·ao =
1583        // (-21,-123,108)·ao = -369, (ab' × abc')·ao = (75,96,72)·ao = -513,
1584        // abc'·ao = 36 > 0 → direction (-24,12,9), size 3. Return false.
1585        let [a, b, c, d] = shifted(v3i(3, 0, 0));
1586        let mut s = simplex_of(&[a, b, c, d]);
1587        let mut dir = Vec3Fix::ZERO;
1588        assert!(!do_simplex_tetrahedron(&mut s, &mut dir));
1589        assert_eq!(s.size, 3);
1590        assert_eq!([s.points[0], s.points[1], s.points[2]], [a, d, b]);
1591        assert_eq!(dir, v3i(-24, 12, 9));
1592
1593        // Origin exactly on face adb (scaled ×2 to keep integers): a = (3,0,8),
1594        // b = (-3,-6,0), c = (9,-6,0), d = (3,6,0): abc = (0,-96,72),
1595        // acd = (96,48,36), adb = (-96,48,36), ao = (-3,0,-8):
1596        // abc·ao = -576, acd·ao = -576, adb·ao = 288 - 288 = 0 → true.
1597        // (`>=` hands [a, d, b] to the triangle case and returns false.)
1598        let (a, b, c, d) = (v3i(3, 0, 8), v3i(-3, -6, 0), v3i(9, -6, 0), v3i(3, 6, 0));
1599        let mut s = simplex_of(&[a, b, c, d]);
1600        let mut dir = v3i(9, 9, 9);
1601        assert!(do_simplex_tetrahedron(&mut s, &mut dir));
1602        assert_eq!(s.size, 4);
1603        assert_eq!(dir, v3i(9, 9, 9));
1604
1605        // Origin strictly inside: shift by (0,0,-1) → ao = (0,0,-3),
1606        // abc·ao = -54, acd·ao = -27, adb·ao = -27 → true.
1607        let [a, b, c, d] = shifted(v3i(0, 0, -1));
1608        let mut s = simplex_of(&[a, b, c, d]);
1609        let mut dir = Vec3Fix::ZERO;
1610        assert!(do_simplex_tetrahedron(&mut s, &mut dir));
1611    }
1612
1613    /// Kills `distance - closest_face.distance < EPSILON` → `<=` (line 576).
1614    ///
1615    /// A ⊖ B is the hull itself (B is the single point at the origin). The
1616    /// initial tetrahedron has its closest face in the plane x = 1 (distance
1617    /// exactly 1, normal exactly (1,0,0): cross = (64,0,0), |cross| = 64 is a
1618    /// perfect square); the other three faces are at 10/√13 and 10/7 > 1. The
1619    /// hull has one extra vertex at (1 + 2^-16, 0, 0), i.e. exactly EPSILON
1620    /// beyond that face. The strict test does not converge yet: the polytope
1621    /// is expanded through the extra vertex and the reported depth exceeds 1
1622    /// (by a little less than 2^-16). The `<=` mutant stops immediately with
1623    /// depth exactly 1.
1624    #[test]
1625    fn epa_convergence_threshold_is_strict() {
1626        let eps = Fix128 {
1627            hi: 0,
1628            lo: 0x0001000000000000,
1629        };
1630        let beyond = v3i(1, 0, 0) + Vec3Fix::new(eps, Fix128::ZERO, Fix128::ZERO);
1631        let simplex = [v3i(1, 4, 4), v3i(1, -4, 4), v3i(1, 0, -4), v3i(-5, 0, 0)];
1632        assert!(tetra_contains_origin(&simplex));
1633        let mut verts = simplex.to_vec();
1634        verts.push(beyond);
1635        let a = ConvexHull::new(verts);
1636        let b = ConvexHull::new(vec![Vec3Fix::ZERO]);
1637        assert_eq!(a.support(Vec3Fix::UNIT_X), beyond);
1638        let c = epa(&a, &b, &simplex).expect("origin inside hull");
1639        let half_eps = Fix128 {
1640            hi: 0,
1641            lo: 0x0000800000000000,
1642        };
1643        assert!(c.depth > Fix128::ONE + half_eps, "depth {:?}", c.depth);
1644        assert!(c.depth <= Fix128::ONE + eps, "depth {:?}", c.depth);
1645        assert!(c.normal.x < Fix128::ONE, "{:?}", c.normal);
1646    }
1647
1648    /// Box-box EPA from hand-built, origin-enclosing initial tetrahedra whose
1649    /// closest face is *not* the answer, so the polytope has to be expanded.
1650    /// Kills `add_face` `distance < 0` → `== 0` (line 648, config A: the
1651    /// inward-facing initial faces are never flipped → EPA fails), the
1652    /// `(-normal, -distance)` → `(normal, -distance)` mutant (line 649,
1653    /// config B: reports 1/4 along y instead of 1/8 along x) and
1654    /// `(i + 1) % 3` → `/ 3` (line 607, config C: the polytope loses faces and
1655    /// EPA fails).
1656    ///
1657    /// A = [-1,1]^3 throughout; with B = [bx,3]×[by,3]×[bz,3] the Minkowski
1658    /// difference A ⊖ B is [-4, 1-bx]×[-4, 1-by]×[-4, 1-bz], whose corners are
1659    /// exact support points, and the penetration depth is the smallest of the
1660    /// three positive extents. The contact points are `A.support(n)` and
1661    /// `B.support(-n)`, so `(point_a - point_b)·n` equals the depth.
1662    #[test]
1663    fn epa_box_box_exact_depth_from_hand_built_simplices() {
1664        let a = AABB::new(v3i(-1, -1, -1), v3i(1, 1, 1));
1665        let r = Fix128::from_ratio;
1666        let check = |b: &AABB, simplex: &[Vec3Fix; 4], depth: Fix128, axis: usize| {
1667            assert!(tetra_contains_origin(simplex), "{simplex:?}");
1668            let c = epa(&a, b, simplex).expect("overlapping boxes");
1669            assert_eq!(c.depth, depth, "depth {:?}", c.depth);
1670            let n = [c.normal.x, c.normal.y, c.normal.z];
1671            for (i, comp) in n.iter().enumerate() {
1672                if i == axis {
1673                    assert_eq!(comp.abs(), Fix128::ONE, "{:?}", c.normal);
1674                } else {
1675                    assert_eq!(*comp, Fix128::ZERO, "{:?}", c.normal);
1676                }
1677            }
1678            // normal is B→A: A's deepest point lies along −normal (A→B)
1679            assert_eq!(c.point_a, a.support(-c.normal));
1680            assert_eq!(c.point_b, b.support(c.normal));
1681            assert_eq!((c.point_a - c.point_b).dot(c.normal).abs(), depth);
1682            // B→A: the normal points from B's centre towards A's centre
1683            let ab = b.min + b.max - (a.min + a.max);
1684            assert!(ab.dot(c.normal) <= Fix128::ZERO, "B→A sign: {:?}", c.normal);
1685        };
1686
1687        // Config A: B = [5/8,3]×[1/2,3]×[1/4,3] → A ⊖ B = [-4,3/8]×[-4,1/2]×[-4,3/4],
1688        // depth 3/8 along x. Tetra = corners (3/8,1/2,-4), (-4,-4,3/4),
1689        // (3/8,-4,3/4), (3/8,1/2,3/4).
1690        let b = AABB::new(Vec3Fix::new(r(5, 8), r(1, 2), r(1, 4)), v3i(3, 3, 3));
1691        let t = [
1692            Vec3Fix::new(r(3, 8), r(1, 2), fi(-4)),
1693            Vec3Fix::new(fi(-4), fi(-4), r(3, 4)),
1694            Vec3Fix::new(r(3, 8), fi(-4), r(3, 4)),
1695            Vec3Fix::new(r(3, 8), r(1, 2), r(3, 4)),
1696        ];
1697        check(&b, &t, r(3, 8), 0);
1698
1699        // Config B: B = [7/8,3]×[3/4,3]×[5/8,3] → A ⊖ B = [-4,1/8]×[-4,1/4]×[-4,3/8],
1700        // depth 1/8 along x. Tetra = corners (1/8,-4,-4), (-4,1/4,-4),
1701        // (-4,-4,3/8), (1/8,1/4,3/8).
1702        let b = AABB::new(Vec3Fix::new(r(7, 8), r(3, 4), r(5, 8)), v3i(3, 3, 3));
1703        let t = [
1704            Vec3Fix::new(r(1, 8), fi(-4), fi(-4)),
1705            Vec3Fix::new(fi(-4), r(1, 4), fi(-4)),
1706            Vec3Fix::new(fi(-4), fi(-4), r(3, 8)),
1707            Vec3Fix::new(r(1, 8), r(1, 4), r(3, 8)),
1708        ];
1709        check(&b, &t, r(1, 8), 0);
1710
1711        // Config C: B = [5/8,3]×[1/2,3]×[3/4,3] → A ⊖ B = [-4,3/8]×[-4,1/2]×[-4,1/4],
1712        // depth 1/4 along z. Tetra = corners (-4,-4,-4), (3/8,-4,1/4),
1713        // (-4,1/2,1/4), (3/8,1/2,1/4).
1714        let b = AABB::new(Vec3Fix::new(r(5, 8), r(1, 2), r(3, 4)), v3i(3, 3, 3));
1715        let t = [
1716            v3i(-4, -4, -4),
1717            Vec3Fix::new(r(3, 8), fi(-4), r(1, 4)),
1718            Vec3Fix::new(fi(-4), r(1, 2), r(1, 4)),
1719            Vec3Fix::new(r(3, 8), r(1, 2), r(1, 4)),
1720        ];
1721        check(&b, &t, r(1, 4), 2);
1722    }
1723
1724    /// Sphere-box EPA with a curved Minkowski boundary, so the polytope is
1725    /// refined over several expansions. Kills `to_new = support - v` →
1726    /// `support + v` (line 602: every face becomes "visible", the edge
1727    /// bookkeeping breaks and EPA returns `None`) and, again, line 607.
1728    ///
1729    /// Box A = [-1,1]^3, sphere B centre (-3/2,0,0) r=1: the sphere reaches
1730    /// x = -1/2 past the box face x = -1, so the penetration depth is 1/2
1731    /// along x (analytic). Initial tetrahedron = Minkowski support points in
1732    /// the directions (1,0,0), (-1,1,0), (-1,-1,1), (-1,-1,-1); origin
1733    /// containment is checked with signed volumes. EPA converges to within
1734    /// its EPSILON (2^-16), so the comparison uses 2^-13.
1735    #[test]
1736    fn epa_sphere_box_face_penetration_is_analytic() {
1737        let a = AABB::new(v3i(-1, -1, -1), v3i(1, 1, 1));
1738        let b = Sphere::new(
1739            Vec3Fix::new(Fix128::from_ratio(-3, 2), Fix128::ZERO, Fix128::ZERO),
1740            Fix128::ONE,
1741        );
1742        let dirs = [v3i(1, 0, 0), v3i(-1, 1, 0), v3i(-1, -1, 1), v3i(-1, -1, -1)];
1743        let mut simplex = [Vec3Fix::ZERO; 4];
1744        for (i, d) in dirs.iter().enumerate() {
1745            simplex[i] = minkowski_support(&a, &b, *d);
1746        }
1747        assert!(tetra_contains_origin(&simplex));
1748        let c = epa(&a, &b, &simplex).expect("sphere overlaps box");
1749        let tol = Fix128::from_ratio(1, 8192);
1750        assert!(
1751            near_tol(c.depth, Fix128::from_ratio(1, 2), tol),
1752            "depth {:?}",
1753            c.depth
1754        );
1755        assert!(
1756            near_tol(c.normal.x.abs(), Fix128::ONE, tol),
1757            "{:?}",
1758            c.normal
1759        );
1760        assert!(
1761            c.normal.y.abs() < tol && c.normal.z.abs() < tol,
1762            "{:?}",
1763            c.normal
1764        );
1765        // sphere B is on −x of box A: B→A normal is +x
1766        assert!(c.normal.x > Fix128::ZERO, "B→A sign: {:?}", c.normal);
1767        assert_eq!(c.point_a, a.support(-c.normal));
1768        assert_eq!(c.point_b, b.support(c.normal));
1769    }
1770
1771    /// Touching boxes: the origin lies on a face of the initial simplex
1772    /// (distance exactly 0) and on the boundary of A ⊖ B. Kills `add_face`
1773    /// `distance < 0` → `<= 0` (line 648: the zero-distance face gets its
1774    /// normal flipped to point into the polytope, EPA expands the wrong way
1775    /// and reports depth 4 = the far face) and again `→ == 0`.
1776    ///
1777    /// A = [0,2]×[-1,1]^2, B = [-2,0]×[-1,1]^2 share the plane x = 0, so
1778    /// A ⊖ B = [0,4]×[-2,2]^2 has the origin on its face x = 0. Initial
1779    /// simplex (all corners / face points of A ⊖ B): p0 = (0,-2,2),
1780    /// p1 = (0,2,2), p2 = (0,0,-2), p3 = (4,0,0). Face (p0,p1,p2):
1781    /// (p1-p0) × (p2-p0) = (0,4,0) × (0,2,-4) = (-16,0,0) → normal (-1,0,0)
1782    /// exactly, distance p0·n = 0. It is the closest face; the support in
1783    /// (-1,0,0) is A.support(-x) - B.support(+x) = (0,1,1) - (0,1,1) = 0 with
1784    /// dot 0, so EPA converges at once: depth 0, normal (-1,0,0), and both
1785    /// contact points are (0,1,1).
1786    ///
1787    /// Note: a zero-distance face keeps the orientation given by its vertex
1788    /// winding (there is no "away from the origin" side); this simplex is
1789    /// wound so that the normal points out of the polytope.
1790    #[test]
1791    fn epa_touching_boxes_zero_depth_face_through_origin() {
1792        let a = AABB::new(v3i(0, -1, -1), v3i(2, 1, 1));
1793        let b = AABB::new(v3i(-2, -1, -1), v3i(0, 1, 1));
1794        // A is on +x of B: B→A normal = +x, depth 0, for either winding of
1795        // the face through the origin (1.2.0: faces are oriented by the
1796        // polytope interior; before that this winding reported depth 4)
1797        for simplex in [
1798            [v3i(0, -2, 2), v3i(0, 2, 2), v3i(0, 0, -2), v3i(4, 0, 0)],
1799            [v3i(0, 2, 2), v3i(0, -2, 2), v3i(0, 0, -2), v3i(4, 0, 0)],
1800        ] {
1801            let c = epa(&a, &b, &simplex).expect("touching boxes");
1802            assert_eq!(c.depth, Fix128::ZERO, "{simplex:?}");
1803            assert_eq!(c.normal, v3i(1, 0, 0), "{simplex:?}");
1804            assert_eq!(c.point_a, v3i(0, 1, 1));
1805            assert_eq!(c.point_b, v3i(0, 1, 1));
1806        }
1807    }
1808}
1809
1810#[cfg(test)]
1811#[path = "collider_mutants_tests.rs"]
1812mod collider_mutants_tests;