1use glam::Vec3;
2
3use super::gjk::{minkowski_support, MinkowskiPoint, SupportFn};
4
5const EPA_MAX_ITERATIONS: u32 = 48;
6
7const EPA_TOLERANCE: f32 = 1e-4;
8
9const FACE_COPLANAR_EPS: f32 = 1e-5;
10
11const FACE_DEGENERATE_CROSS: f32 = 1e-8;
12
13const SEED_DEGENERATE_VOLUME: f32 = 1e-8;
14
15const BARYCENTRIC_SINGULAR_EPS: f32 = 1e-12;
16
17#[derive(Clone, Copy)]
18struct Thresholds {
19 support_gap: f32,
20 coplanar_band: f32,
21 cross_norm: f32,
22 barycentric_gram: f32,
23}
24
25impl Thresholds {
26 fn for_scale(scale: f32) -> Self {
27 Self {
28 support_gap: EPA_TOLERANCE * scale,
29 coplanar_band: FACE_COPLANAR_EPS * scale,
30 cross_norm: FACE_DEGENERATE_CROSS * scale * scale,
31 barycentric_gram: BARYCENTRIC_SINGULAR_EPS * scale.powi(4),
32 }
33 }
34}
35
36#[derive(Clone, Copy, Debug)]
37pub struct ContactInfo {
38 pub normal: Vec3,
40 pub penetration: f32,
41 pub point: Vec3,
43}
44
45#[derive(Clone, Copy, Debug)]
46struct Face {
47 v: [usize; 3],
48 normal: Vec3,
49 distance: f32,
50}
51
52struct Polytope {
53 vertices: Vec<MinkowskiPoint>,
54 faces: Vec<Face>,
55 horizon: Vec<(usize, usize)>,
56 interior: glam::Vec3,
57 thresholds: Thresholds,
58}
59
60impl Polytope {
61 fn from_tetra(tetra: [MinkowskiPoint; 4], thresholds: Thresholds) -> Self {
62 let vertices = tetra.to_vec();
63 let interior = (tetra[0].point + tetra[1].point + tetra[2].point + tetra[3].point) * 0.25;
64 let mut faces = Vec::with_capacity(4);
65 for &(i, j, k, l) in &[(0, 1, 2, 3), (0, 3, 1, 2), (0, 2, 3, 1), (1, 3, 2, 0)] {
66 faces.push(build_face_vs_point(
67 &vertices,
68 i,
69 j,
70 k,
71 vertices[l].point,
72 thresholds.cross_norm,
73 ));
74 }
75 Self {
76 vertices,
77 faces,
78 horizon: Vec::new(),
79 interior,
80 thresholds,
81 }
82 }
83
84 fn closest_face(&self) -> Option<usize> {
85 let (idx, _) = self
86 .faces
87 .iter()
88 .enumerate()
89 .min_by(|a, b| a.1.distance.total_cmp(&b.1.distance))?;
90 Some(idx)
91 }
92
93 fn expand(&mut self, support: MinkowskiPoint) {
94 let new_idx = self.vertices.len();
95 self.vertices.push(support);
96
97 self.horizon.clear();
98 let horizon = &mut self.horizon;
99 let vertices = &self.vertices;
100
101 let coplanar_band = self.thresholds.coplanar_band;
102 self.faces.retain(|f| {
103 let view = support.point - vertices[f.v[0]].point;
104 if f.normal.dot(view) > -coplanar_band {
105 add_or_remove_edge(horizon, f.v[0], f.v[1]);
106 add_or_remove_edge(horizon, f.v[1], f.v[2]);
107 add_or_remove_edge(horizon, f.v[2], f.v[0]);
108 false
109 } else {
110 true
111 }
112 });
113
114 let interior = self.interior;
115 let cross_norm = self.thresholds.cross_norm;
116 for &(i, j) in horizon.iter() {
117 self.faces.push(build_face_vs_point(
118 &self.vertices,
119 i,
120 j,
121 new_idx,
122 interior,
123 cross_norm,
124 ));
125 }
126 }
127}
128
129fn build_face_vs_point(
130 verts: &[MinkowskiPoint],
131 a: usize,
132 b: usize,
133 c: usize,
134 interior_point: Vec3,
135 cross_norm_floor: f32,
136) -> Face {
137 let pa = verts[a].point;
138 let pb = verts[b].point;
139 let pc = verts[c].point;
140
141 let mut normal = (pb - pa).cross(pc - pa);
142 let len = normal.length();
143 if len < cross_norm_floor {
144 normal = Vec3::Y;
145 } else {
146 normal /= len;
147 }
148
149 let to_interior = interior_point - pa;
150 let (v_order, outward_normal) = if normal.dot(to_interior) > 0.0 {
151 ([a, c, b], -normal)
152 } else {
153 ([a, b, c], normal)
154 };
155
156 let raw_distance = outward_normal.dot(verts[v_order[0]].point);
157 let distance = raw_distance.max(0.0);
158 Face {
159 v: v_order,
160 normal: outward_normal,
161 distance,
162 }
163}
164
165fn add_or_remove_edge(horizon: &mut Vec<(usize, usize)>, a: usize, b: usize) {
166 if let Some(pos) = horizon.iter().position(|&e| e == (b, a)) {
167 horizon.swap_remove(pos);
168 } else {
169 horizon.push((a, b));
170 }
171}
172
173fn barycentric(a: Vec3, b: Vec3, c: Vec3, p: Vec3, gram_floor: f32) -> (f32, f32, f32) {
174 let v0 = b - a;
175 let v1 = c - a;
176 let v2 = p - a;
177 let d00 = v0.dot(v0);
178 let d01 = v0.dot(v1);
179 let d11 = v1.dot(v1);
180 let d20 = v2.dot(v0);
181 let d21 = v2.dot(v1);
182 let denom = d00 * d11 - d01 * d01;
183 if denom.abs() < gram_floor {
184 return (1.0, 0.0, 0.0);
185 }
186 let v = (d11 * d20 - d01 * d21) / denom;
187 let w = (d00 * d21 - d01 * d20) / denom;
188 let u = 1.0 - v - w;
189 (u, v, w)
190}
191
192pub fn epa<A: SupportFn, B: SupportFn>(
194 a: &A,
195 b: &B,
196 initial_simplex: [MinkowskiPoint; 4],
197 scale: f32,
198) -> Option<ContactInfo> {
199 let (polytope, face) = expand_to_boundary(a, b, initial_simplex, scale)?;
200 Some(contact_from_face(&polytope, face))
201}
202
203fn expand_to_boundary<A: SupportFn, B: SupportFn>(
204 a: &A,
205 b: &B,
206 initial_simplex: [MinkowskiPoint; 4],
207 scale: f32,
208) -> Option<(Polytope, Face)> {
209 let thresholds = Thresholds::for_scale(scale);
210
211 let p0 = initial_simplex[0].point;
212 let p1 = initial_simplex[1].point;
213 let p2 = initial_simplex[2].point;
214 let p3 = initial_simplex[3].point;
215 let volume6 = (p1 - p0).dot((p2 - p0).cross(p3 - p0)).abs();
216 if volume6 < SEED_DEGENERATE_VOLUME * scale.powi(3) {
217 return None;
218 }
219
220 let mut polytope = Polytope::from_tetra(initial_simplex, thresholds);
221
222 for _ in 0..EPA_MAX_ITERATIONS {
223 let face_idx = polytope.closest_face()?;
224 let face = polytope.faces[face_idx];
225
226 let support = minkowski_support(a, b, face.normal);
227 let new_distance = support.point.dot(face.normal);
228
229 if !new_distance.is_finite() || !support.point.is_finite() {
230 return None;
231 }
232
233 if (new_distance - face.distance).abs() < thresholds.support_gap {
234 return Some((polytope, face));
235 }
236
237 polytope.expand(support);
238 }
239
240 tracing::debug!(
241 max_iterations = EPA_MAX_ITERATIONS,
242 vertices = polytope.vertices.len(),
243 "EPA 3D hit iteration cap; returning best-estimate contact",
244 );
245 let face_idx = polytope.closest_face()?;
246 let face = polytope.faces[face_idx];
247 Some((polytope, face))
248}
249
250fn contact_from_face(polytope: &Polytope, face: Face) -> ContactInfo {
251 let v0 = polytope.vertices[face.v[0]];
252 let v1 = polytope.vertices[face.v[1]];
253 let v2 = polytope.vertices[face.v[2]];
254
255 let closest = face.normal * face.distance;
256
257 let (u, v, w) = barycentric(
258 v0.point,
259 v1.point,
260 v2.point,
261 closest,
262 polytope.thresholds.barycentric_gram,
263 );
264
265 let point_a = v0.sa * u + v1.sa * v + v2.sa * w;
266 let point_b = v0.sb * u + v1.sb * v + v2.sb * w;
267
268 ContactInfo {
269 normal: face.normal,
270 penetration: face.distance,
271 point: (point_a + point_b) * 0.5,
272 }
273}
274
275#[cfg(test)]
276mod tests {
277 use super::super::gjk::{gjk_intersect, ConvexHull, GjkResult, Sphere};
278 use super::*;
279
280 fn box_vertices(center: Vec3, half: Vec3) -> Vec<Vec3> {
281 vec![
282 center + Vec3::new(-half.x, -half.y, -half.z),
283 center + Vec3::new(half.x, -half.y, -half.z),
284 center + Vec3::new(half.x, half.y, -half.z),
285 center + Vec3::new(-half.x, half.y, -half.z),
286 center + Vec3::new(-half.x, -half.y, half.z),
287 center + Vec3::new(half.x, -half.y, half.z),
288 center + Vec3::new(half.x, half.y, half.z),
289 center + Vec3::new(-half.x, half.y, half.z),
290 ]
291 }
292
293 fn run(a: &impl SupportFn, b: &impl SupportFn, d: Vec3, scale: f32) -> ContactInfo {
294 match gjk_intersect(a, b, d) {
295 GjkResult::Intersecting { simplex } => {
296 epa(a, b, simplex, scale).expect("EPA should converge")
297 }
298 GjkResult::Separated => panic!("GJK says separated, EPA can't run"),
299 }
300 }
301
302 fn box_radius(half: f32) -> f32 {
303 half * 3.0_f32.sqrt()
304 }
305
306 fn assert_close(a: f32, b: f32, tol: f32) {
307 assert!(
308 (a - b).abs() <= tol,
309 "expected {a} close to {b} (tol {tol})"
310 );
311 }
312
313 #[test]
314 fn sphere_sphere_penetration_matches_distance() {
315 let a = Sphere {
316 center: Vec3::ZERO,
317 radius: 1.0,
318 };
319 let b = Sphere {
320 center: Vec3::new(1.5, 0.0, 0.0),
321 radius: 1.0,
322 };
323 let info = run(&a, &b, Vec3::new(1.5, 0.0, 0.0), 2.0);
324 assert_close(info.penetration, 0.5, 1e-3);
325 assert!(info.normal.dot(Vec3::X) > 0.99, "normal: {:?}", info.normal);
326 }
327
328 #[test]
329 fn box_box_axis_aligned_overlap_penetration_matches_axis() {
330 let va = box_vertices(Vec3::ZERO, Vec3::ONE);
331 let vb = box_vertices(Vec3::new(1.5, 0.0, 0.0), Vec3::ONE);
332 let a = ConvexHull { vertices: &va };
333 let b = ConvexHull { vertices: &vb };
334
335 let info = run(&a, &b, Vec3::new(1.5, 0.0, 0.0), 2.0 * box_radius(1.0));
336 assert!(
337 info.normal.dot(Vec3::X) > 0.99,
338 "normal must run from A toward B along +x, got {:?}",
339 info.normal
340 );
341 assert_close(info.penetration, 0.5, 1e-3);
342 }
343
344 #[test]
345 fn sphere_box_corner_penetration_points_outward() {
346 let vb = box_vertices(Vec3::ZERO, Vec3::ONE);
347 let b = ConvexHull { vertices: &vb };
348 let s = Sphere {
349 center: Vec3::new(1.2, 1.2, 1.2),
350 radius: 0.5,
351 };
352
353 let info = run(&b, &s, Vec3::new(1.0, 1.0, 1.0), box_radius(1.0) + 0.5);
354 let expected = Vec3::new(1.0, 1.0, 1.0).normalize();
355 assert!(
356 info.normal.dot(expected) > 0.95,
357 "normal {:?} not aligned with {:?}",
358 info.normal,
359 expected
360 );
361 assert_close(info.penetration, 0.5 - 3.0_f32.sqrt() * 0.2, 1e-2);
362 }
363
364 const SPHERE_PAIR_SCALE: f32 = 2.0;
365
366 fn seed(points: [Vec3; 4]) -> [MinkowskiPoint; 4] {
367 points.map(|point| MinkowskiPoint {
368 point,
369 sa: point,
370 sb: Vec3::ZERO,
371 })
372 }
373
374 fn seed_of_height(h: f32) -> [MinkowskiPoint; 4] {
375 seed([
376 Vec3::new(-1.0, -1.0, 0.0),
377 Vec3::new(1.0, -1.0, 0.0),
378 Vec3::new(0.0, 1.0, 0.0),
379 Vec3::new(0.0, 0.0, h),
380 ])
381 }
382
383 #[test]
384 fn seeds_across_the_volume_floor_resolve_finitely_or_not_at_all() {
385 let a = Sphere {
386 center: Vec3::ZERO,
387 radius: 1.0,
388 };
389 let b = Sphere {
390 center: Vec3::new(0.5, 0.0, 0.0),
391 radius: 1.0,
392 };
393
394 assert!(
395 epa(&a, &b, seed_of_height(0.0), SPHERE_PAIR_SCALE).is_none(),
396 "a seed with no 3-volume has no interior to orient against"
397 );
398 for h in [1e-6, 1e-4, 1e-2, 1.0] {
399 let contact = epa(&a, &b, seed_of_height(h), SPHERE_PAIR_SCALE)
400 .unwrap_or_else(|| panic!("seed of height {h} clears the volume floor"));
401 assert!(
402 contact.normal.is_finite()
403 && contact.point.is_finite()
404 && contact.penetration.is_finite(),
405 "seed of height {h} resolved to {contact:?}"
406 );
407 assert!(
408 contact.penetration >= 0.0,
409 "seed of height {h}: negative depth"
410 );
411 }
412 }
413
414 #[test]
415 fn the_seed_volume_floor_admits_the_same_seed_shapes_at_every_size() {
416 fn resized(seed: [MinkowskiPoint; 4], s: f32) -> [MinkowskiPoint; 4] {
417 seed.map(|p| MinkowskiPoint {
418 point: p.point * s,
419 sa: p.sa * s,
420 sb: p.sb * s,
421 })
422 }
423
424 for s in [1e-3_f32, 1.0, 1e3] {
425 let a = Sphere {
426 center: Vec3::ZERO,
427 radius: s,
428 };
429 let b = Sphere {
430 center: Vec3::new(0.5 * s, 0.0, 0.0),
431 radius: s,
432 };
433 let scale = SPHERE_PAIR_SCALE * s;
434 assert!(
435 epa(&a, &b, resized(seed_of_height(1e-9), s), scale).is_none(),
436 "at size {s} a seed 20x under the volume floor resolved a contact"
437 );
438 assert!(
439 epa(&a, &b, resized(seed_of_height(1e-6), s), scale).is_some(),
440 "at size {s} a seed 50x over the volume floor resolved to nothing"
441 );
442 }
443 }
444
445 #[test]
446 fn collinear_and_repeated_vertex_seeds_are_rejected_rather_than_resolved() {
447 let a = Sphere {
448 center: Vec3::ZERO,
449 radius: 1.0,
450 };
451 let b = Sphere {
452 center: Vec3::new(0.5, 0.0, 0.0),
453 radius: 1.0,
454 };
455 let collinear = seed([
456 Vec3::new(-1.0, 0.0, 0.0),
457 Vec3::new(-0.5, 0.0, 0.0),
458 Vec3::new(0.5, 0.0, 0.0),
459 Vec3::new(1.0, 0.0, 0.0),
460 ]);
461 let repeated = seed([
462 Vec3::new(-1.0, -1.0, -1.0),
463 Vec3::new(1.0, -1.0, -1.0),
464 Vec3::new(1.0, -1.0, -1.0),
465 Vec3::new(0.0, 1.0, 1.0),
466 ]);
467 assert!(epa(&a, &b, collinear, SPHERE_PAIR_SCALE).is_none());
468 assert!(epa(&a, &b, repeated, SPHERE_PAIR_SCALE).is_none());
469 }
470
471 #[test]
472 fn deeply_nested_boxes_contact_matches_shallowest_axis() {
473 let va = box_vertices(Vec3::ZERO, Vec3::ONE);
474 let vb = box_vertices(Vec3::new(0.3, 0.1, 0.2), Vec3::ONE);
475 let a = ConvexHull { vertices: &va };
476 let b = ConvexHull { vertices: &vb };
477 let info = run(&a, &b, Vec3::new(0.3, 0.1, 0.2), 2.0 * box_radius(1.0));
478 assert_close(info.penetration, 2.0 - 0.3, EPA_TOLERANCE);
479 assert!(
480 info.normal.dot(Vec3::X) > 0.999,
481 "normal must run from A toward B along +x, got {:?}",
482 info.normal
483 );
484 }
485
486 #[test]
487 fn flush_box_contacts_resolve_against_a_difference_body_facet() {
488 let cases: [(f32, Vec3, &[Vec3]); 5] = [
489 (1.0, Vec3::new(0.1, 0.1, 0.0), &[Vec3::X, Vec3::Y]),
490 (1.0, Vec3::new(0.1, 0.05, 0.0), &[Vec3::X]),
491 (1.0, Vec3::new(0.0, 0.05, 0.1), &[Vec3::Z]),
492 (0.1, Vec3::new(0.002, 0.0, 0.0), &[Vec3::X]),
493 (0.1, Vec3::new(0.0, 0.0, -0.002), &[Vec3::NEG_Z]),
494 ];
495
496 for (half, offset, admissible) in cases {
497 let va = box_vertices(Vec3::ZERO, Vec3::splat(half));
498 let vb = box_vertices(offset, Vec3::splat(half));
499 let a = ConvexHull { vertices: &va };
500 let b = ConvexHull { vertices: &vb };
501
502 let info = run(&a, &b, offset, 2.0 * box_radius(half));
503 assert_close(
504 info.penetration,
505 2.0 * half - offset.abs().max_element(),
506 EPA_TOLERANCE,
507 );
508 let alignment = admissible.iter().fold(f32::NEG_INFINITY, |best, &axis| {
509 best.max(info.normal.dot(axis))
510 });
511 assert!(
512 alignment > 0.999,
513 "half {half}, offset {offset:?}: normal {:?} on none of {admissible:?}",
514 info.normal
515 );
516 }
517 }
518
519 #[test]
520 fn two_axis_flush_expansion_terminates_with_a_bounded_face_count() {
521 let offset = Vec3::new(0.0, 0.0, -0.01);
522 let va = box_vertices(Vec3::ZERO, Vec3::splat(0.1));
523 let vb = box_vertices(offset, Vec3::splat(0.1));
524 let a = ConvexHull { vertices: &va };
525 let b = ConvexHull { vertices: &vb };
526
527 let simplex = match gjk_intersect(&a, &b, offset) {
528 GjkResult::Intersecting { simplex } => simplex,
529 GjkResult::Separated => panic!("boxes overlap; GJK must report intersecting"),
530 };
531 let (polytope, face) = expand_to_boundary(&a, &b, simplex, 2.0 * box_radius(0.1))
532 .expect("EPA should converge");
533
534 assert!(
535 polytope.faces.len() <= 24,
536 "expansion left {} faces",
537 polytope.faces.len()
538 );
539
540 let info = contact_from_face(&polytope, face);
541 assert_close(info.penetration, 0.19, EPA_TOLERANCE);
542 assert!(
543 info.normal.dot(Vec3::NEG_Z) > 0.999,
544 "normal must run from A toward B along -z, got {:?}",
545 info.normal
546 );
547 }
548
549 #[test]
550 fn epa_contact_is_equivariant_under_uniform_scaling() {
551 fn scaled(simplex: [MinkowskiPoint; 4], s: f32) -> [MinkowskiPoint; 4] {
552 simplex.map(|p| MinkowskiPoint {
553 point: p.point * s,
554 sa: p.sa * s,
555 sb: p.sb * s,
556 })
557 }
558
559 fn seed_at_unit_scale(
560 a: &impl SupportFn,
561 b: &impl SupportFn,
562 d: Vec3,
563 ) -> [MinkowskiPoint; 4] {
564 match gjk_intersect(a, b, d) {
565 GjkResult::Intersecting { simplex } => simplex,
566 GjkResult::Separated => panic!("unit-scale fixture must overlap"),
567 }
568 }
569
570 let box_offsets = [Vec3::new(0.3, 0.1, 0.2), Vec3::new(0.1, 0.05, 0.0)];
571 let unit_seeds: Vec<[MinkowskiPoint; 4]> = box_offsets
572 .iter()
573 .map(|&offset| {
574 let va = box_vertices(Vec3::ZERO, Vec3::ONE);
575 let vb = box_vertices(offset, Vec3::ONE);
576 seed_at_unit_scale(
577 &ConvexHull { vertices: &va },
578 &ConvexHull { vertices: &vb },
579 offset,
580 )
581 })
582 .collect();
583 let sphere_seed = seed_at_unit_scale(
584 &Sphere {
585 center: Vec3::ZERO,
586 radius: 1.0,
587 },
588 &Sphere {
589 center: Vec3::new(1.5, 0.0, 0.0),
590 radius: 1.0,
591 },
592 Vec3::X,
593 );
594
595 for s in [1e-3_f32, 1e-2, 1.0, 1e2, 1e3] {
596 let sphere_a = Sphere {
597 center: Vec3::ZERO,
598 radius: s,
599 };
600 let sphere_b = Sphere {
601 center: Vec3::new(1.5 * s, 0.0, 0.0),
602 radius: s,
603 };
604 let scale = 2.0 * s;
605 let contact = epa(&sphere_a, &sphere_b, scaled(sphere_seed, s), scale)
606 .unwrap_or_else(|| panic!("spheres at scale {s} resolved to no contact"));
607 assert!(
608 (contact.penetration - 0.5 * s).abs() <= EPA_TOLERANCE * scale,
609 "spheres at scale {s}: depth {} is not {}",
610 contact.penetration,
611 0.5 * s
612 );
613 assert!(
614 contact.normal.dot(Vec3::X) > 0.999,
615 "spheres at scale {s}: normal {:?}",
616 contact.normal
617 );
618
619 for (&offset, &unit_seed) in box_offsets.iter().zip(unit_seeds.iter()) {
620 let va = box_vertices(Vec3::ZERO, Vec3::splat(s));
621 let vb = box_vertices(offset * s, Vec3::splat(s));
622 let a = ConvexHull { vertices: &va };
623 let b = ConvexHull { vertices: &vb };
624 let scale = 2.0 * box_radius(s);
625 let contact = epa(&a, &b, scaled(unit_seed, s), scale)
626 .unwrap_or_else(|| panic!("boxes {offset:?} at scale {s}: no contact"));
627 let want = (2.0 - offset.abs().max_element()) * s;
628 assert!(
629 (contact.penetration - want).abs() <= EPA_TOLERANCE * scale,
630 "boxes {offset:?} at scale {s}: depth {} is not {want}",
631 contact.penetration
632 );
633 assert!(
634 contact.normal.dot(Vec3::X) > 0.999,
635 "boxes {offset:?} at scale {s}: normal {:?}",
636 contact.normal
637 );
638 }
639 }
640 }
641}