1use glam::Vec4;
2
3use super::gjk_r4::{minkowski_support_r4, MinkowskiPoint4, SupportFn4};
4use super::simplex_r4::project_origin_onto_affine_hull;
5
6const EPA_MAX_ITERATIONS: u32 = 96;
7
8const EPA_TOLERANCE: f32 = 1e-3;
9
10#[derive(Clone, Copy, Debug)]
11pub struct ContactInfo4 {
12 pub normal: Vec4,
13 pub penetration: f32,
14 pub point: Vec4,
15}
16
17#[derive(Clone, Copy, Debug)]
18struct Face4 {
19 v: [usize; 4],
20 normal: Vec4,
21 distance: f32,
22}
23
24#[derive(Clone, Copy)]
25struct Thresholds {
26 support_gap: f32,
27 coplanar_band: f32,
28 wedge_norm: f32,
29}
30
31impl Thresholds {
32 fn for_scale(scale: f32) -> Self {
33 Self {
34 support_gap: EPA_TOLERANCE * scale,
35 coplanar_band: FACE_COPLANAR_EPS * scale,
36 wedge_norm: FACE_DEGENERATE_WEDGE * scale.powi(3),
37 }
38 }
39}
40
41struct Polytope4 {
42 vertices: Vec<MinkowskiPoint4>,
43 faces: Vec<Face4>,
44 horizon: Vec<Triangle>,
45 centroid: Vec4,
46 thresholds: Thresholds,
47}
48
49impl Polytope4 {
50 fn from_simplex(simplex: [MinkowskiPoint4; 5], thresholds: Thresholds) -> Self {
51 let vertices = simplex.to_vec();
52 let centroid = (simplex[0].point
53 + simplex[1].point
54 + simplex[2].point
55 + simplex[3].point
56 + simplex[4].point)
57 * 0.2;
58
59 let mut faces = Vec::with_capacity(5);
60 for l in 0..5 {
61 let mut tet = [0usize; 4];
62 let mut idx = 0;
63 for i in 0..5 {
64 if i != l {
65 tet[idx] = i;
66 idx += 1;
67 }
68 }
69 if let Some(face) = build_face(
70 &vertices,
71 tet[0],
72 tet[1],
73 tet[2],
74 tet[3],
75 centroid,
76 thresholds.wedge_norm,
77 ) {
78 faces.push(face);
79 }
80 }
81 Self {
82 vertices,
83 faces,
84 horizon: Vec::new(),
85 centroid,
86 thresholds,
87 }
88 }
89
90 fn closest_face(&self) -> Option<usize> {
92 self.faces
93 .iter()
94 .enumerate()
95 .min_by(|a, b| a.1.distance.total_cmp(&b.1.distance))
96 .map(|(idx, _)| idx)
97 }
98
99 fn expand(&mut self, support: MinkowskiPoint4) {
100 let new_idx = self.vertices.len();
101 self.vertices.push(support);
102
103 self.horizon.clear();
104 let horizon = &mut self.horizon;
105 let vertices = &self.vertices;
106
107 let coplanar_band = self.thresholds.coplanar_band;
108 self.faces.retain(|f| {
109 let view = support.point - vertices[f.v[0]].point;
110 if f.normal.dot(view) > -coplanar_band {
111 for tri in tet_triangles(&f.v) {
112 add_or_remove_triangle(horizon, tri);
113 }
114 false
115 } else {
116 true
117 }
118 });
119
120 let centroid = self.centroid;
121 let wedge_norm = self.thresholds.wedge_norm;
122 for tri in horizon.iter() {
123 if let Some(face) = build_face(
124 &self.vertices,
125 tri.0,
126 tri.1,
127 tri.2,
128 new_idx,
129 centroid,
130 wedge_norm,
131 ) {
132 self.faces.push(face);
133 }
134 }
135 }
136}
137
138const FACE_COPLANAR_EPS: f32 = 1e-5;
139
140const FACE_DEGENERATE_WEDGE: f32 = 1e-8;
141
142const SEED_DEGENERATE_VOLUME: f32 = 1e-8;
143
144type Triangle = (usize, usize, usize);
145
146fn tet_triangles(tet: &[usize; 4]) -> [Triangle; 4] {
147 let (a, b, c, d) = (tet[0], tet[1], tet[2], tet[3]);
148 [(a, b, c), (a, b, d), (a, c, d), (b, c, d)]
149}
150
151fn add_or_remove_triangle(horizon: &mut Vec<Triangle>, tri: Triangle) {
152 let key = sort_triangle(tri);
153 if let Some(pos) = horizon.iter().position(|t| sort_triangle(*t) == key) {
154 horizon.swap_remove(pos);
155 } else {
156 horizon.push(tri);
157 }
158}
159
160fn sort_triangle(t: Triangle) -> (usize, usize, usize) {
161 let mut a = [t.0, t.1, t.2];
162 a.sort_unstable();
163 (a[0], a[1], a[2])
164}
165
166fn build_face(
168 verts: &[MinkowskiPoint4],
169 a: usize,
170 b: usize,
171 c: usize,
172 d: usize,
173 centroid: Vec4,
174 wedge_norm_floor: f32,
175) -> Option<Face4> {
176 let pa = verts[a].point;
177 let pb = verts[b].point;
178 let pc = verts[c].point;
179 let pd = verts[d].point;
180
181 let raw_normal = hodge_dual_of_trivector_wedge(pb - pa, pc - pa, pd - pa);
182 let len = raw_normal.length();
183 if len < wedge_norm_floor {
184 return None;
185 }
186 let normal = raw_normal / len;
187
188 let flip = normal.dot(centroid - pa) > 0.0;
189
190 let (outward, v_order) = if flip {
191 (-normal, [a, b, d, c])
192 } else {
193 (normal, [a, b, c, d])
194 };
195
196 let distance = outward.dot(pa).max(0.0);
197
198 Some(Face4 {
199 v: v_order,
200 normal: outward,
201 distance,
202 })
203}
204
205fn hodge_dual_of_trivector_wedge(u: Vec4, v: Vec4, w: Vec4) -> Vec4 {
206 let t_234 = det3(u.y, u.z, u.w, v.y, v.z, v.w, w.y, w.z, w.w);
207 let t_134 = det3(u.x, u.z, u.w, v.x, v.z, v.w, w.x, w.z, w.w);
208 let t_124 = det3(u.x, u.y, u.w, v.x, v.y, v.w, w.x, w.y, w.w);
209 let t_123 = det3(u.x, u.y, u.z, v.x, v.y, v.z, w.x, w.y, w.z);
210
211 Vec4::new(t_234, -t_134, t_124, -t_123)
212}
213
214#[inline]
215#[allow(clippy::too_many_arguments)]
216fn det3(
217 a00: f32,
218 a01: f32,
219 a02: f32,
220 a10: f32,
221 a11: f32,
222 a12: f32,
223 a20: f32,
224 a21: f32,
225 a22: f32,
226) -> f32 {
227 a00 * (a11 * a22 - a12 * a21) - a01 * (a10 * a22 - a12 * a20) + a02 * (a10 * a21 - a11 * a20)
228}
229
230pub fn epa_r4<A: SupportFn4, B: SupportFn4>(
232 a: &A,
233 b: &B,
234 initial_simplex: [MinkowskiPoint4; 5],
235 scale: f32,
236) -> Option<ContactInfo4> {
237 let thresholds = Thresholds::for_scale(scale);
238
239 let p0 = initial_simplex[0].point;
240 let d1 = initial_simplex[1].point - p0;
241 let d2 = initial_simplex[2].point - p0;
242 let d3 = initial_simplex[3].point - p0;
243 let d4 = initial_simplex[4].point - p0;
244 let volume = det4(d1, d2, d3, d4).abs();
245 if volume < SEED_DEGENERATE_VOLUME * scale.powi(4) {
246 return None;
247 }
248
249 let mut polytope = Polytope4::from_simplex(initial_simplex, thresholds);
250
251 for _ in 0..EPA_MAX_ITERATIONS {
252 let face_idx = polytope.closest_face()?;
253 let face = polytope.faces[face_idx];
254
255 let support = minkowski_support_r4(a, b, face.normal);
256 let new_distance = support.point.dot(face.normal);
257
258 if !new_distance.is_finite() || !support.point.is_finite() {
259 return None;
260 }
261
262 if (new_distance - face.distance).abs() < thresholds.support_gap {
263 return Some(contact_from_face(&polytope, face));
264 }
265
266 polytope.expand(support);
267 }
268
269 tracing::debug!(
270 max_iterations = EPA_MAX_ITERATIONS,
271 vertices = polytope.vertices.len(),
272 "EPA 4D hit iteration cap; returning best-estimate contact",
273 );
274 let face_idx = polytope.closest_face()?;
275 Some(contact_from_face(&polytope, polytope.faces[face_idx]))
276}
277
278fn det4(r0: Vec4, r1: Vec4, r2: Vec4, r3: Vec4) -> f32 {
279 r0.x * det3(r1.y, r1.z, r1.w, r2.y, r2.z, r2.w, r3.y, r3.z, r3.w)
280 - r0.y * det3(r1.x, r1.z, r1.w, r2.x, r2.z, r2.w, r3.x, r3.z, r3.w)
281 + r0.z * det3(r1.x, r1.y, r1.w, r2.x, r2.y, r2.w, r3.x, r3.y, r3.w)
282 - r0.w * det3(r1.x, r1.y, r1.z, r2.x, r2.y, r2.z, r3.x, r3.y, r3.z)
283}
284
285fn contact_from_face(polytope: &Polytope4, face: Face4) -> ContactInfo4 {
286 let tetra = face.v.map(|i| polytope.vertices[i]);
287
288 let closest = face.normal * face.distance;
289 let weights = face_barycentrics(&tetra.map(|p| p.point), closest);
290
291 let mut point_a = Vec4::ZERO;
292 let mut point_b = Vec4::ZERO;
293 for (vertex, w) in tetra.iter().zip(weights) {
294 point_a += vertex.sa * w;
295 point_b += vertex.sb * w;
296 }
297
298 ContactInfo4 {
299 normal: face.normal,
300 penetration: face.distance,
301 point: (point_a + point_b) * 0.5,
302 }
303}
304
305fn face_barycentrics(points: &[Vec4; 4], closest: Vec4) -> [f32; 4] {
306 let shifted = points.map(|p| p - closest);
307 match project_origin_onto_affine_hull(&[0, 1, 2, 3], &shifted) {
308 Some((_, w)) => [w[0], w[1], w[2], w[3]],
309 None => [0.25; 4],
310 }
311}
312
313#[cfg(test)]
314mod tests {
315 use super::super::gjk_r4::{gjk_intersect_r4, ConvexHull4, GjkResult4, Sphere4};
316 use super::*;
317
318 fn assert_close(a: f32, b: f32, tol: f32) {
319 assert!(
320 (a - b).abs() <= tol,
321 "{a} not close to {b} (tol {tol}, diff {})",
322 (a - b).abs()
323 );
324 }
325
326 const UNIT_POLYCHORON_SCALE: f32 = 2.0;
327
328 #[test]
329 fn sphere_sphere_penetration_matches_analytical() {
330 let a = Sphere4 {
331 center: Vec4::new(0.0, 0.0, 0.0, 0.0),
332 radius: 0.5,
333 };
334 let b = Sphere4 {
335 center: Vec4::new(0.8, 0.0, 0.0, 0.0),
336 radius: 0.5,
337 };
338 let simplex = match gjk_intersect_r4(&a, &b, Vec4::X) {
339 GjkResult4::Intersecting { simplex } => simplex,
340 _ => panic!("spheres should overlap"),
341 };
342 let contact = epa_r4(&a, &b, simplex, 1.0).expect("EPA should succeed");
343 assert_close(contact.penetration, 0.2, 5e-3);
344 assert!(
345 contact.normal.dot(Vec4::X) > 0.99,
346 "normal must run from A toward B along +x, got {:?}",
347 contact.normal
348 );
349 }
350
351 #[test]
352 fn contact_from_face_realizes_the_plane_projection_outside_the_tetra() {
353 use super::super::simplex_r4::closest_to_origin;
354
355 let points = [
356 Vec4::new(1.0, 1.0, 0.0, 0.0),
357 Vec4::new(1.0, 0.0, 1.0, 0.0),
358 Vec4::new(1.0, 0.0, 0.0, 1.0),
359 Vec4::new(1.0, 6.0, -3.0, 2.0),
360 ];
361 let pre_images_b = [
362 Vec4::new(0.0, 1.0, 0.0, 0.0),
363 Vec4::new(0.0, 0.0, 1.0, 0.0),
364 Vec4::new(0.0, 0.0, 0.0, 1.0),
365 Vec4::new(1.0, 0.0, 0.0, 0.0),
366 ];
367 let vertices: Vec<MinkowskiPoint4> = points
368 .iter()
369 .zip(pre_images_b)
370 .map(|(&point, sb)| MinkowskiPoint4 {
371 point,
372 sa: point + sb,
373 sb,
374 })
375 .collect();
376
377 let clamped = closest_to_origin(&points.map(|p| p - Vec4::X));
378 assert_eq!(clamped.kept(), &[0, 1, 2]);
379
380 let weights = [1.5, -0.75, 0.5, -0.25];
381 let realized = points
382 .iter()
383 .zip(weights)
384 .fold(Vec4::ZERO, |acc, (&p, w)| acc + p * w);
385 assert!((realized - Vec4::X).length() < 1e-6, "{realized:?}");
386
387 let solved = face_barycentrics(&points, Vec4::X);
388 assert!(
389 solved
390 .iter()
391 .zip(weights)
392 .all(|(&got, want)| (got - want).abs() < 1e-5),
393 "weights {solved:?} should be {weights:?}"
394 );
395
396 let centroid = (points[0] + points[1] + points[2] + points[3]) * 0.25;
397 let thresholds = Thresholds::for_scale(1.0);
398 let face = build_face(&vertices, 0, 1, 2, 3, centroid, thresholds.wedge_norm)
399 .expect("tetra is non-degenerate");
400 assert_close(face.distance, 1.0, 1e-6);
401
402 let polytope = Polytope4 {
403 vertices,
404 faces: vec![face],
405 horizon: Vec::new(),
406 centroid,
407 thresholds,
408 };
409 let contact = contact_from_face(&polytope, face);
410
411 let expected = Vec4::new(0.25, 1.5, -0.75, 0.5);
412 assert!(
413 (contact.point - expected).length() < 1e-5,
414 "contact {:?} should be {expected:?}",
415 contact.point
416 );
417 }
418
419 const SPHERE_PAIR_SCALE: f32 = 2.0;
420
421 fn seed(points: [Vec4; 5]) -> [MinkowskiPoint4; 5] {
422 points.map(|point| MinkowskiPoint4 {
423 point,
424 sa: point,
425 sb: Vec4::ZERO,
426 })
427 }
428
429 fn seed_of_height(h: f32) -> [MinkowskiPoint4; 5] {
430 seed([
431 Vec4::new(-1.0, -1.0, -1.0, 0.0),
432 Vec4::new(1.0, -1.0, -1.0, 0.0),
433 Vec4::new(0.0, 1.0, -1.0, 0.0),
434 Vec4::new(0.0, 0.0, 1.0, 0.0),
435 Vec4::new(0.0, 0.0, 0.0, h),
436 ])
437 }
438
439 #[test]
440 fn seeds_across_the_volume_floor_resolve_finitely_or_not_at_all() {
441 let a = Sphere4 {
442 center: Vec4::ZERO,
443 radius: 1.0,
444 };
445 let b = Sphere4 {
446 center: Vec4::new(0.5, 0.0, 0.0, 0.0),
447 radius: 1.0,
448 };
449
450 assert!(
451 epa_r4(&a, &b, seed_of_height(0.0), SPHERE_PAIR_SCALE).is_none(),
452 "a seed inside a hyperplane has no interior to orient against"
453 );
454 for h in [1e-6, 1e-4, 1e-2, 1.0] {
455 let contact = epa_r4(&a, &b, seed_of_height(h), SPHERE_PAIR_SCALE)
456 .unwrap_or_else(|| panic!("seed of height {h} clears the volume floor"));
457 assert!(
458 contact.normal.is_finite()
459 && contact.point.is_finite()
460 && contact.penetration.is_finite(),
461 "seed of height {h} resolved to {contact:?}"
462 );
463 assert!(
464 contact.penetration >= 0.0,
465 "seed of height {h}: negative depth"
466 );
467 }
468 }
469
470 #[test]
471 fn the_seed_volume_floor_admits_the_same_seed_shapes_at_every_size() {
472 fn resized(seed: [MinkowskiPoint4; 5], s: f32) -> [MinkowskiPoint4; 5] {
473 seed.map(|p| MinkowskiPoint4 {
474 point: p.point * s,
475 sa: p.sa * s,
476 sb: p.sb * s,
477 })
478 }
479
480 for s in [1e-3_f32, 1.0, 1e3] {
481 let a = Sphere4 {
482 center: Vec4::ZERO,
483 radius: s,
484 };
485 let b = Sphere4 {
486 center: Vec4::new(0.5 * s, 0.0, 0.0, 0.0),
487 radius: s,
488 };
489 let scale = SPHERE_PAIR_SCALE * s;
490 assert!(
491 epa_r4(&a, &b, resized(seed_of_height(1e-9), s), scale).is_none(),
492 "at size {s} a seed 20x under the volume floor resolved a contact"
493 );
494 assert!(
495 epa_r4(&a, &b, resized(seed_of_height(1e-6), s), scale).is_some(),
496 "at size {s} a seed 50x over the volume floor resolved to nothing"
497 );
498 }
499 }
500
501 #[test]
502 fn collinear_and_repeated_vertex_seeds_are_rejected_rather_than_resolved() {
503 let a = Sphere4 {
504 center: Vec4::ZERO,
505 radius: 1.0,
506 };
507 let b = Sphere4 {
508 center: Vec4::new(0.5, 0.0, 0.0, 0.0),
509 radius: 1.0,
510 };
511 let collinear = seed([
512 Vec4::new(-1.0, 0.0, 0.0, 0.0),
513 Vec4::new(-0.5, 0.0, 0.0, 0.0),
514 Vec4::ZERO,
515 Vec4::new(0.5, 0.0, 0.0, 0.0),
516 Vec4::new(1.0, 0.0, 0.0, 0.0),
517 ]);
518 let mut repeated = seed_of_height(1.0);
519 repeated[2] = repeated[1];
520 assert!(epa_r4(&a, &b, collinear, SPHERE_PAIR_SCALE).is_none());
521 assert!(epa_r4(&a, &b, repeated, SPHERE_PAIR_SCALE).is_none());
522 }
523
524 fn box4_vertices(half: Vec4) -> Vec<Vec4> {
525 let mut vertices = Vec::with_capacity(16);
526 for &x in &[-half.x, half.x] {
527 for &y in &[-half.y, half.y] {
528 for &z in &[-half.z, half.z] {
529 for &w in &[-half.w, half.w] {
530 vertices.push(Vec4::new(x, y, z, w));
531 }
532 }
533 }
534 }
535 vertices
536 }
537
538 const WALL_HALF: f32 = 0.05;
539 const WALL_SPAN: f32 = 2.0;
540 const BALL_RADIUS: f32 = 0.1;
541 const CAPTURE: f32 = WALL_HALF + BALL_RADIUS;
542 fn wall_scale() -> f32 {
543 BALL_RADIUS + Vec4::new(WALL_HALF, WALL_SPAN, WALL_SPAN, WALL_SPAN).length()
544 }
545
546 fn ball_vs_wall(x: f32) -> ContactInfo4 {
547 let vertices = box4_vertices(Vec4::new(WALL_HALF, WALL_SPAN, WALL_SPAN, WALL_SPAN));
548 let wall = ConvexHull4 {
549 vertices: &vertices,
550 };
551 let ball = Sphere4 {
552 center: Vec4::new(x, 0.0, 0.0, 0.0),
553 radius: BALL_RADIUS,
554 };
555 let simplex = match gjk_intersect_r4(&ball, &wall, -ball.center) {
556 GjkResult4::Intersecting { simplex } => simplex,
557 GjkResult4::Separated => panic!("ball at {x} overlaps the wall"),
558 };
559 epa_r4(&ball, &wall, simplex, wall_scale()).expect("EPA should resolve an overlap")
560 }
561
562 #[test]
563 fn wall_contact_leaves_through_the_face_the_ball_entered() {
564 for side in [-1.0_f32, 1.0] {
565 for k in 1..(CAPTURE / 5e-4) as u32 {
566 let x = side * (CAPTURE - 5e-4 * k as f32);
567 let contact = ball_vs_wall(x);
568 assert!(
569 contact.normal.x * side < -0.99,
570 "ball at {x} leaves along {:?}, not back out of its own face",
571 -contact.normal
572 );
573 assert_close(contact.penetration, CAPTURE - x.abs(), EPA_TOLERANCE);
574 }
575 }
576 }
577
578 #[test]
579 fn wall_contact_rejects_gaps_and_resolves_overlap() {
580 let vertices = box4_vertices(Vec4::new(WALL_HALF, WALL_SPAN, WALL_SPAN, WALL_SPAN));
581 let wall = ConvexHull4 {
582 vertices: &vertices,
583 };
584 for gap in [1e-3_f32, 1e-4, 1e-5] {
585 let ball = Sphere4 {
586 center: Vec4::X * -(CAPTURE + gap),
587 radius: BALL_RADIUS,
588 };
589 assert!(
590 matches!(
591 gjk_intersect_r4(&ball, &wall, Vec4::X),
592 GjkResult4::Separated
593 ),
594 "gap={gap}"
595 );
596 }
597 for overlap in [1e-5_f32, 1e-4, 1e-3, 1e-2] {
598 let contact = ball_vs_wall(-(CAPTURE - overlap));
599 assert_close(contact.penetration, overlap, EPA_TOLERANCE);
600 assert!(
601 contact.normal.x > 0.99,
602 "an overlap of {overlap} leaves along {:?}",
603 -contact.normal
604 );
605 }
606 }
607
608 #[test]
609 fn tesseract_tesseract_contact_matches_deepest_axis() {
610 use crate::collision::gjk_r4::ConvexHull4;
611 use crate::euclidean_r4::tesseract_vertices;
612
613 let va: Vec<Vec4> = tesseract_vertices(1.0);
614 let vb: Vec<Vec4> = tesseract_vertices(1.0)
615 .into_iter()
616 .map(|v| v + Vec4::new(0.4, 0.2, 0.1, 0.0))
617 .collect();
618
619 let a = ConvexHull4 { vertices: &va };
620 let b = ConvexHull4 { vertices: &vb };
621 let simplex = match gjk_intersect_r4(&a, &b, Vec4::X) {
622 GjkResult4::Intersecting { simplex } => simplex,
623 _ => panic!("tesseracts should overlap"),
624 };
625 let contact = epa_r4(&a, &b, simplex, UNIT_POLYCHORON_SCALE).expect("EPA should succeed");
626
627 assert_close(contact.penetration, 1.0 - 0.4, EPA_TOLERANCE);
628 assert!(
629 contact.normal.dot(Vec4::X) > 0.999,
630 "normal must run from A toward B along +x, got {:?}",
631 contact.normal
632 );
633 }
634
635 #[test]
636 fn epa_r4_contact_is_equivariant_under_uniform_scaling() {
637 use crate::euclidean_r4::{
638 cell120_vertices, cell16_vertices, cell24_vertices, cell600_vertices,
639 pentatope_vertices, tesseract_vertices,
640 };
641
642 let root5 = 5.0_f32.sqrt();
643 let root6 = 6.0_f32.sqrt();
644 let phi = (1.0 + root5) * 0.5;
645 let inradius600 = phi * phi / (2.0 * 2.0_f32.sqrt());
646
647 type ScaleFixture = (&'static str, fn(f32) -> Vec<Vec4>, f32, f32);
648 let fixtures: [ScaleFixture; 6] = [
649 (
650 "pentatope",
651 pentatope_vertices,
652 5.0 / (2.0 * root6) - 0.3 * root5 / root6,
653 root5 / root6,
654 ),
655 ("tesseract", tesseract_vertices, 1.0 - 0.3, 1.0),
656 ("16-cell", cell16_vertices, 1.0 - 0.15, 0.5),
657 ("24-cell", cell24_vertices, 2.0_f32.sqrt() - 0.3, 1.0),
658 (
659 "600-cell",
660 cell600_vertices,
661 inradius600 * (2.0 - 0.3),
662 inradius600,
663 ),
664 (
665 "120-cell",
666 cell120_vertices,
667 phi * phi / 2.0_f32.sqrt() - 0.3,
668 1.0,
669 ),
670 ];
671
672 for (name, vertices_of, unit_depth, unit_normal_x) in fixtures {
673 for s in [1e-3_f32, 1e-2, 1.0, 1e2, 1e3] {
674 let va = vertices_of(s);
675 let vb: Vec<Vec4> = vertices_of(s)
676 .into_iter()
677 .map(|v| v + Vec4::new(0.3 * s, 0.0, 0.0, 0.0))
678 .collect();
679 let a = ConvexHull4 { vertices: &va };
680 let b = ConvexHull4 { vertices: &vb };
681 let simplex = match gjk_intersect_r4(&a, &b, Vec4::X) {
682 GjkResult4::Intersecting { simplex } => simplex,
683 GjkResult4::Separated => panic!("{name} at scale {s} should overlap"),
684 };
685
686 let scale = 2.0 * s;
687 let contact = epa_r4(&a, &b, simplex, scale)
688 .unwrap_or_else(|| panic!("{name} at scale {s} resolved to no contact"));
689
690 let want_depth = unit_depth * s;
691 assert!(
692 (contact.penetration - want_depth).abs() <= EPA_TOLERANCE * scale,
693 "{name} at scale {s}: depth {} is not {want_depth}",
694 contact.penetration
695 );
696 assert!(
697 (contact.normal.x - unit_normal_x).abs() <= 1e-3,
698 "{name} at scale {s}: normal {:?} has x != {unit_normal_x}",
699 contact.normal
700 );
701 assert!(
702 (contact.normal.length_squared() - 1.0).abs() < 1e-3,
703 "{name} at scale {s}: normal {:?} is not unit",
704 contact.normal
705 );
706 }
707 }
708 }
709}