1use itertools::Itertools;
7use serde::{Deserialize, Serialize};
8use std::{array, fmt};
9
10use hoomd_vector::{Cartesian, Cross, InnerProduct, Rotate, Rotation, RotationMatrix};
11
12use crate::{IntersectsAt, IntersectsAtGlobal, SupportMapping, Volume};
13
14#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
71pub struct Simplex3 {
72 vertices: [Cartesian<3>; 4], }
75
76impl SupportMapping<Cartesian<3>> for Simplex3 {
77 #[inline]
78 fn support_mapping(&self, n: &Cartesian<3>) -> Cartesian<3> {
79 let dots = self.vertices.map(|v| v.dot(n));
80 self.vertices[dots
81 .iter()
82 .position_max_by(|x, y| x.partial_cmp(y).unwrap_or(std::cmp::Ordering::Equal))
84 .expect("dot product results should always be comparable")]
85 }
86}
87
88impl Volume for Simplex3 {
89 #[inline]
90 fn volume(&self) -> f64 {
91 let (ba, ca, da) = (
92 self.b() - self.a(),
93 self.c() - self.a(),
94 self.d() - self.a(),
95 );
96 1.0 / 6.0 * da.dot(&ba.cross(&ca)).abs()
97 }
98}
99
100impl From<[Cartesian<3>; 4]> for Simplex3 {
101 #[inline]
102 fn from(vertices: [Cartesian<3>; 4]) -> Self {
103 Simplex3 { vertices }.orient()
104 }
105}
106impl From<[[f64; 3]; 4]> for Simplex3 {
107 #[inline]
108 fn from(arrs: [[f64; 3]; 4]) -> Self {
109 Simplex3 {
110 vertices: arrs.map(Cartesian::from),
111 }
112 .orient()
113 }
114}
115
116impl Default for Simplex3 {
117 #[inline]
120 fn default() -> Self {
121 Simplex3 {
123 vertices: [
124 [1.0, 1.0, 1.0].into(),
125 [1.0, -1.0, -1.0].into(),
126 [-1.0, 1.0, -1.0].into(),
127 [-1.0, -1.0, 1.0].into(),
128 ],
129 }
130 }
131}
132
133impl fmt::Display for Simplex3 {
134 #[inline]
135 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
136 write!(
137 f,
138 "Simplex {{ [{}] }}",
139 self.vertices
140 .iter()
141 .map(Cartesian::to_string)
142 .collect::<Vec<String>>()
143 .join(", ")
144 )
145 }
146}
147impl Simplex3 {
148 #[inline]
150 #[must_use]
151 pub fn vertices(&self) -> [Cartesian<3>; 4] {
152 self.vertices
153 }
154
155 #[inline]
157 #[must_use]
158 pub fn translate_by(&mut self, rhs: &Cartesian<3>) -> Self {
159 Self {
160 vertices: self.vertices.map(|v| v + *rhs),
161 }
162 }
163
164 #[inline]
166 #[must_use]
167 pub fn centroid(&self) -> Cartesian<3> {
168 array::from_fn::<_, 3, _>(|i| self.vertices.iter().fold(0.0, |acc, v| acc + v[i])).into()
169 }
170
171 #[inline]
174 #[must_use]
175 pub fn get_edges(&self) -> [[Cartesian<3>; 2]; 5] {
176 [
177 [self.b(), self.a()],
178 [self.c(), self.a()],
179 [self.d(), self.a()],
180 [self.c(), self.b()],
181 [self.d(), self.b()],
182 ]
183 }
184
185 #[inline]
187 #[must_use]
188 pub fn get_edge_vectors(&self) -> [Cartesian<3>; 5] {
189 self.get_edges().map(|[l, r]| l - r)
190 }
191
192 #[inline]
193 #[must_use]
194 pub(crate) fn a(&self) -> Cartesian<3> {
196 self.vertices[0]
197 }
198 #[inline]
199 #[must_use]
200 pub(crate) fn b(&self) -> Cartesian<3> {
202 self.vertices[1]
203 }
204 #[inline]
205 #[must_use]
206 pub(crate) fn c(&self) -> Cartesian<3> {
208 self.vertices[2]
209 }
210 #[inline]
211 #[must_use]
212 pub(crate) fn d(&self) -> Cartesian<3> {
214 self.vertices[3]
215 }
216 #[inline]
218 pub(crate) fn orient(&self) -> Simplex3 {
219 let dp = (self.d() - self.a()).dot(&((self.b() - self.a()).cross(&(self.c() - self.a()))));
220 if dp < 0.0 {
221 Simplex3 {
222 vertices: self.vertices,
223 }
224 } else {
225 Simplex3 {
226 vertices: [self.a(), self.c(), self.b(), self.d()],
227 }
228 }
229 }
230}
231#[inline]
234#[must_use]
235fn check_face_on_p_is_separating(aff: &[f64; 4]) -> u8 {
236 aff.iter().enumerate().fold(
237 0u8,
238 |acc, (i, &x)| {
239 if x > 0.0 { acc | (1 << i) } else { acc }
240 },
241 )
242}
243
244#[inline]
246#[must_use]
247fn check_face_on_q_is_separating(aff: &[f64; 4]) -> bool {
248 aff.iter().all(|&x| x > 0.0)
249}
250
251#[expect(
254 clippy::too_many_arguments,
255 reason = "Internal function not exposed to users."
256)]
257#[inline]
258#[must_use]
259fn edge_test(
260 ma: u8,
261 mb: u8,
262 a: u8,
263 b: u8,
264 i: usize,
265 j: usize,
266 ea: &[f64; 4],
267 eb: &[f64; 4],
268) -> bool {
269 let cp = (ea[j] * eb[i]) - (ea[i] * eb[j]);
270 ((ma & a) != 0 && (mb & b) != 0 && (cp >= 0.0)) || (ma & b) != 0 && (mb & a) != 0 && (cp <= 0.0)
272}
273
274const _SEPARATING_EDGE_CASES: [(u8, u8, usize, usize); 6] = [
276 (1, 2, 0, 1),
277 (1, 4, 0, 2),
278 (1, 8, 0, 3),
279 (2, 4, 1, 2),
280 (2, 8, 1, 3),
281 (4, 8, 2, 3),
282];
283
284#[inline]
287#[must_use]
288fn check_edge_is_separating(aff_a: &[f64; 4], aff_b: &[f64; 4], ma: u8, mb: u8) -> bool {
289 if (ma | mb) != 15 {
290 return false; }
292 let xa = ma & (ma ^ mb);
294 let xb = mb & (ma ^ mb);
295
296 if _SEPARATING_EDGE_CASES
297 .iter()
298 .any(|&(a, b, i, j)| edge_test(xa, xb, a, b, i, j, aff_a, aff_b))
299 {
300 return false;
301 }
302
303 true
305}
306
307impl<R> IntersectsAtGlobal<Simplex3, Cartesian<3>, R> for Simplex3
308where
309 R: Rotation + Rotate<Cartesian<3>>,
310 RotationMatrix<3>: From<R>,
311{
312 #[inline]
313 fn intersects_at_global(
314 &self,
315 other: &Simplex3,
316 r_self: &Cartesian<3>,
317 o_self: &R,
318 r_other: &Cartesian<3>,
319 o_other: &R,
320 ) -> bool {
321 let (v_ij, o_ij) = hoomd_vector::pair_system_to_local(r_self, o_self, r_other, o_other);
322
323 self.intersects_at(other, &v_ij, &o_ij)
324 }
325}
326
327impl<R> IntersectsAt<Simplex3, Cartesian<3>, R> for Simplex3
328where
329 R: Rotation + Rotate<Cartesian<3>>,
330 RotationMatrix<3>: From<R>,
331{
332 #[inline]
338 fn intersects_at(&self, other: &Simplex3, v_ij: &Cartesian<3>, o_ij: &R) -> bool {
339 let r = RotationMatrix::from(*o_ij);
340 let q = Simplex3::from(other.vertices.map(|v| r.rotate(&v))).translate_by(v_ij);
341
342 let q_deltas = q.vertices.map(|v| v - self.vertices[0]);
343
344 let mut edge_vectors_p = [Cartesian::<3>::default(); 5];
346 let mut masks = [0u8; 4];
347 let mut affs = [[0_f64; 4]; 4];
348
349 edge_vectors_p[0] = self.vertices[1] - self.vertices[0];
350 edge_vectors_p[1] = self.vertices[2] - self.vertices[0];
351
352 let n = edge_vectors_p[0].cross(&edge_vectors_p[1]);
356 affs[0] = q_deltas.map(|v| v.dot(&n));
357 let mask = check_face_on_p_is_separating(&affs[0]);
358 if mask == 15 {
359 return false;
360 }
361 masks[0] = mask;
362
363 edge_vectors_p[2] = self.vertices[3] - self.vertices[0];
365 let n = edge_vectors_p[2].cross(&edge_vectors_p[0]);
366
367 affs[1] = q_deltas.map(|v| v.dot(&n));
368 let mask = check_face_on_p_is_separating(&affs[1]);
369 if mask == 15 {
370 return false;
371 }
372 masks[1] = mask;
373
374 if check_edge_is_separating(&affs[0], &affs[1], masks[0], masks[1]) {
376 return false;
377 }
378
379 let n = edge_vectors_p[1].cross(&edge_vectors_p[2]);
381 affs[2] = q_deltas.map(|v| v.dot(&n));
382 let mask = check_face_on_p_is_separating(&affs[2]);
383 if mask == 15 {
384 return false;
385 }
386 masks[2] = mask;
387
388 if check_edge_is_separating(&affs[0], &affs[2], masks[0], masks[2]) {
390 return false;
391 }
392
393 if check_edge_is_separating(&affs[1], &affs[2], masks[1], masks[2]) {
395 return false;
396 }
397
398 edge_vectors_p[3] = self.vertices[2] - self.vertices[1];
400 edge_vectors_p[4] = self.vertices[3] - self.vertices[1];
401
402 let n = edge_vectors_p[4].cross(&edge_vectors_p[3]);
403 affs[3] = q.vertices.map(|v| (v - self.vertices[1]).dot(&n));
404 let mask = check_face_on_p_is_separating(&affs[3]);
405 if mask == 15 {
406 return false;
407 }
408 masks[3] = mask;
409
410 if [(0, 3), (1, 3), (2, 3)]
412 .iter()
413 .any(|&(i, j)| check_edge_is_separating(&affs[i], &affs[j], masks[i], masks[j]))
414 {
415 return false;
416 }
417
418 if masks.iter().fold(0, |acc, &m| acc | m) != 15 {
421 return true;
422 }
423
424 let p_deltas = self.vertices.map(|v| v - q.vertices[0]);
428 let mut edge_vectors_q = [Cartesian::<3>::default(); 5];
429 edge_vectors_q[0] = q.vertices[1] - q.vertices[0];
430 edge_vectors_q[1] = q.vertices[2] - q.vertices[0];
431
432 let n = edge_vectors_q[0].cross(&edge_vectors_q[1]);
434 if check_face_on_q_is_separating(&p_deltas.map(|v| v.dot(&n))) {
435 return false;
436 }
437
438 edge_vectors_q[2] = q.vertices[3] - q.vertices[0];
439
440 let n = edge_vectors_q[2].cross(&edge_vectors_q[0]);
442 if check_face_on_q_is_separating(&p_deltas.map(|v| v.dot(&n))) {
443 return false;
444 }
445
446 let n = edge_vectors_q[1].cross(&edge_vectors_q[2]);
448 if check_face_on_q_is_separating(&p_deltas.map(|v| v.dot(&n))) {
449 return false;
450 }
451 edge_vectors_q[3] = q.vertices[2] - q.vertices[1];
452 edge_vectors_q[4] = q.vertices[3] - q.vertices[1];
453
454 let n = edge_vectors_q[4].cross(&edge_vectors_q[3]);
456 let aff = self.vertices.map(|v| (v - other.vertices[1]).dot(&n));
457 if check_face_on_q_is_separating(&aff) {
458 return false;
459 }
460 true }
462}
463#[cfg(test)]
464mod tests {
465 use crate::xenocollide::collide3d;
466
467 use super::*;
468 use hoomd_vector::{Quaternion, Rotation, Versor};
469
470 use rstest::rstest;
471
472 #[test]
473 fn test_compute_mask() {
474 let arrays = (0u8..=15).map(|i| {
475 (
476 i,
477 [
478 f64::from(i & 1),
479 f64::from((i >> 1) & 1),
480 f64::from((i >> 2) & 1),
481 f64::from((i >> 3) & 1),
482 ],
483 )
484 });
485 arrays.for_each(|(i, arr)| assert_eq!(check_face_on_p_is_separating(&arr), i));
486 }
487
488 #[rstest(
489 ma, mb, ea, eb,
490 case(
491 0, 2,
492 [0.0, -100_000.0, 0.0, -500_000.0],
493 [0.0, 50_000.0, -500_000.0, 0.0]
494 ),
495 case(
496 6, 2,
497 [0.0, 1_025_000.0, 500_000.0, -500_000.0],
498 [0.0, 50_000.0, -500_000.0, 0.0]
499 ),
500 case(
501 0, 8,
502 [0.0, -100_000.0, 0.0, -500_000.0],
503 [-500_000.0, -1_475_000.0, -500_000.0, 500_000.0]
504 ),
505 case(
506 6, 8,
507 [0.0, 1_025_000.0, 500_000.0, -500_000.0],
508 [-500_000.0, -1_475_000.0, -500_000.0, 500_000.0]
509 ),
510 case(
511 2, 8,
512 [0.0, 50_000.0, -500_000.0, 0.0],
513 [-500_000.0, -1_475_000.0, -500_000.0, 500_000.0]
514 ),
515 case(
516 0, 0,
517 [0.0, 0.0, 0.0, -50_000.0],
518 [-500_005.0, -500_000.0, -1_000_000.0, -612_500.0]
519 ),
520 case(
521 0, 0,
522 [0.0, 0.0, 0.0, -50_000.0],
523 [0.0, -500_000.0, 0.0, -225_000.0]
524 ),
525 case(
526 0, 0,
527 [-500_005.0, -500_000.0, -1_000_000.0, -612_500.0],
528 [0.0, -500_000.0, 0.0, -225_000.0]
529 )
530 )]
531 fn test_edge_a(ma: u8, mb: u8, ea: [f64; 4], eb: [f64; 4]) {
532 assert!(!check_edge_is_separating(&ea, &eb, ma, mb));
534 }
535 #[rstest(
536 n=> [[0.0, 0.0, -5000.0], [-5000.0, 5000.0, 1250.0], [0.0, -10000.0, 2500.0]],
537 )]
538 fn test_face_a(n: [f64; 3]) {
539 let deltas: [Cartesian<3>; 4] = [
540 [0.0, 0.0, 0.0],
541 [-200.0, 0.0, 20.0],
542 [-50.0, 50.0, 0.0],
543 [150.0, 25.0, 100.0],
544 ]
545 .map(Cartesian::from);
546 let aff = deltas.map(|v| v.dot(&n.into()));
547 let result = check_face_on_p_is_separating(&aff);
548 let expected_result = match n {
549 [0.0, 0.0, -5000.0] => (0, [0.0, -100_000.0, 0.0, -500_000.0]),
550 [-5000.0, 5000.0, 1250.0] => (6, [0.0, 1_025_000.0, 500_000.0, -500_000.0]),
551 [0.0, -10_000.0, 2500.0] => (2, [0.0, 50_000.0, -500_000.0, 0.0]),
552 _ => unreachable!(),
553 };
554 assert_eq!((result, aff), expected_result);
555 }
556
557 #[test]
558 fn test_tetisect() {
559 let mut p = Simplex3::from([
560 [0.0, 0.0, 0.0],
561 [50.0, 50.0, 0.0],
562 [100.0, 0.0, 0.0],
563 [50.0, 25.0, 100.0],
564 ]);
565 let mut q = Simplex3::from([
566 [0.0, 0.0, 0.0],
567 [-50.0, 50.0, 0.0],
568 [-200.0, 0.0, 20.0],
569 [150.0, 25.0, 100.0],
570 ]);
571
572 assert!(p.intersects_at(&q, &Cartesian::default(), &Versor::identity()));
574
575 let p_centered = p.translate_by(&-p.centroid());
576 let q_centered = q.translate_by(&-q.centroid());
577
578 assert_eq!(
579 p.intersects_at(&q, &Cartesian::default(), &Versor::identity()),
580 collide3d(
581 &p_centered,
582 &q_centered,
583 &(q.centroid() - p.centroid()),
584 &Versor::identity()
585 )
586 );
587
588 let mut q_nooverlap = Simplex3::from([
589 [100.001, 0.0, 0.0],
590 [150.0, 50.0, 0.0],
591 [200.0, 0.0, 0.0],
592 [150.0, 25.0, 10.0],
593 ]);
594
595 assert!(!p.intersects_at(&q_nooverlap, &Cartesian::default(), &Versor::identity()));
597 let q_nooverlap_centered = q_nooverlap.translate_by(&-p.centroid());
598 assert_eq!(
599 p.intersects_at(&q_nooverlap, &Cartesian::default(), &Versor::identity()),
600 collide3d(
601 &p_centered,
602 &q_nooverlap_centered,
603 &(q_nooverlap.centroid() - p.centroid()),
604 &Versor::identity()
605 )
606 );
607 }
608
609 #[rstest(
610 v_ij, o_ij, overlaps,
611 case::perfect_overlap(
612 [0.0, 0.0, 0.0].into(),
613 Versor::identity(),
614 true,
615 ),
616 case::particle_at_infinity(
617 [f64::INFINITY, 0.0, 0.0].into(),
618 Versor::identity(),
619 false,
620 ),
621 case::particle_at_negative_infinity(
622 [f64::NEG_INFINITY, 0.0, 0.0].into(),
623 Versor::identity(),
624 false,
625 ),
626 case::tip_tip_intersection_exact(
627 [2.0, 2.0, 2.0].into(),
628 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
629 true,
630 ),
631 case::tip_tip_intersection_imprecise(
632 [1.999_999_999, 1.999_999_999, 1.999_999_999].into(),
633 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
634 true,
635 ),
636 case::tip_tip_intersection_nooverlap(
637 [2.000_000_001, 2.000_000_001, 2.000_000_001].into(),
638 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
639 false,
640 ),
641 case::unrotated_tip_tip_intersection_exact(
642 [2.0, 2.0, 0.0].into(),
643 Versor::identity(),
644 true,
645 ),
646 case::unrotated_tip_tip_intersection_imprecise(
647 [1.999_999_999, 1.999_999_999, 0.0].into(),
648 Versor::identity(),
649 true,
650 ),
651 case::unrotated_tip_tip_intersection_nooverlap(
652 [2.000_000_001, 2.000_000_001, 0.0].into(),
653 Versor::identity(),
654 false,
655 ),
656 case::tip_edge_intersection_exact(
658 [1.0, 1.0, 2.0].into(),
659 Versor::default(),
660 true,
661 ),
662 case::tip_edge_intersection_imprecise(
663 [1.0, 1.0, 1.999_999_999].into(),
664 Versor::default(),
665 true,
666 ),
667 case::tip_edge_intersection_nooverlap(
668 [1.0, 1.0, 2.000_000_001].into(),
669 Versor::default(),
670 false,
671 ),
672 case::parallel_edge_edge_intersection_exact(
673 [1.0, 1.0, 2.0].into(),
674 Quaternion::from([2.0_f64.sqrt() / 2.0, 2.0_f64.sqrt()/2.0, 0.0, 0.0]).to_versor_unchecked(),
676 true,
677 ),
678 case::parallel_edge_edge_intersection_imprecise(
679 [1.0, 1.0, 1.999_999_999].into(),
680 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
681 true,
682 ),
683 case::parallel_edge_edge_intersection_nooverlap(
684 [1.0, 1.0, 2.000_000_001].into(),
685 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
686 false,
687 ),
688 case::orthogonal_edge_edge_intersection_exact(
689 [1.0, 0.0, 2.0].into(),
690 Versor::identity(),
691 true,
692 ),
693 case::orthogonal_edge_edge_intersection_imprecise(
694 [1.0, 0.0, 1.999_999_999].into(),
695 Versor::identity(),
696 true,
697 ),
698 case::orthogonal_edge_edge_intersection_nooverlap(
699 [1.0, 0.0, 2.000_000_001].into(),
700 Versor::identity(),
701 false,
702 ),
703 case::nonorthogonal_edge_edge_intersection_exact(
704 [1.0, 0.0, 2.0].into(),
705 Versor::from_axis_angle([0.0, 0.0, 1.0].try_into().unwrap(), std::f64::consts::PI / 3.1),
706 true,
707 ),
708 case::nonorthogonal_edge_edge_intersection_imprecise(
709 [1.0, 0.0, 1.999_999_999].into(),
710 Versor::from_axis_angle([0.0, 0.0, 1.0].try_into().unwrap(), std::f64::consts::PI / 3.1),
711 true,
712 ),
713 case::nonorthogonal_edge_edge_intersection_nooverlap(
714 [1.0, 0.0, 2.000_000_001].into(),
715 Versor::from_axis_angle([0.0, 0.0, 1.0].try_into().unwrap(), std::f64::consts::PI / 3.1),
716 false,
717 ),
718 case::partial_aligned_overlap_exact(
720 [0.0, 1.0, -1.0].into(),
721 Versor::identity(),
722 true,
723 ),
724 case::partial_aligned_overlap_imprecise(
725 [0.0, 1.0, -0.999_999_999].into(),
726 Versor::identity(),
727 true,
728 ),
729 case::partial_parallel_overlap(
731 [0.0, 0.0, -1.0].into(),
732 Versor::identity(),
733 true,
734 ),
735 case::vertex_into_edge_shallow_exact(
737 [0.0, 1.0, 2.0].into(),
738 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_4),
739 true,
740 ),
741 case::vertex_into_edge_shallow_imprecise(
742 [0.0, 0.999_999_999, 2.0].into(),
743 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_4),
744 true,
745 ),
746 case::vertex_into_edge_deep_exact(
747 [0.0, 1.0, 1.0].into(),
748 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_4),
749 true,
750 ),
751 case::vertex_into_edge_deep_imprecise(
752 [0.0, 0.999_999_999, 1.0].into(),
753 Versor::from_axis_angle([1.0, 0.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_4),
754 true,
755 ),
756 case::vertex_face_nooverlap(
765 [1.2765, -1.2765, 1.2765].into(),
766 Versor::from_axis_angle([1.0, 1.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
767 false,
768 ),
769 case::vertex_face_near_exact(
770 [1.275, -1.275, 1.275].into(),
771 Versor::from_axis_angle([1.0, 1.0, 0.0].try_into().unwrap(), std::f64::consts::FRAC_PI_2),
772 true,
773 ),
774 )]
775 fn test_tetrahedron_overlap_param(
776 #[values("intersects_at", "xenocollide")] method: &str,
777 v_ij: Cartesian<3>,
778 o_ij: Versor,
779 overlaps: bool,
780 ) {
781 let p = Simplex3::default();
782 let q = Simplex3::default();
783
784 let result = if method == "intersects_at" {
785 p.intersects_at(&q, &v_ij, &o_ij)
786 } else {
787 collide3d(&p, &q, &v_ij, &o_ij)
788 };
789
790 assert_eq!(
791 result, overlaps,
792 "Method `{method}` gave wrong overlap result.",
793 );
794 }
795
796 #[rstest]
797 fn test_tetrahedron_volume() {
798 let p = Simplex3::default();
799 assert_eq!(p.volume(), 8.0 / 3.0);
800 }
801}