1use crate::{scalars::Scalar, vector::Vector};
2
3const MAX_ITERS: usize = 64;
6
7const MAX_SIMPLEX: usize = 8;
24
25const MAX_GRAM: usize = MAX_SIMPLEX + 1;
28
29fn farthest_point<S: Scalar, const N: usize>(
31 points: &[Vector<S, N>],
32 d: &Vector<S, N>,
33) -> Vector<S, N> {
34 let mut best = points[0];
35 let mut best_dot = best.prod_dot(d);
36 for p in &points[1..] {
37 let dp = p.prod_dot(d);
38 if dp.definitely_greater(best_dot) {
39 best = *p;
40 best_dot = dp;
41 }
42 }
43 best
44}
45
46fn support<S: Scalar, const N: usize>(
48 a: &[Vector<S, N>],
49 b: &[Vector<S, N>],
50 d: &Vector<S, N>,
51) -> Vector<S, N> {
52 farthest_point(a, d).sub(&farthest_point(b, &d.neg()))
53}
54
55fn separates<S: Scalar, const N: usize>(
67 a: &[Vector<S, N>],
68 b: &[Vector<S, N>],
69 d: &Vector<S, N>,
70) -> bool {
71 a.iter().all(|pa| {
72 let da = pa.prod_dot(d);
73 b.iter().all(|pb| da.definitely_less(pb.prod_dot(d)))
74 })
75}
76
77fn axis_separates<S: Scalar, const N: usize>(a: &[Vector<S, N>], b: &[Vector<S, N>]) -> bool {
90 for axis in 0..N {
91 let mut e = Vector::<S, N>::zero();
92 e[axis] = S::ONE;
93 if separates(a, b, &e) || separates(b, a, &e) {
94 return true;
95 }
96 }
97 false
98}
99
100fn solve_barycentric<S: Scalar, const N: usize>(
117 pts: &[Vector<S, N>],
118) -> Option<([S; MAX_SIMPLEX], usize)> {
119 let m = pts.len();
120 if m == 1 {
121 let mut lambda = [S::ZERO; MAX_SIMPLEX];
122 lambda[0] = S::ONE;
123 return Some((lambda, 1));
124 }
125
126 let n = m + 1;
127 let mut a = [[S::ZERO; MAX_GRAM]; MAX_GRAM];
128 for i in 0..m {
129 for j in 0..m {
130 a[i][j] = pts[i].prod_dot(&pts[j]);
131 }
132 a[i][m] = S::ONE;
133 a[m][i] = S::ONE;
134 }
135
136 let mut rhs = [S::ZERO; MAX_GRAM];
137 rhs[m] = S::ONE;
138
139 for col in 0..n {
140 let pivot = a[col][col];
141 for row in (col + 1)..n {
142 let factor = a[row][col].div(pivot).ok()?;
143 for c in col..n {
144 a[row][c] = a[row][c].sub(factor.mul(a[col][c]));
145 }
146 rhs[row] = rhs[row].sub(factor.mul(rhs[col]));
147 }
148 }
149
150 let mut x = [S::ZERO; MAX_GRAM];
151 for row in (0..n).rev() {
152 let mut sum = rhs[row];
153 for col in (row + 1)..n {
154 sum = sum.sub(a[row][col].mul(x[col]));
155 }
156 x[row] = sum.div(a[row][row]).ok()?;
157 }
158
159 let mut lambda = [S::ZERO; MAX_SIMPLEX];
160 lambda[..m].copy_from_slice(&x[..m]);
161 Some((lambda, m))
162}
163
164fn closest_point_on_simplex<S: Scalar, const N: usize>(
180 simplex: &[Vector<S, N>],
181) -> Option<(Vector<S, N>, [usize; MAX_SIMPLEX], usize)> {
182 let k = simplex.len();
183 debug_assert!(k <= MAX_SIMPLEX, "GJK simplex exceeded MAX_SIMPLEX");
184 let mut best: Option<(Vector<S, N>, [usize; MAX_SIMPLEX], usize, S)> = None;
185
186 for mask in 1..(1u32 << k) {
187 let mut indices = [0usize; MAX_SIMPLEX];
188 let mut count = 0;
189 for i in 0..k {
190 if mask & (1 << i) != 0 {
191 indices[count] = i;
192 count += 1;
193 }
194 }
195 let mut pts = [Vector::<S, N>::zero(); MAX_SIMPLEX];
196 for j in 0..count {
197 pts[j] = simplex[indices[j]];
198 }
199
200 let Some((lambda, m)) = solve_barycentric(&pts[..count]) else {
201 continue;
202 };
203 if (0..m).any(|i| lambda[i].definitely_less(S::ZERO)) {
204 continue;
205 }
206
207 let mut point = Vector::<S, N>::zero();
208 for i in 0..m {
209 point = point.add(&pts[i].prod_scalar(lambda[i]));
210 }
211 let dist = point.norm_sq();
212
213 let is_better = match &best {
214 Some((_, _, _, best_dist)) => dist.definitely_less(*best_dist),
215 None => true,
216 };
217 if is_better {
218 best = Some((point, indices, count, dist));
219 }
220 }
221
222 best.map(|(p, idx, count, _)| (p, idx, count))
223}
224
225fn reduce_simplex<S: Scalar, const N: usize>(
229 simplex: &mut [Vector<S, N>; MAX_SIMPLEX],
230 len: &mut usize,
231 d: &mut Vector<S, N>,
232) -> bool {
233 match closest_point_on_simplex(&simplex[..*len]) {
234 Some((point, indices, count)) => {
235 if point.norm_sq().could_be_equal(S::ZERO) {
236 return true;
237 }
238 let mut reduced = [Vector::<S, N>::zero(); MAX_SIMPLEX];
239 for i in 0..count {
240 reduced[i] = simplex[indices[i]];
241 }
242 *simplex = reduced;
243 *len = count;
244 *d = point.neg();
245 false
246 }
247 None => true,
252 }
253}
254
255pub fn could_overlap<S: Scalar, const N: usize>(a: &[Vector<S, N>], b: &[Vector<S, N>]) -> bool {
262 if axis_separates(a, b) {
263 return false;
264 }
265
266 let mut d = b[0].sub(&a[0]);
267 if d.norm_sq().could_be_equal(S::ZERO) {
268 return true;
269 }
270
271 let mut simplex = [Vector::<S, N>::zero(); MAX_SIMPLEX];
272 simplex[0] = support(a, b, &d);
273 let mut len = 1;
274 d = simplex[0].neg();
275 if d.norm_sq().could_be_equal(S::ZERO) {
276 return true;
277 }
278
279 for _ in 0..MAX_ITERS {
280 if separates(a, b, &d) {
281 return false;
282 }
283 let new_pt = support(a, b, &d);
284 if len >= MAX_SIMPLEX {
289 return true;
290 }
291 simplex[len] = new_pt;
292 len += 1;
293
294 if reduce_simplex(&mut simplex, &mut len, &mut d) {
295 return true;
296 }
297 if d.norm_sq().could_be_equal(S::ZERO) {
298 return true;
299 }
300 }
301
302 true
305}
306
307pub fn definitely_no_overlap<S: Scalar, const N: usize>(
309 a: &[Vector<S, N>],
310 b: &[Vector<S, N>],
311) -> bool {
312 !could_overlap(a, b)
313}
314
315#[cfg(test)]
316mod tests {
317 use super::{could_overlap, definitely_no_overlap};
318 use crate::{
319 for_all_scalars,
320 scalars::{ScalInF64, Scalar},
321 vector::{Vector, Vector2, Vector3},
322 };
323
324 fn v3<S: Scalar>(x: f64, y: f64, z: f64) -> Vector3<S> {
325 Vector3::from_array([S::from_f64(x), S::from_f64(y), S::from_f64(z)])
326 }
327
328 fn v2<S: Scalar>(x: f64, y: f64) -> Vector2<S> {
329 Vector2::from_array([S::from_f64(x), S::from_f64(y)])
330 }
331
332 fn v1<S: Scalar>(x: f64) -> Vector<S, 1> {
333 Vector::from_array([S::from_f64(x)])
334 }
335
336 fn iv3(xlo: f64, xhi: f64, ylo: f64, yhi: f64, zlo: f64, zhi: f64) -> Vector3<ScalInF64> {
337 Vector3::from_array([
338 ScalInF64::new(xlo, xhi),
339 ScalInF64::new(ylo, yhi),
340 ScalInF64::new(zlo, zhi),
341 ])
342 }
343
344 #[test]
352 fn near_collinear_coplanar_hulls_that_overlap() {
353 let a0 = iv3(
354 0.3537597655932039,
355 0.3537597656567957,
356 0.2999999999730407,
357 0.3000000000269586,
358 -4.450147717414304e-308,
359 4.450147717414304e-308,
360 );
361 let a1 = iv3(
362 0.3540039062411684,
363 0.35400390625883144,
364 0.2999999999925169,
365 0.30000000000748256,
366 -4.450147717125399e-308,
367 4.450147717125399e-308,
368 );
369
370 let b0 = iv3(
371 0.3539306639795137,
372 0.3539306641454851,
373 0.30010940872548025,
374 0.3001094088661847,
375 -4.450147718057801e-308,
376 4.450147718057801e-308,
377 );
378 let b1 = iv3(
379 0.3539367674404758,
380 0.3539367677157728,
381 0.3000425434435929,
382 0.3000425436769378,
383 -4.450147718745072e-308,
384 4.450147718745072e-308,
385 );
386 let b2 = iv3(
387 0.35394287092364923,
388 0.3539428712638493,
389 0.2999756837684811,
390 0.2999756840567862,
391 -4.450147719153063e-308,
392 4.450147719153063e-308,
393 );
394
395 let a = vec![a0, a1];
396 let b = vec![b0, b1, b2];
397
398 assert!(could_overlap(&a, &b));
399 assert!(!definitely_no_overlap(&a, &b));
400 }
401
402 fn cube<S: Scalar>(cx: f64, cy: f64, cz: f64, half: f64) -> Vec<Vector3<S>> {
404 let mut pts = Vec::with_capacity(8);
405 for &dx in &[-half, half] {
406 for &dy in &[-half, half] {
407 for &dz in &[-half, half] {
408 pts.push(v3(cx + dx, cy + dy, cz + dz));
409 }
410 }
411 }
412 pts
413 }
414
415 fn check_overlapping_cubes_could_overlap<S: Scalar>() {
416 let a = cube::<S>(0., 0., 0., 1.);
417 let b = cube::<S>(0.5, 0., 0., 1.);
418 assert!(could_overlap(&a, &b));
419 assert!(!definitely_no_overlap(&a, &b));
420 }
421 #[test]
422 fn overlapping_cubes_could_overlap() {
423 for_all_scalars!(check_overlapping_cubes_could_overlap);
424 }
425
426 fn check_separated_cubes_no_overlap<S: Scalar>() {
427 let a = cube::<S>(0., 0., 0., 1.);
428 let b = cube::<S>(10., 0., 0., 1.);
429 assert!(definitely_no_overlap(&a, &b));
430 assert!(!could_overlap(&a, &b));
431 }
432 #[test]
433 fn separated_cubes_no_overlap() {
434 for_all_scalars!(check_separated_cubes_no_overlap);
435 }
436
437 fn check_touching_cubes_could_overlap<S: Scalar>() {
438 let a = cube::<S>(0., 0., 0., 1.);
440 let b = cube::<S>(2., 0., 0., 1.);
441 assert!(could_overlap(&a, &b));
442 }
443 #[test]
444 fn touching_cubes_could_overlap() {
445 for_all_scalars!(check_touching_cubes_could_overlap);
446 }
447
448 fn check_nested_point_inside_cube_overlaps<S: Scalar>() {
449 let cube = cube::<S>(0., 0., 0., 1.);
450 let point = vec![v3::<S>(0.25, -0.25, 0.5)];
451 assert!(could_overlap(&cube, &point));
452 }
453 #[test]
454 fn nested_point_inside_cube_overlaps() {
455 for_all_scalars!(check_nested_point_inside_cube_overlaps);
456 }
457
458 fn check_point_outside_cube_no_overlap<S: Scalar>() {
459 let cube = cube::<S>(0., 0., 0., 1.);
460 let point = vec![v3::<S>(5., 5., 5.)];
461 assert!(definitely_no_overlap(&cube, &point));
462 }
463 #[test]
464 fn point_outside_cube_no_overlap() {
465 for_all_scalars!(check_point_outside_cube_no_overlap);
466 }
467
468 fn check_identical_cubes_overlap<S: Scalar>() {
469 let a = cube::<S>(0., 0., 0., 1.);
470 let b = cube::<S>(0., 0., 0., 1.);
471 assert!(could_overlap(&a, &b));
472 }
473 #[test]
474 fn identical_cubes_overlap() {
475 for_all_scalars!(check_identical_cubes_overlap);
476 }
477
478 fn check_crossing_triangles_overlap<S: Scalar>() {
480 let a = vec![
481 v3::<S>(-1., 0., 0.),
482 v3::<S>(1., 0., 0.),
483 v3::<S>(0., 1., 0.),
484 ];
485 let b = vec![
486 v3::<S>(0., -1., 0.),
487 v3::<S>(0., 1., 0.),
488 v3::<S>(1., -1., 0.),
489 ];
490 assert!(could_overlap(&a, &b));
491 }
492 #[test]
493 fn crossing_triangles_overlap() {
494 for_all_scalars!(check_crossing_triangles_overlap);
495 }
496
497 fn check_single_points<S: Scalar>() {
499 let a = vec![v3::<S>(1., 2., 3.)];
500 let b = vec![v3::<S>(1., 2., 3.)];
501 assert!(could_overlap(&a, &b));
502
503 let c = vec![v3::<S>(1., 2., 3.0001)];
504 assert!(definitely_no_overlap(&a, &c));
505 }
506 #[test]
507 fn single_points() {
508 for_all_scalars!(check_single_points);
509 }
510
511 fn check_segment_through_cube_overlaps<S: Scalar>() {
513 let cube = cube::<S>(0., 0., 0., 1.);
514 let segment = vec![v3::<S>(-5., 0., 0.), v3::<S>(5., 0., 0.)];
515 assert!(could_overlap(&cube, &segment));
516 }
517 #[test]
518 fn segment_through_cube_overlaps() {
519 for_all_scalars!(check_segment_through_cube_overlaps);
520 }
521
522 fn check_segment_missing_cube_no_overlap<S: Scalar>() {
524 let cube = cube::<S>(0., 0., 0., 1.);
525 let segment = vec![v3::<S>(-5., 5., 5.), v3::<S>(5., 5., 5.)];
526 assert!(definitely_no_overlap(&cube, &segment));
527 }
528 #[test]
529 fn segment_missing_cube_no_overlap() {
530 for_all_scalars!(check_segment_missing_cube_no_overlap);
531 }
532
533 fn check_collinear_segments_overlap<S: Scalar>() {
537 let a = vec![v3::<S>(0., 0., 0.), v3::<S>(1., 0., 0.)];
538 let b = vec![v3::<S>(0.5, 0., 0.), v3::<S>(1.5, 0., 0.)];
539 assert!(could_overlap(&a, &b));
540 }
541 #[test]
542 fn collinear_segments_overlap() {
543 for_all_scalars!(check_collinear_segments_overlap);
544 }
545
546 fn check_collinear_segments_touching<S: Scalar>() {
548 let a = vec![v3::<S>(0., 0., 0.), v3::<S>(1., 0., 0.)];
549 let b = vec![v3::<S>(1., 0., 0.), v3::<S>(2., 0., 0.)];
550 assert!(could_overlap(&a, &b));
551 }
552 #[test]
553 fn collinear_segments_touching() {
554 for_all_scalars!(check_collinear_segments_touching);
555 }
556
557 fn check_collinear_segments_separated<S: Scalar>() {
559 let a = vec![v3::<S>(0., 0., 0.), v3::<S>(1., 0., 0.)];
560 let b = vec![v3::<S>(2., 0., 0.), v3::<S>(3., 0., 0.)];
561 assert!(definitely_no_overlap(&a, &b));
562 }
563 #[test]
564 fn collinear_segments_separated() {
565 for_all_scalars!(check_collinear_segments_separated);
566 }
567
568 fn check_crossing_collinear_segments_overlap<S: Scalar>() {
571 let a = vec![v3::<S>(-1., 0., 0.), v3::<S>(1., 0., 0.)];
572 let b = vec![v3::<S>(0., -1., 0.), v3::<S>(0., 1., 0.)];
573 assert!(could_overlap(&a, &b));
574 }
575 #[test]
576 fn crossing_collinear_segments_overlap() {
577 for_all_scalars!(check_crossing_collinear_segments_overlap);
578 }
579
580 fn check_parallel_collinear_segments_no_overlap<S: Scalar>() {
582 let a = vec![v3::<S>(0., 0., 0.), v3::<S>(1., 0., 0.)];
583 let b = vec![v3::<S>(0., 1., 0.), v3::<S>(1., 1., 0.)];
584 assert!(definitely_no_overlap(&a, &b));
585 }
586 #[test]
587 fn parallel_collinear_segments_no_overlap() {
588 for_all_scalars!(check_parallel_collinear_segments_no_overlap);
589 }
590
591 fn check_collinear_hull_vs_point<S: Scalar>() {
594 let a = vec![
595 v3::<S>(0., 0., 0.),
596 v3::<S>(0.5, 0., 0.),
597 v3::<S>(1., 0., 0.),
598 ];
599 assert!(could_overlap(&a, &[v3::<S>(0.3, 0., 0.)]));
601 assert!(definitely_no_overlap(&a, &[v3::<S>(2., 0., 0.)]));
603 assert!(definitely_no_overlap(&a, &[v3::<S>(0.3, 1., 0.)]));
605 }
606 #[test]
607 fn collinear_hull_vs_point() {
608 for_all_scalars!(check_collinear_hull_vs_point);
609 }
610
611 fn check_coplanar_squares<S: Scalar>() {
613 let square = |cx: f64, cy: f64| -> Vec<Vector3<S>> {
614 vec![
615 v3(cx, cy, 0.),
616 v3(cx + 1., cy, 0.),
617 v3(cx, cy + 1., 0.),
618 v3(cx + 1., cy + 1., 0.),
619 ]
620 };
621 let a = square(0., 0.);
622 let overlapping = square(0.5, 0.5);
623 let separated = square(5., 5.);
624 assert!(could_overlap(&a, &overlapping));
625 assert!(definitely_no_overlap(&a, &separated));
626 }
627 #[test]
628 fn coplanar_squares() {
629 for_all_scalars!(check_coplanar_squares);
630 }
631
632 fn check_degenerate_repeated_point_hull<S: Scalar>() {
635 let a = vec![v3::<S>(1., 2., 3.); 4];
636 let same = vec![v3::<S>(1., 2., 3.); 3];
637 let elsewhere = vec![v3::<S>(1., 2., 3.0001); 2];
638 assert!(could_overlap(&a, &same));
639 assert!(definitely_no_overlap(&a, &elsewhere));
640 }
641 #[test]
642 fn degenerate_repeated_point_hull() {
643 for_all_scalars!(check_degenerate_repeated_point_hull);
644 }
645
646 fn check_degenerate_edge_hull_vs_cube<S: Scalar>() {
650 let folded = vec![
653 v3::<S>(0., 0., 0.),
654 v3::<S>(0., 0., 0.),
655 v3::<S>(1., 0., 0.),
656 v3::<S>(1., 0., 0.),
657 ];
658 let overlapping_cube = cube::<S>(0., 0., 0., 1.);
659 let far_cube = cube::<S>(10., 0., 0., 1.);
660 assert!(could_overlap(&folded, &overlapping_cube));
661 assert!(definitely_no_overlap(&folded, &far_cube));
662 }
663 #[test]
664 fn degenerate_edge_hull_vs_cube() {
665 for_all_scalars!(check_degenerate_edge_hull_vs_cube);
666 }
667
668 fn square2<S: Scalar>(cx: f64, cy: f64, half: f64) -> Vec<Vector2<S>> {
671 vec![
672 v2(cx - half, cy - half),
673 v2(cx + half, cy - half),
674 v2(cx - half, cy + half),
675 v2(cx + half, cy + half),
676 ]
677 }
678
679 fn check_overlapping_squares_2d<S: Scalar>() {
680 let a = square2::<S>(0., 0., 1.);
681 let b = square2::<S>(0.5, 0., 1.);
682 assert!(could_overlap(&a, &b));
683 assert!(!definitely_no_overlap(&a, &b));
684 }
685 #[test]
686 fn overlapping_squares_2d() {
687 for_all_scalars!(check_overlapping_squares_2d);
688 }
689
690 fn check_separated_squares_2d<S: Scalar>() {
691 let a = square2::<S>(0., 0., 1.);
692 let b = square2::<S>(10., 0., 1.);
693 assert!(definitely_no_overlap(&a, &b));
694 assert!(!could_overlap(&a, &b));
695 }
696 #[test]
697 fn separated_squares_2d() {
698 for_all_scalars!(check_separated_squares_2d);
699 }
700
701 fn check_touching_squares_2d<S: Scalar>() {
702 let a = square2::<S>(0., 0., 1.);
703 let b = square2::<S>(2., 0., 1.);
704 assert!(could_overlap(&a, &b));
705 }
706 #[test]
707 fn touching_squares_2d() {
708 for_all_scalars!(check_touching_squares_2d);
709 }
710
711 fn check_point_inside_triangle_2d<S: Scalar>() {
712 let tri = vec![v2::<S>(0., 0.), v2::<S>(2., 0.), v2::<S>(0., 2.)];
713 assert!(could_overlap(&tri, &[v2::<S>(0.5, 0.5)]));
714 assert!(definitely_no_overlap(&tri, &[v2::<S>(5., 5.)]));
715 }
716 #[test]
717 fn point_inside_triangle_2d() {
718 for_all_scalars!(check_point_inside_triangle_2d);
719 }
720
721 fn check_crossing_segments_2d<S: Scalar>() {
723 let a = vec![v2::<S>(-1., -1.), v2::<S>(1., 1.)];
724 let b = vec![v2::<S>(-1., 1.), v2::<S>(1., -1.)];
725 assert!(could_overlap(&a, &b));
726 }
727 #[test]
728 fn crossing_segments_2d() {
729 for_all_scalars!(check_crossing_segments_2d);
730 }
731
732 fn check_collinear_segments_2d<S: Scalar>() {
733 let overlapping_a = vec![v2::<S>(0., 0.), v2::<S>(1., 0.)];
734 let overlapping_b = vec![v2::<S>(0.5, 0.), v2::<S>(1.5, 0.)];
735 assert!(could_overlap(&overlapping_a, &overlapping_b));
736
737 let touching_a = vec![v2::<S>(0., 0.), v2::<S>(1., 0.)];
738 let touching_b = vec![v2::<S>(1., 0.), v2::<S>(2., 0.)];
739 assert!(could_overlap(&touching_a, &touching_b));
740
741 let separated_a = vec![v2::<S>(0., 0.), v2::<S>(1., 0.)];
742 let separated_b = vec![v2::<S>(2., 0.), v2::<S>(3., 0.)];
743 assert!(definitely_no_overlap(&separated_a, &separated_b));
744
745 let parallel_a = vec![v2::<S>(0., 0.), v2::<S>(1., 0.)];
746 let parallel_b = vec![v2::<S>(0., 1.), v2::<S>(1., 1.)];
747 assert!(definitely_no_overlap(¶llel_a, ¶llel_b));
748 }
749 #[test]
750 fn collinear_segments_2d() {
751 for_all_scalars!(check_collinear_segments_2d);
752 }
753
754 fn check_degenerate_repeated_point_hull_2d<S: Scalar>() {
755 let a = vec![v2::<S>(1., 2.); 4];
756 let same = vec![v2::<S>(1., 2.); 3];
757 let elsewhere = vec![v2::<S>(1., 2.0001); 2];
758 assert!(could_overlap(&a, &same));
759 assert!(definitely_no_overlap(&a, &elsewhere));
760 }
761 #[test]
762 fn degenerate_repeated_point_hull_2d() {
763 for_all_scalars!(check_degenerate_repeated_point_hull_2d);
764 }
765
766 fn check_intervals_1d<S: Scalar>() {
769 let a = vec![v1::<S>(0.), v1::<S>(1.)];
770 let overlapping = vec![v1::<S>(0.5), v1::<S>(1.5)];
771 let separated = vec![v1::<S>(2.), v1::<S>(3.)];
772 assert!(could_overlap(&a, &overlapping));
773 assert!(definitely_no_overlap(&a, &separated));
774
775 let point_inside = vec![v1::<S>(0.5)];
776 let point_outside = vec![v1::<S>(5.)];
777 assert!(could_overlap(&a, &point_inside));
778 assert!(definitely_no_overlap(&a, &point_outside));
779 }
780 #[test]
781 fn intervals_1d() {
782 for_all_scalars!(check_intervals_1d);
783 }
784}