loam_physics/collision/
gjk_r4.rs

1use glam::Vec4;
2use loam_math::{Rotor, Rotor4};
3
4use super::simplex_r4::{closest_to_origin, project_origin_onto_affine_hull};
5
6pub trait SupportFn4 {
7    fn support(&self, direction: Vec4) -> Vec4;
8}
9
10pub struct ConvexHull4<'a> {
11    pub vertices: &'a [Vec4],
12}
13
14impl<'a> SupportFn4 for ConvexHull4<'a> {
15    fn support(&self, direction: Vec4) -> Vec4 {
16        let mut best = self.vertices[0];
17        let mut best_d = best.dot(direction);
18        for &v in &self.vertices[1..] {
19            let d = v.dot(direction);
20            if d > best_d {
21                best_d = d;
22                best = v;
23            }
24        }
25        best
26    }
27}
28
29pub struct PosedHull4<'a> {
30    pub local: &'a [Vec4],
31    pub position: Vec4,
32    pub rotation: Rotor4,
33}
34
35impl SupportFn4 for PosedHull4<'_> {
36    fn support(&self, direction: Vec4) -> Vec4 {
37        let local_dir = self.rotation.inverse().apply(direction);
38        let mut best = self.local[0];
39        let mut best_d = best.dot(local_dir);
40        for &v in &self.local[1..] {
41            let d = v.dot(local_dir);
42            if d > best_d {
43                best_d = d;
44                best = v;
45            }
46        }
47        self.rotation.apply(best) + self.position
48    }
49}
50
51pub struct Sphere4 {
52    pub center: Vec4,
53    pub radius: f32,
54}
55
56impl SupportFn4 for Sphere4 {
57    fn support(&self, direction: Vec4) -> Vec4 {
58        let d = direction.length_squared();
59        let dir = if d > 1e-12 {
60            direction / d.sqrt()
61        } else {
62            Vec4::Y
63        };
64        self.center + dir * self.radius
65    }
66}
67
68/// `sa` and `sb` are the contributing support points on A and B.
69#[derive(Clone, Copy, Debug)]
70pub struct MinkowskiPoint4 {
71    pub point: Vec4,
72    pub sa: Vec4,
73    pub sb: Vec4,
74}
75
76pub fn minkowski_support_r4<A: SupportFn4, B: SupportFn4>(
77    a: &A,
78    b: &B,
79    direction: Vec4,
80) -> MinkowskiPoint4 {
81    let sa = a.support(direction);
82    let sb = b.support(-direction);
83    MinkowskiPoint4 {
84        point: sa - sb,
85        sa,
86        sb,
87    }
88}
89
90#[derive(Debug)]
91#[allow(clippy::large_enum_variant)]
92pub enum GjkResult4 {
93    Intersecting { simplex: [MinkowskiPoint4; 5] },
94    Separated,
95}
96
97const GJK_MAX_ITERATIONS: u32 = 48;
98const GJK_EPS: f32 = 1e-6;
99
100/// On overlap the returned 4-simplex is the seed EPA expects.
101pub fn gjk_intersect_r4<A: SupportFn4, B: SupportFn4>(
102    a: &A,
103    b: &B,
104    initial_direction: Vec4,
105) -> GjkResult4 {
106    let mut dir = if initial_direction.length_squared() > GJK_EPS {
107        initial_direction
108    } else {
109        Vec4::X
110    };
111    let mut simplex = [minkowski_support_r4(a, b, dir); 5];
112    let mut len = 1;
113    dir = -simplex[0].point;
114
115    for _ in 0..GJK_MAX_ITERATIONS {
116        if dir.length_squared() < GJK_EPS {
117            if let Some(seed) = complete_simplex(a, b, simplex, len) {
118                return GjkResult4::Intersecting { simplex: seed };
119            }
120        }
121        let direction = dir.try_normalize().unwrap_or(Vec4::X);
122        let new_point = minkowski_support_r4(a, b, direction);
123        if new_point.point.dot(direction) < 0.0 {
124            return GjkResult4::Separated;
125        }
126        if simplex[..len].iter().any(|p| p.point == new_point.point) {
127            return GjkResult4::Separated;
128        }
129        simplex[len] = new_point;
130        len += 1;
131        let points = simplex.map(|p| p.point);
132        let closest = closest_to_origin(&points[..len]);
133        let previous = simplex;
134        for (slot, &i) in closest.kept().iter().enumerate() {
135            simplex[slot] = previous[i];
136        }
137        len = closest.kept().len();
138        if len == 5 {
139            return GjkResult4::Intersecting { simplex };
140        }
141        dir = -closest.point;
142    }
143    GjkResult4::Separated
144}
145
146fn complete_simplex<A: SupportFn4, B: SupportFn4>(
147    a: &A,
148    b: &B,
149    mut simplex: [MinkowskiPoint4; 5],
150    len: usize,
151) -> Option<[MinkowskiPoint4; 5]> {
152    if len == 5 {
153        let points = simplex.map(|p| p.point);
154        let (_, weights) = project_origin_onto_affine_hull(&[0, 1, 2, 3, 4], &points)?;
155        return weights
156            .iter()
157            .all(|&w| w.is_finite() && w >= 0.0)
158            .then_some(simplex);
159    }
160    let probe = orthogonal_to_hull(&simplex[..len])?;
161    for direction in [probe, -probe] {
162        let support = minkowski_support_r4(a, b, direction);
163        if simplex[..len].iter().any(|p| p.point == support.point) {
164            continue;
165        }
166        simplex[len] = support;
167        if let Some(seed) = complete_simplex(a, b, simplex, len + 1) {
168            return Some(seed);
169        }
170    }
171    None
172}
173
174fn orthogonal_to_hull(simplex: &[MinkowskiPoint4]) -> Option<Vec4> {
175    let mut onb = [Vec4::ZERO; 4];
176    let mut rank = 0;
177    for p in &simplex[1..] {
178        let mut r = p.point - simplex[0].point;
179        for o in &onb[..rank] {
180            r -= *o * r.dot(*o);
181        }
182        let m = r.length_squared();
183        if m > 1e-10 {
184            onb[rank] = r / m.sqrt();
185            rank += 1;
186        }
187    }
188    let mut best: Option<(f32, Vec4)> = None;
189    for axis in [Vec4::X, Vec4::Y, Vec4::Z, Vec4::W] {
190        let mut r = axis;
191        for o in &onb[..rank] {
192            r -= *o * r.dot(*o);
193        }
194        let mag_sq = r.length_squared();
195        if mag_sq > 1e-8 && best.is_none_or(|(m, _)| mag_sq > m) {
196            best = Some((mag_sq, r));
197        }
198    }
199    best.map(|(_, v)| v.normalize())
200}
201
202#[cfg(test)]
203mod tests {
204    use super::*;
205    use loam_math::{Bivector, Bivector4, Plane4};
206
207    #[test]
208    fn a_posed_hull_supports_where_the_world_vertices_would() {
209        let local = [
210            Vec4::new(0.6, 0.1, -0.2, 0.35),
211            Vec4::new(-0.4, 0.7, 0.15, -0.5),
212            Vec4::new(0.05, -0.8, 0.45, 0.2),
213            Vec4::new(-0.3, 0.2, -0.65, -0.1),
214            Vec4::new(0.25, 0.4, 0.5, 0.6),
215        ];
216        let position = Vec4::new(3.0, -1.5, 0.75, -2.25);
217        let simple = (Plane4::Xw.unit_bivector() * 0.8).exp().normalize();
218        let double = (Bivector4::new(0.5, 0.0, 0.0, 0.0, 0.0, -0.9).exp()).normalize();
219
220        for rotation in [Rotor4::identity(), simple, double] {
221            let world: Vec<Vec4> = local
222                .iter()
223                .map(|v| rotation.apply(*v) + position)
224                .collect();
225            let posed = PosedHull4 {
226                local: &local,
227                position,
228                rotation,
229            };
230            let materialised = ConvexHull4 { vertices: &world };
231            for dir in [
232                Vec4::X,
233                Vec4::W,
234                Vec4::new(1.0, 1.0, 1.0, 1.0),
235                Vec4::new(-0.3, 0.9, -0.2, 0.7),
236                Vec4::new(0.0, -1.0, 0.4, -0.4),
237            ] {
238                let (a, b) = (posed.support(dir), materialised.support(dir));
239                assert!(
240                    (a - b).length() < 1e-5,
241                    "posed support {a:?} against materialised {b:?} for {dir:?}"
242                );
243            }
244        }
245    }
246
247    #[test]
248    fn a_small_positive_gap_is_not_an_intersection() {
249        for gap in [1e-5, 1e-4, 5e-4, 1e-3] {
250            let a = Sphere4 {
251                center: Vec4::ZERO,
252                radius: 1.0,
253            };
254            let b = Sphere4 {
255                center: Vec4::X * (2.0 + gap),
256                radius: 1.0,
257            };
258            for direction in [Vec4::X, -Vec4::X, Vec4::Y] {
259                assert!(
260                    matches!(gjk_intersect_r4(&a, &b, direction), GjkResult4::Separated),
261                    "gap={gap}, direction={direction:?}"
262                );
263            }
264        }
265    }
266
267    #[test]
268    fn rotated_hulls_with_disjoint_x_intervals_are_separated() {
269        use crate::euclidean_r4::tesseract_vertices;
270        let rotor = Bivector4::new(0.37, -0.21, 0.43, 0.17, -0.29, 0.13).exp();
271        for scale in [0.1, 1.0, 100.0] {
272            let a_vertices = tesseract_vertices(scale);
273            let rotated: Vec<_> = a_vertices.iter().map(|&v| rotor.apply(v)).collect();
274            let right_a = a_vertices
275                .iter()
276                .map(|v| v.x)
277                .fold(f32::NEG_INFINITY, f32::max);
278            let left_b = rotated.iter().map(|v| v.x).fold(f32::INFINITY, f32::min);
279            for relative_gap in [1e-5, 1e-3, 0.1] {
280                let translation = Vec4::new(
281                    right_a - left_b + relative_gap * scale,
282                    0.2 * scale,
283                    -0.15 * scale,
284                    0.1 * scale,
285                );
286                let b_vertices: Vec<_> = rotated.iter().map(|&v| v + translation).collect();
287                assert!(b_vertices.iter().all(|v| v.x > right_a));
288                let a = ConvexHull4 {
289                    vertices: &a_vertices,
290                };
291                let b = ConvexHull4 {
292                    vertices: &b_vertices,
293                };
294                for direction in [Vec4::Y, Vec4::Z, Vec4::new(0.31, -0.7, 0.5, 0.2)] {
295                    assert!(
296                        matches!(gjk_intersect_r4(&a, &b, direction), GjkResult4::Separated),
297                        "scale={scale}, gap={relative_gap}, direction={direction:?}"
298                    );
299                }
300            }
301        }
302    }
303
304    #[test]
305    fn overlapping_spheres_complete_lower_dimensional_searches() {
306        for center in [
307            Vec4::X,
308            Vec4::new(1.1, 0.7, 0.0, 0.0),
309            Vec4::new(1.1, 0.7, 0.3, 0.2),
310        ] {
311            let a = Sphere4 {
312                center: Vec4::ZERO,
313                radius: 2.0,
314            };
315            let b = Sphere4 {
316                center,
317                radius: 2.0,
318            };
319            assert!(center.length() < a.radius + b.radius);
320            for direction in [Vec4::X, Vec4::Y, Vec4::W] {
321                assert!(
322                    matches!(
323                        gjk_intersect_r4(&a, &b, direction),
324                        GjkResult4::Intersecting { .. }
325                    ),
326                    "center={center:?}, direction={direction:?}"
327                );
328            }
329        }
330    }
331
332    #[test]
333    fn deeply_overlapping_pentatopes() {
334        use crate::euclidean_r4::pentatope_vertices;
335        let va: Vec<Vec4> = pentatope_vertices(1.0);
336        let vb: Vec<Vec4> = pentatope_vertices(1.0)
337            .into_iter()
338            .map(|v| v + Vec4::new(0.2, 0.0, 0.0, 0.0))
339            .collect();
340        let a = ConvexHull4 { vertices: &va };
341        let b = ConvexHull4 { vertices: &vb };
342        assert!(matches!(
343            gjk_intersect_r4(&a, &b, Vec4::X),
344            GjkResult4::Intersecting { .. }
345        ));
346    }
347
348    #[test]
349    fn tesseract_face_touching_is_bracketed_and_never_resolves_to_a_depth() {
350        use crate::collision::epa_r4::epa_r4;
351        use crate::euclidean_r4::tesseract_vertices;
352
353        let va: Vec<Vec4> = tesseract_vertices(1.0);
354        let a = ConvexHull4 { vertices: &va };
355        for (shift, expect_overlap) in [
356            (1.0 - 1e-1, true),
357            (1.0 - 1e-2, true),
358            (1.0, false),
359            (1.0 + 1e-1, false),
360            (1.0 + 1.0, false),
361        ] {
362            let vb: Vec<Vec4> = tesseract_vertices(1.0)
363                .into_iter()
364                .map(|v| v + Vec4::new(shift, 0.0, 0.0, 0.0))
365                .collect();
366            let b = ConvexHull4 { vertices: &vb };
367            let depth = match gjk_intersect_r4(&a, &b, Vec4::X) {
368                GjkResult4::Intersecting { simplex } => {
369                    epa_r4(&a, &b, simplex, 2.0).map_or(0.0, |c| c.penetration)
370                }
371                GjkResult4::Separated => 0.0,
372            };
373            if expect_overlap {
374                assert!(
375                    (depth - (1.0 - shift)).abs() < 1e-2,
376                    "shift {shift} overlaps by {} but resolved to {depth}",
377                    1.0 - shift
378                );
379            } else {
380                assert_eq!(depth, 0.0, "shift {shift} is clear but resolved to {depth}");
381            }
382        }
383    }
384}
loam_physics/collision/gjk_r4.rs

loam_physics/collision/
gjk_r4.rs
