1use crate::math::{Fix128, Vec3Fix};
11use crate::metric::MetricWeights;
12
13#[cfg(not(feature = "std"))]
14use alloc::vec::Vec;
15
16#[derive(Clone, Copy, Debug, PartialEq, Eq)]
22pub struct CollisionResult {
23 pub colliding: bool,
25 pub depth: Fix128,
27 pub normal: Vec3Fix,
31 pub point_a: Vec3Fix,
33 pub point_b: Vec3Fix,
35}
36
37impl CollisionResult {
38 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 #[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
60pub trait Support {
66 fn support(&self, direction: Vec3Fix) -> Vec3Fix;
68}
69
70#[derive(Clone, Copy, Debug, PartialEq, Eq)]
72pub struct Sphere {
73 pub center: Vec3Fix,
75 pub radius: Fix128,
77}
78
79impl Sphere {
80 #[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 let dir_norm = direction.rescaled_direction().normalize();
92 self.center + dir_norm * self.radius
93 }
94}
95
96#[derive(Clone, Copy, Debug, PartialEq, Eq)]
98pub struct AABB {
99 pub min: Vec3Fix,
101 pub max: Vec3Fix,
103}
104
105#[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 #[must_use]
129 pub const fn new(min: Vec3Fix, max: Vec3Fix) -> Self {
130 Self { min, max }
131 }
132
133 #[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 #[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 #[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 #[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 #[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#[derive(Clone, Debug)]
242pub struct ConvexHull {
243 pub vertices: Vec<Vec3Fix>,
245}
246
247impl ConvexHull {
248 #[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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
283pub struct Capsule {
284 pub a: Vec3Fix,
286 pub b: Vec3Fix,
288 pub radius: Fix128,
290}
291
292impl Capsule {
293 #[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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
322pub struct ScaledShape<S> {
323 pub shape: S,
325 pub scale: Fix128,
327}
328
329impl<S> ScaledShape<S> {
330 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 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#[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#[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 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#[derive(Clone, Copy, Debug, PartialEq, Eq)]
393pub struct GjkResult {
394 pub colliding: bool,
396 pub closest_point: Vec3Fix,
398}
399
400pub fn gjk<A: Support, B: Support>(a: &A, b: &B) -> GjkResult {
405 gjk_core(a, b).0
406}
407
408fn gjk_core<A: Support, B: Support>(a: &A, b: &B) -> (GjkResult, Simplex) {
412 const MAX_ITERATIONS: usize = 64;
413
414 let mut direction = Vec3Fix::UNIT_X;
416
417 let mut simplex = Simplex::new();
419 simplex.push(minkowski_support(a, b, direction));
420
421 direction = -simplex.points[0];
423
424 for _ in 0..MAX_ITERATIONS {
425 if direction.length_squared().is_zero() {
426 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 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 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 let closest_point = simplex.points[0];
465 (
466 GjkResult {
467 colliding: false,
468 closest_point,
469 },
470 simplex,
471 )
472}
473
474fn 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#[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 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
547fn 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 let mut new_dir = ab.cross(ao).cross(ab);
566 if new_dir.length_squared().is_zero() {
569 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 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 true
656}
657
658#[derive(Clone, Copy, Debug, PartialEq, Eq)]
664pub struct Contact {
665 pub depth: Fix128,
667 pub normal: Vec3Fix,
669 pub point_a: Vec3Fix,
671 pub point_b: Vec3Fix,
673}
674
675#[derive(Clone, Copy, Debug, PartialEq)]
677struct EpaFace {
678 indices: [usize; 3],
679 normal: Vec3Fix,
680 distance: Fix128,
681}
682
683pub 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 }; 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 let interior =
718 (vertices[0] + vertices[1] + vertices[2] + vertices[3]) * Fix128::from_ratio(1, 4);
719
720 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 let mut best_upper: Option<(Fix128, Vec3Fix)> = None;
730
731 for _ in 0..MAX_ITERATIONS {
732 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 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 if distance - closest_face.distance < EPSILON {
748 let outward = closest_face.normal;
751 let depth = closest_face.distance;
752
753 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 let new_idx = vertices.len();
767 vertices.push(support);
768
769 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 for i in 0..3 {
779 let edge = (face.indices[i], face.indices[(i + 1) % 3]);
780 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 for (i, j) in edges {
795 add_face(&mut faces, &vertices, interior, i, j, new_idx);
796 }
797 }
798
799 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 if cross.length_squared().is_zero() {
839 return;
840 }
841
842 let normal = cross.normalize();
843
844 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 #[test]
875 fn raises_dimension_is_true_only_off_the_span() {
876 assert!(raises_dimension(&[p(1, 2, 3)], p(1, 2, 4)));
878 assert!(!raises_dimension(&[p(1, 2, 3)], p(1, 2, 3)));
879 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 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 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 let result = gjk(&a, &b);
968 assert!(!result.colliding, "Unscaled spheres should not collide");
969
970 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 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 assert_eq!(a.surface_area(), fi(88));
997 let b = AABB::new(v3i(-5, 1, 2), v3i(1, 3, 9));
998 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 let base = AABB::new(Vec3Fix::ZERO, v3i(1, 2, 3)); 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 ); assert_eq!(
1012 AABB::new(Vec3Fix::ZERO, v3i(1, 3, 3)).surface_area(),
1013 fi(30)
1014 ); assert_eq!(
1016 AABB::new(Vec3Fix::ZERO, v3i(1, 2, 4)).surface_area(),
1017 fi(28)
1018 ); 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 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 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 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 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 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 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 let corner_out = Sphere::new(v3i(2, 2, 2), Fix128::ONE); 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 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 assert_eq!(gjk(&a, &corner_in).colliding, gjk(&corner_in, &a).colliding);
1190 }
1191
1192 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 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 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 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 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 assert!(epa(&a, &b, &tetra(&a, &b)[..3]).is_none());
1259 assert!(epa(&a, &b, &[]).is_none());
1260 }
1261
1262 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 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 #[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 s.set(&[v3i(1, 0, 0); 6]);
1320 assert_eq!(s.size, 4);
1321 }
1322
1323 #[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 #[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 #[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 #[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 #[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 #[test]
1443 fn simplex_triangle_boundary_cases() {
1444 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 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 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 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 #[test]
1500 fn simplex_triangle_edge_ab_region_and_above_below_plane() {
1501 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 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 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 #[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 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 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 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 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 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 #[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 #[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 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 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 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 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 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 #[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 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 #[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 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;