loam_physics/collision/
gjk.rs

1use glam::{Quat, Vec3};
2
3pub trait SupportFn {
4    fn support(&self, direction: Vec3) -> Vec3;
5}
6
7pub struct ConvexHull<'a> {
8    pub vertices: &'a [Vec3],
9}
10
11impl<'a> SupportFn for ConvexHull<'a> {
12    fn support(&self, direction: Vec3) -> Vec3 {
13        let mut best = self.vertices[0];
14        let mut best_d = best.dot(direction);
15        for &v in &self.vertices[1..] {
16            let d = v.dot(direction);
17            if d > best_d {
18                best_d = d;
19                best = v;
20            }
21        }
22        best
23    }
24}
25
26/// `rotation` must be a unit quaternion.
27pub struct PosedHull<'a> {
28    pub local: &'a [Vec3],
29    pub position: Vec3,
30    pub rotation: Quat,
31}
32
33impl SupportFn for PosedHull<'_> {
34    fn support(&self, direction: Vec3) -> Vec3 {
35        let local_direction = self.rotation.conjugate() * direction;
36        let hull = ConvexHull {
37            vertices: self.local,
38        };
39        self.rotation * hull.support(local_direction) + self.position
40    }
41}
42
43pub struct Sphere {
44    pub center: Vec3,
45    pub radius: f32,
46}
47
48impl SupportFn for Sphere {
49    fn support(&self, direction: Vec3) -> Vec3 {
50        let dir = direction.normalize_or(Vec3::Y);
51        self.center + dir * self.radius
52    }
53}
54
55/// `sa` and `sb` are the contributing support points on A and B.
56#[derive(Clone, Copy, Debug)]
57pub struct MinkowskiPoint {
58    pub point: Vec3,
59    pub sa: Vec3,
60    pub sb: Vec3,
61}
62
63pub fn minkowski_support<A: SupportFn, B: SupportFn>(
64    a: &A,
65    b: &B,
66    direction: Vec3,
67) -> MinkowskiPoint {
68    let sa = a.support(direction);
69    let sb = b.support(-direction);
70    MinkowskiPoint {
71        point: sa - sb,
72        sa,
73        sb,
74    }
75}
76
77#[derive(Debug)]
78pub enum GjkResult {
79    Intersecting { simplex: [MinkowskiPoint; 4] },
80    Separated,
81}
82
83const GJK_MAX_ITERATIONS: u32 = 32;
84const GJK_EPS: f32 = 1e-6;
85
86/// On intersection the returned tetrahedron is the seed EPA expects.
87pub fn gjk_intersect<A: SupportFn, B: SupportFn>(
88    a: &A,
89    b: &B,
90    initial_direction: Vec3,
91) -> GjkResult {
92    let mut dir = if initial_direction.length_squared() > GJK_EPS {
93        initial_direction
94    } else {
95        Vec3::X
96    };
97
98    let mut simplex: [MinkowskiPoint; 4] = [MinkowskiPoint {
99        point: Vec3::ZERO,
100        sa: Vec3::ZERO,
101        sb: Vec3::ZERO,
102    }; 4];
103    simplex[0] = minkowski_support(a, b, dir);
104    let mut n = 1usize;
105    dir = -simplex[0].point;
106
107    for _ in 0..GJK_MAX_ITERATIONS {
108        let new_point = minkowski_support(a, b, dir);
109        if new_point.point.dot(dir) < 0.0 {
110            return GjkResult::Separated;
111        }
112
113        simplex[n] = new_point;
114        n += 1;
115
116        let (contains_origin, new_n, new_dir) = do_simplex(&mut simplex, n);
117        n = new_n;
118        if contains_origin {
119            return GjkResult::Intersecting { simplex };
120        }
121        if new_dir.length_squared() < GJK_EPS {
122            if n >= 4 {
123                return GjkResult::Intersecting { simplex };
124            }
125            return GjkResult::Separated;
126        }
127        dir = new_dir;
128    }
129
130    GjkResult::Separated
131}
132
133// Newest point at `n-1` on entry; `simplex[0..new_n]` holds survivors on exit.
134fn do_simplex(simplex: &mut [MinkowskiPoint; 4], n: usize) -> (bool, usize, Vec3) {
135    match n {
136        2 => do_line(simplex),
137        3 => do_triangle(simplex),
138        4 => do_tetrahedron(simplex),
139        _ => unreachable!("simplex size {n} out of range"),
140    }
141}
142
143fn do_line(simplex: &mut [MinkowskiPoint; 4]) -> (bool, usize, Vec3) {
144    let a = simplex[1].point;
145    let b = simplex[0].point;
146    let ab = b - a;
147    let ao = -a;
148
149    if ab.dot(ao) > 0.0 {
150        let dir = triple_product(ab, ao, ab);
151        if dir.length_squared() < 1e-10 {
152            (false, 2, any_perpendicular(ab))
153        } else {
154            (false, 2, dir)
155        }
156    } else {
157        simplex[0] = simplex[1];
158        (false, 1, ao)
159    }
160}
161
162fn any_perpendicular(v: Vec3) -> Vec3 {
163    if v.x.abs() <= v.y.abs() && v.x.abs() <= v.z.abs() {
164        v.cross(Vec3::X)
165    } else if v.y.abs() <= v.z.abs() {
166        v.cross(Vec3::Y)
167    } else {
168        v.cross(Vec3::Z)
169    }
170}
171
172fn fall_back_to_ab(simplex: &mut [MinkowskiPoint; 4]) -> (bool, usize, Vec3) {
173    simplex[0] = simplex[1];
174    simplex[1] = simplex[2];
175    do_line(simplex)
176}
177
178fn do_triangle(simplex: &mut [MinkowskiPoint; 4]) -> (bool, usize, Vec3) {
179    let a = simplex[2].point;
180    let b = simplex[1].point;
181    let c = simplex[0].point;
182
183    let ab = b - a;
184    let ac = c - a;
185    let ao = -a;
186    let abc = ab.cross(ac);
187
188    if abc.cross(ac).dot(ao) > 0.0 {
189        if ac.dot(ao) > 0.0 {
190            simplex[1] = simplex[2];
191            let dir = triple_product(ac, ao, ac);
192            return (false, 2, dir);
193        }
194        return fall_back_to_ab(simplex);
195    }
196
197    if ab.cross(abc).dot(ao) > 0.0 {
198        return fall_back_to_ab(simplex);
199    }
200
201    if abc.dot(ao) > 0.0 {
202        (false, 3, abc)
203    } else {
204        simplex.swap(0, 1);
205        (false, 3, -abc)
206    }
207}
208
209// Face normals use the opposite vertex; simplex pruning can change winding.
210fn do_tetrahedron(simplex: &mut [MinkowskiPoint; 4]) -> (bool, usize, Vec3) {
211    let a = simplex[3].point;
212    let b = simplex[2].point;
213    let c = simplex[1].point;
214    let d = simplex[0].point;
215
216    let ab = b - a;
217    let ac = c - a;
218    let ad = d - a;
219    let ao = -a;
220
221    let mut abc = ab.cross(ac);
222    if abc.dot(ad) > 0.0 {
223        abc = -abc;
224    }
225    let mut acd = ac.cross(ad);
226    if acd.dot(ab) > 0.0 {
227        acd = -acd;
228    }
229    let mut adb = ad.cross(ab);
230    if adb.dot(ac) > 0.0 {
231        adb = -adb;
232    }
233
234    if abc.dot(ao) > 0.0 {
235        simplex[0] = simplex[1];
236        simplex[1] = simplex[2];
237        simplex[2] = simplex[3];
238        return do_triangle(simplex);
239    }
240    if acd.dot(ao) > 0.0 {
241        simplex[2] = simplex[3];
242        return do_triangle(simplex);
243    }
244    if adb.dot(ao) > 0.0 {
245        let d_point = simplex[0];
246        simplex[0] = simplex[2];
247        simplex[1] = d_point;
248        simplex[2] = simplex[3];
249        return do_triangle(simplex);
250    }
251
252    (true, 4, Vec3::ZERO)
253}
254
255fn triple_product(a: Vec3, b: Vec3, c: Vec3) -> Vec3 {
256    a.cross(b).cross(c)
257}
258
259#[cfg(test)]
260mod tests {
261    use super::*;
262
263    fn box_vertices(center: Vec3, half: Vec3) -> Vec<Vec3> {
264        vec![
265            center + Vec3::new(-half.x, -half.y, -half.z),
266            center + Vec3::new(half.x, -half.y, -half.z),
267            center + Vec3::new(half.x, half.y, -half.z),
268            center + Vec3::new(-half.x, half.y, -half.z),
269            center + Vec3::new(-half.x, -half.y, half.z),
270            center + Vec3::new(half.x, -half.y, half.z),
271            center + Vec3::new(half.x, half.y, half.z),
272            center + Vec3::new(-half.x, half.y, half.z),
273        ]
274    }
275
276    #[test]
277    fn posed_support_uses_the_inverse_rotation() {
278        let local = [
279            Vec3::new(0.6, 0.1, -0.2),
280            Vec3::new(-0.4, 0.7, 0.15),
281            Vec3::new(0.05, -0.8, 0.45),
282        ];
283        let position = Vec3::new(1.0, -2.0, 0.5);
284        let rotation = Quat::from_rotation_z(0.7) * Quat::from_rotation_x(-0.3);
285        let vertices = local.map(|v| rotation * v + position);
286        let posed = PosedHull {
287            local: &local,
288            position,
289            rotation,
290        };
291        let world = ConvexHull {
292            vertices: &vertices,
293        };
294        for direction in [Vec3::X, -Vec3::Y, Vec3::Z, Vec3::new(0.3, -0.7, 0.9)] {
295            assert!((posed.support(direction) - world.support(direction)).length() < 1e-6);
296        }
297    }
298
299    #[test]
300    fn separated_boxes_report_no_intersection() {
301        let va = box_vertices(Vec3::ZERO, Vec3::ONE);
302        let vb = box_vertices(Vec3::new(3.0, 0.0, 0.0), Vec3::ONE);
303        let a = ConvexHull { vertices: &va };
304        let b = ConvexHull { vertices: &vb };
305
306        match gjk_intersect(&a, &b, Vec3::new(3.0, 0.0, 0.0)) {
307            GjkResult::Separated => {}
308            GjkResult::Intersecting { .. } => panic!("should be separated"),
309        }
310    }
311
312    #[test]
313    fn touching_boxes_report_intersection() {
314        let va = box_vertices(Vec3::ZERO, Vec3::ONE);
315        let vb = box_vertices(Vec3::new(2.0, 0.0, 0.0), Vec3::ONE);
316        let a = ConvexHull { vertices: &va };
317        let b = ConvexHull { vertices: &vb };
318
319        match gjk_intersect(&a, &b, Vec3::new(2.0, 0.0, 0.0)) {
320            GjkResult::Intersecting { .. } => {}
321            GjkResult::Separated => panic!("touching boundaries should count as intersecting"),
322        }
323    }
324
325    #[test]
326    fn sphere_vs_sphere_matches_distance_test() {
327        for &(ax, bx, overlap) in &[(0.0, 3.0, false), (0.0, 1.5, true), (0.0, 2.0, true)] {
328            let a = Sphere {
329                center: Vec3::new(ax, 0.0, 0.0),
330                radius: 1.0,
331            };
332            let b = Sphere {
333                center: Vec3::new(bx, 0.0, 0.0),
334                radius: 1.0,
335            };
336            let result = gjk_intersect(&a, &b, Vec3::new(bx - ax, 0.0, 0.0));
337            let got = matches!(result, GjkResult::Intersecting { .. });
338            assert_eq!(
339                got, overlap,
340                "centres at ({ax},0,0) and ({bx},0,0): expected intersecting={overlap}, got {got}"
341            );
342        }
343    }
344
345    #[test]
346    fn box_vs_sphere_corner_contact() {
347        let vb = box_vertices(Vec3::ZERO, Vec3::ONE);
348        let b = ConvexHull { vertices: &vb };
349
350        let far = Sphere {
351            center: Vec3::new(1.35, 1.35, 1.35),
352            radius: 0.5,
353        };
354        assert!(matches!(
355            gjk_intersect(&far, &b, Vec3::new(-1.0, -1.0, -1.0)),
356            GjkResult::Separated
357        ));
358
359        let near = Sphere {
360            center: Vec3::new(1.2, 1.2, 1.2),
361            radius: 0.5,
362        };
363        assert!(matches!(
364            gjk_intersect(&near, &b, Vec3::new(-1.0, -1.0, -1.0)),
365            GjkResult::Intersecting { .. }
366        ));
367    }
368
369    #[test]
370    fn rotated_boxes_separate_as_axes_allow() {
371        use glam::Quat;
372        let va = box_vertices(Vec3::ZERO, Vec3::ONE);
373        let rot = Quat::from_rotation_z(std::f32::consts::FRAC_PI_4);
374        let vb_rot: Vec<Vec3> = box_vertices(Vec3::ZERO, Vec3::ONE)
375            .iter()
376            .map(|&v| rot * v + Vec3::new(2.5, 0.0, 0.0))
377            .collect();
378        let a = ConvexHull { vertices: &va };
379        let b = ConvexHull { vertices: &vb_rot };
380
381        assert!(matches!(
382            gjk_intersect(&a, &b, Vec3::new(2.5, 0.0, 0.0)),
383            GjkResult::Separated
384        ));
385
386        let vb_close: Vec<Vec3> = box_vertices(Vec3::ZERO, Vec3::ONE)
387            .iter()
388            .map(|&v| rot * v + Vec3::new(2.2, 0.0, 0.0))
389            .collect();
390        let b_close = ConvexHull {
391            vertices: &vb_close,
392        };
393        assert!(matches!(
394            gjk_intersect(&a, &b_close, Vec3::new(2.2, 0.0, 0.0)),
395            GjkResult::Intersecting { .. }
396        ));
397    }
398}
loam_physics/collision/gjk.rs

loam_physics/collision/
gjk.rs
