1use fast_mesh_bvh::Bvh;
12use fast_mesh_store::{Mesh, TriangleId};
13
14use crate::pair::PAIR_STACK;
15use crate::pairs::{for_each_pair, overlap_key, validate};
16use crate::tritri::{
17 P3, collinear, in_tri_2d, normal_axis, orient2d_approx, side, side2, tri_tri_intersect,
18};
19use crate::{QueryError, Result, Rigid, allocation_allowed};
20
21#[derive(Debug, Clone, Default, PartialEq)]
24pub struct Polyline {
25 pub points: Vec<[f32; 3]>,
28 pub closed: bool,
30 pub manifold: bool,
34}
35
36#[derive(Debug, Clone, Copy, PartialEq)]
45pub struct PairSegment {
46 pub a: TriangleId,
48 pub b: TriangleId,
50 pub from: [f32; 3],
52 pub to: [f32; 3],
54}
55
56pub fn intersecting_segments(
71 mesh_a: &Mesh,
72 bvh_a: &Bvh,
73 mesh_b: &Mesh,
74 bvh_b: &Bvh,
75 xf: &Rigid,
76 out: &mut Vec<PairSegment>,
77) -> Result<usize> {
78 validate(mesh_a, bvh_a, mesh_b, bvh_b, xf, None)?;
79 let start = out.len();
80 let r = pair_segments(mesh_a, bvh_a, mesh_b, bvh_b, xf, out);
81 if r.is_err() {
82 out.truncate(start);
83 }
84 r
85}
86
87fn pair_segments(
88 mesh_a: &Mesh,
89 bvh_a: &Bvh,
90 mesh_b: &Mesh,
91 bvh_b: &Bvh,
92 xf: &Rigid,
93 out: &mut Vec<PairSegment>,
94) -> Result<usize> {
95 let start = out.len();
96 let mut buf = [Seg64::EMPTY; 6];
97 let mut failed = false;
98 for_each_pair::<PAIR_STACK>(
99 mesh_a,
100 bvh_a,
101 mesh_b,
102 bvh_b,
103 xf,
104 None,
105 overlap_key,
106 |a, pa, b, pb| {
107 if failed {
108 return false;
109 }
110 if !tri_tri_intersect(pa, pb) {
111 return true;
112 }
113 let n = clip_pair(pa, pb, &mut buf);
114 for s in &buf[..n] {
115 let (from, to) = (s.from.map(|x| x as f32), s.to.map(|x| x as f32));
116 if from == to {
117 continue;
118 }
119 if push(out, PairSegment { a, b, from, to }).is_err() {
120 failed = true;
121 return false;
122 }
123 }
124 true
125 },
126 );
127 if failed {
128 return Err(QueryError::OutOfMemory);
129 }
130 Ok(out.len() - start)
131}
132
133pub fn intersection_polylines(
155 mesh_a: &Mesh,
156 bvh_a: &Bvh,
157 mesh_b: &Mesh,
158 bvh_b: &Bvh,
159 xf: &Rigid,
160 out: &mut Vec<Polyline>,
161) -> Result<usize> {
162 validate(mesh_a, bvh_a, mesh_b, bvh_b, xf, None)?;
163 let start = out.len();
164 let r = polylines(mesh_a, bvh_a, mesh_b, bvh_b, xf, out);
165 if r.is_err() {
166 out.truncate(start);
167 }
168 r
169}
170
171fn polylines(
172 mesh_a: &Mesh,
173 bvh_a: &Bvh,
174 mesh_b: &Mesh,
175 bvh_b: &Bvh,
176 xf: &Rigid,
177 out: &mut Vec<Polyline>,
178) -> Result<usize> {
179 let mut segs: Vec<Seg64> = Vec::new();
182 let mut failed = false;
183 let mut buf = [Seg64::EMPTY; 6];
184 for_each_pair::<PAIR_STACK>(
185 mesh_a,
186 bvh_a,
187 mesh_b,
188 bvh_b,
189 xf,
190 None,
191 overlap_key,
192 |_, pa, _, pb| {
193 if failed {
194 return false;
195 }
196 if !tri_tri_intersect(pa, pb) {
197 return true;
198 }
199 let n = clip_pair(pa, pb, &mut buf);
200 for s in &buf[..n] {
201 if s.from == s.to {
202 continue;
203 }
204 if push(&mut segs, *s).is_err() {
205 failed = true;
206 return false;
207 }
208 }
209 true
210 },
211 );
212 if failed {
213 return Err(QueryError::OutOfMemory);
214 }
215 if segs.is_empty() {
216 return Ok(0);
217 }
218 cancel_dupes(&mut segs);
220 if segs.is_empty() {
221 return Ok(0);
222 }
223 let mut segs32: Vec<Seg32> = Vec::new();
224 for s in &segs {
225 push(
226 &mut segs32,
227 Seg32 {
228 from: s.from.map(|x| x as f32),
229 to: s.to.map(|x| x as f32),
230 },
231 )?;
232 }
233 segs32.retain(|s| s.from != s.to);
234 if segs32.is_empty() {
235 return Ok(0);
236 }
237 chain(&segs32, out)
238}
239
240#[derive(Debug, Clone, Copy)]
242struct Seg32 {
243 from: [f32; 3],
244 to: [f32; 3],
245}
246
247fn cancel_dupes(segs: &mut Vec<Seg64>) {
252 segs.sort_unstable_by_key(|s| {
253 let (a, b) = (s.from.map(f64::to_bits), s.to.map(f64::to_bits));
254 if a <= b { (a, b) } else { (b, a) }
255 });
256 let key = |s: &Seg64| {
257 let (a, b) = (s.from.map(f64::to_bits), s.to.map(f64::to_bits));
258 if a <= b { (a, b) } else { (b, a) }
259 };
260 let mut w = 0;
261 let mut i = 0;
262 while i < segs.len() {
263 let k0 = key(&segs[i]);
264 let (mut fwd, mut bwd) = (None, None);
265 let (mut nf, mut nb) = (0, 0);
266 while i + nf + nb < segs.len() && key(&segs[i + nf + nb]) == k0 {
267 let s = segs[i + nf + nb];
268 if s.from.map(f64::to_bits) == k0.0 {
269 nf += 1;
270 fwd = Some(s);
271 } else {
272 nb += 1;
273 bwd = Some(s);
274 }
275 }
276 let cancel = nf.min(nb);
278 if nf - cancel > 0 {
279 segs[w] = fwd.unwrap();
280 w += 1;
281 } else if nb - cancel > 0 {
282 segs[w] = bwd.unwrap();
283 w += 1;
284 }
285 i += nf + nb;
286 }
287 segs.truncate(w);
288}
289
290const BOUNDARY: usize = usize::MAX;
293const SPLIT: usize = usize::MAX - 1;
294
295fn chain(segs: &[Seg32], out: &mut Vec<Polyline>) -> Result<usize> {
300 let start = out.len();
301 let key_of = |e: usize| {
302 let s = &segs[e / 2];
303 let p = if e.is_multiple_of(2) { s.from } else { s.to };
304 p.map(f32::to_bits)
305 };
306 let mut ends: Vec<usize> = Vec::new();
307 reserve(&mut ends, 2 * segs.len())?;
308 ends.extend(0..2 * segs.len());
309 ends.sort_unstable_by_key(|&e| (key_of(e), e & 1, e));
310 let mut index = Vec::new();
312 reserve(&mut index, ends.len())?;
313 index.resize(ends.len(), 0);
314 for (pos, &e) in ends.iter().enumerate() {
315 index[e] = pos;
316 }
317 let mut link = Vec::new();
318 reserve(&mut link, ends.len())?;
319 link.resize(ends.len(), BOUNDARY);
320 for group in ends.chunk_by(|a, b| key_of(*a) == key_of(*b)) {
321 match group.len() {
322 1 => link[index[group[0]]] = BOUNDARY,
323 2 => {
324 link[index[group[0]]] = group[1];
325 link[index[group[1]]] = group[0];
326 }
327 _ => {
328 for &e in group {
329 link[index[e]] = SPLIT;
330 }
331 }
332 }
333 }
334 let mut used = Vec::new();
335 reserve(&mut used, segs.len())?;
336 used.resize(segs.len(), false);
337 for closed_pass in [false, true] {
339 for (pos, &e) in ends.iter().enumerate() {
340 if used[e / 2] || (!closed_pass && link[pos] < SPLIT) {
341 continue;
342 }
343 let first = e;
344 let mut points = Vec::new();
345 let mut entry = e;
346 let next = loop {
347 used[entry / 2] = true;
348 let s = &segs[entry / 2];
349 push(
350 &mut points,
351 if entry.is_multiple_of(2) {
352 s.from
353 } else {
354 s.to
355 },
356 )?;
357 let exit = entry ^ 1;
358 let l = link[index[exit]];
359 if l >= SPLIT || l == first {
360 break l;
361 }
362 entry = l;
363 };
364 let closed = next == first;
365 if !closed {
366 let s = &segs[entry / 2];
369 push(
370 &mut points,
371 if entry.is_multiple_of(2) {
372 s.to
373 } else {
374 s.from
375 },
376 )?;
377 }
378 let manifold = link[pos] != SPLIT && next != SPLIT;
379 push(
380 out,
381 Polyline {
382 points,
383 closed,
384 manifold,
385 },
386 )?;
387 }
388 }
389 out[start..].sort_unstable_by_key(|c| (c.closed, c.points[0].map(f32::to_bits)));
390 Ok(out.len() - start)
391}
392
393fn reserve<T>(v: &mut Vec<T>, n: usize) -> Result<()> {
395 allocation_allowed()?;
396 v.try_reserve(n).map_err(|_| QueryError::OutOfMemory)
397}
398
399fn push<T>(v: &mut Vec<T>, x: T) -> Result<()> {
401 reserve(v, 1)?;
402 v.push(x);
403 Ok(())
404}
405
406#[derive(Debug, Clone, Copy, PartialEq)]
408struct Seg64 {
409 from: [f64; 3],
410 to: [f64; 3],
411}
412
413impl Seg64 {
414 const EMPTY: Seg64 = Seg64 {
415 from: [0.0; 3],
416 to: [0.0; 3],
417 };
418}
419
420fn clip_pair(pa: [P3; 3], pb: [P3; 3], segs: &mut [Seg64; 6]) -> usize {
428 let (da, db) = (collinear::<false>(&pa), collinear::<false>(&pb));
429 if da || db {
430 return clip_degenerate(pa, da, pb, db, segs);
431 }
432 let sa = pa.map(|p| side::<false>(pb[0], pb[1], pb[2], p));
433 let sb = pb.map(|p| side::<false>(pa[0], pa[1], pa[2], p));
434 if sa == [0; 3] && sb == [0; 3] {
435 return clip_coplanar(pa, pb, segs);
436 }
437 clip_crossing(pa, pb, segs)
438}
439
440fn clip_crossing(pa: [P3; 3], pb: [P3; 3], segs: &mut [Seg64; 6]) -> usize {
444 let (na, nb) = (normal(pa), normal(pb));
445 let mut pts = [[0.0; 3]; 12];
446 let mut n = 0;
447 for (edges, plane, nrm) in [(pa, pb, nb), (pb, pa, na)] {
448 let axis = normal_axis::<false>(&plane).expect("non-degenerate triangle");
449 let plane2 = plane.map(|p| drop_axis(p, axis));
450 for i in 0..3 {
451 let (u, v) = (edges[i], edges[(i + 1) % 3]);
452 let (su, sv) = (
453 side::<false>(plane[0], plane[1], plane[2], u),
454 side::<false>(plane[0], plane[1], plane[2], v),
455 );
456 if su * sv > 0 {
457 continue;
458 }
459 if su == 0 && sv == 0 {
460 for &x in &[u, v] {
462 if in_tri_2d::<false>(drop_axis(x, axis), &plane2) && !pts[..n].contains(&x) {
463 pts[n] = x;
464 n += 1;
465 }
466 }
467 continue;
468 }
469 let x = if su == 0 {
470 u
471 } else if sv == 0 {
472 v
473 } else {
474 edge_plane_cross(u, v, plane[0], nrm)
475 };
476 if !finite(x) {
477 continue;
478 }
479 if in_tri_2d::<false>(drop_axis(x, axis), &plane2) && !pts[..n].contains(&x) {
480 pts[n] = x;
481 n += 1;
482 }
483 }
484 }
485 if n < 2 {
486 return 0;
487 }
488 let d = cross(na, nb);
491 let mut lo = 0;
492 let mut hi = 0;
493 for i in 1..n {
494 if dot(pts[i], d) < dot(pts[lo], d) {
495 lo = i;
496 }
497 if dot(pts[i], d) > dot(pts[hi], d) {
498 hi = i;
499 }
500 }
501 if pts[lo] == pts[hi] {
502 return 0;
503 }
504 segs[0] = Seg64 {
505 from: pts[lo],
506 to: pts[hi],
507 };
508 1
509}
510
511fn edge_plane_cross(u: P3, v: P3, o: P3, n: P3) -> P3 {
516 let (lo, hi) = order(u, v);
517 let hlo = dot(n, sub(lo, o));
518 let hhi = dot(n, sub(hi, o));
519 let t = hlo / (hlo - hhi);
520 let t = if t.is_finite() {
521 t.clamp(0.0, 1.0)
522 } else {
523 0.5
524 };
525 std::array::from_fn(|k| lo[k] + t * (hi[k] - lo[k]))
526}
527
528fn finite(p: P3) -> bool {
532 p.iter().all(|x| x.is_finite())
533}
534
535fn order(a: P3, b: P3) -> (P3, P3) {
537 if a.map(f64::to_bits) <= b.map(f64::to_bits) {
538 (a, b)
539 } else {
540 (b, a)
541 }
542}
543
544fn sub(a: P3, b: P3) -> P3 {
545 std::array::from_fn(|k| a[k] - b[k])
546}
547
548fn dot(a: P3, b: P3) -> f64 {
549 a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
550}
551
552fn cross(a: P3, b: P3) -> P3 {
553 [
554 a[1] * b[2] - a[2] * b[1],
555 a[2] * b[0] - a[0] * b[2],
556 a[0] * b[1] - a[1] * b[0],
557 ]
558}
559
560fn normal(t: [P3; 3]) -> P3 {
561 cross(sub(t[1], t[0]), sub(t[2], t[0]))
562}
563
564fn drop_axis(p: P3, axis: usize) -> [f64; 2] {
566 [p[(axis + 1) % 3], p[(axis + 2) % 3]]
567}
568
569fn clip_stage(
576 poly: &[[f64; 2]],
577 e0: [f64; 2],
578 e1: [f64; 2],
579 s_in: i8,
580 next: &mut [[f64; 2]; 6],
581) -> usize {
582 let n = poly.len();
583 let mut m = 0;
584 for i in 0..n {
585 let (p, q) = (poly[i], poly[(i + 1) % n]);
586 let (sp, sq) = (side2::<false>(e0, e1, p), side2::<false>(e0, e1, q));
587 let pin = sp == 0 || sp == s_in;
588 let qin = sq == 0 || sq == s_in;
589 if pin {
590 if m == 6 {
591 return 0;
592 }
593 next[m] = p;
594 m += 1;
595 }
596 if pin != qin {
597 if m == 6 {
598 return 0;
599 }
600 next[m] = if sp == 0 {
601 p
602 } else if sq == 0 {
603 q
604 } else {
605 cross_2d(p, q, e0, e1)
606 };
607 m += 1;
608 }
609 }
610 m
611}
612
613fn clip_coplanar(pa: [P3; 3], pb: [P3; 3], segs: &mut [Seg64; 6]) -> usize {
616 let axis = normal_axis::<false>(&pa).expect("non-degenerate triangle");
617 let a = pa.map(|p| drop_axis(p, axis));
618 let b = pb.map(|p| drop_axis(p, axis));
619 let mut poly = [a[0], a[1], a[2], a[0], a[0], a[0]];
620 let mut n = 3;
621 for j in 0..3 {
624 let s_in = side2::<false>(b[j], b[(j + 1) % 3], b[(j + 2) % 3]);
625 debug_assert!(s_in != 0);
626 let mut next = [[0.0; 2]; 6];
627 n = clip_stage(&poly[..n], b[j], b[(j + 1) % 3], s_in, &mut next);
628 poly = next;
629 if n == 0 {
630 return 0;
631 }
632 }
633 let mut q = [[0.0; 2]; 6];
635 let mut m = 0;
636 for p in poly.iter().take(n) {
637 if !p.iter().all(|x| x.is_finite()) {
638 continue;
639 }
640 if m == 0 || *p != q[m - 1] {
641 q[m] = *p;
642 m += 1;
643 }
644 }
645 if m > 1 && q[m - 1] == q[0] {
646 m -= 1;
647 }
648 if m < 3 {
649 return 0;
655 }
656 if (0..m).all(|i| side2::<false>(q[i], q[(i + 1) % m], q[(i + 2) % m]) == 0) {
657 return 0;
659 }
660 let corners = [
663 (drop_axis(pa[0], axis), pa[0]),
664 (drop_axis(pa[1], axis), pa[1]),
665 (drop_axis(pa[2], axis), pa[2]),
666 (drop_axis(pb[0], axis), pb[0]),
667 (drop_axis(pb[1], axis), pb[1]),
668 (drop_axis(pb[2], axis), pb[2]),
669 ];
670 let mut pts = [[0.0; 3]; 6];
671 for i in 0..m {
672 pts[i] = lift(q[i], axis, pa, &corners);
673 }
674 for i in 0..m {
675 segs[i] = Seg64 {
676 from: pts[i],
677 to: pts[(i + 1) % m],
678 };
679 }
680 m
681}
682
683fn cross_2d(p: [f64; 2], q: [f64; 2], e0: [f64; 2], e1: [f64; 2]) -> [f64; 2] {
686 let (lo, hi) = if p.map(f64::to_bits) <= q.map(f64::to_bits) {
687 (p, q)
688 } else {
689 (q, p)
690 };
691 let o3 = orient2d_approx(e0, e1, lo);
692 let o4 = orient2d_approx(e0, e1, hi);
693 let t = o3 / (o3 - o4);
694 let t = if t.is_finite() {
695 t.clamp(0.0, 1.0)
696 } else {
697 0.5
698 };
699 [lo[0] + t * (hi[0] - lo[0]), lo[1] + t * (hi[1] - lo[1])]
700}
701
702fn clip_degenerate(pa: [P3; 3], da: bool, pb: [P3; 3], db: bool, segs: &mut [Seg64; 6]) -> usize {
707 let (sa, sb) = (longest_edge(pa), longest_edge(pb));
708 if da && db {
709 return seg_seg_overlap(sa, sb, segs);
710 }
711 let (s, t) = if da { (sa, pb) } else { (sb, pa) };
712 seg_in_plane_overlap(s, t, segs)
713}
714
715fn longest_edge(t: [P3; 3]) -> (P3, P3) {
718 let mut best = (t[0], t[1]);
719 let mut len = dist2(t[0], t[1]);
720 for i in 1..3 {
721 let (u, v) = (t[i], t[(i + 1) % 3]);
722 if dist2(u, v) > len {
723 len = dist2(u, v);
724 best = (u, v);
725 }
726 }
727 best
728}
729
730fn dist2(a: P3, b: P3) -> f64 {
731 (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) + (a[2] - b[2]) * (a[2] - b[2])
732}
733
734fn seg_in_plane_overlap(s: (P3, P3), t: [P3; 3], segs: &mut [Seg64; 6]) -> usize {
737 let (su, sv) = (
738 side::<false>(t[0], t[1], t[2], s.0),
739 side::<false>(t[0], t[1], t[2], s.1),
740 );
741 if su != 0 || sv != 0 {
742 return 0;
744 }
745 let axis = normal_axis::<false>(&t).expect("non-degenerate triangle");
746 let t2 = t.map(|p| drop_axis(p, axis));
747 let (mut p, mut q) = (drop_axis(s.0, axis), drop_axis(s.1, axis));
748 for j in 0..3 {
751 let s_in = side2::<false>(t2[j], t2[(j + 1) % 3], t2[(j + 2) % 3]);
752 debug_assert!(s_in != 0);
753 let (sp, sq) = (
754 side2::<false>(t2[j], t2[(j + 1) % 3], p),
755 side2::<false>(t2[j], t2[(j + 1) % 3], q),
756 );
757 let pin = sp == 0 || sp == s_in;
758 let qin = sq == 0 || sq == s_in;
759 if !pin && !qin {
760 return 0;
761 }
762 if pin && qin {
763 continue;
764 }
765 let x = if sp == 0 {
766 p
767 } else if sq == 0 {
768 q
769 } else {
770 cross_2d(p, q, t2[j], t2[(j + 1) % 3])
771 };
772 if pin {
773 q = x;
774 } else {
775 p = x;
776 }
777 }
778 if p == q {
779 return 0;
780 }
781 let corners = [
782 (drop_axis(s.0, axis), s.0),
783 (drop_axis(s.1, axis), s.1),
784 (t2[0], t[0]),
785 (t2[1], t[1]),
786 (t2[2], t[2]),
787 ];
788 let (a, b) = (lift(p, axis, t, &corners), lift(q, axis, t, &corners));
789 if a == b {
790 return 0;
791 }
792 segs[0] = Seg64 { from: a, to: b };
793 1
794}
795
796fn seg_seg_overlap(a: (P3, P3), b: (P3, P3), segs: &mut [Seg64; 6]) -> usize {
799 let (p, q, r, s) = (a.0, a.1, b.0, b.1);
800 if side::<false>(p, q, r, s) != 0 {
801 return 0;
802 }
803 for t in [[p, q, r], [p, q, s], [r, s, p], [r, s, q]] {
806 if let Some(k) = normal_axis::<false>(&t) {
807 let (p2, q2, r2, s2) = (
808 drop_axis(p, k),
809 drop_axis(q, k),
810 drop_axis(r, k),
811 drop_axis(s, k),
812 );
813 if side2::<false>(p2, q2, r2) != 0 || side2::<false>(p2, q2, s2) != 0 {
814 return 0;
816 }
817 return collinear_overlap_2d(
818 p2,
819 q2,
820 r2,
821 s2,
822 [(p, p2), (q, q2), (r, r2), (s, s2)],
823 segs,
824 );
825 }
826 }
827 let mut k = 0;
829 for j in 1..3 {
830 let spread = |k: usize| {
831 [p[k], q[k], r[k], s[k]]
832 .into_iter()
833 .fold((f64::INFINITY, f64::NEG_INFINITY), |(lo, hi), x| {
834 (lo.min(x), hi.max(x))
835 })
836 };
837 let (lo0, hi0) = spread(k);
838 let (lo1, hi1) = spread(j);
839 if hi1 - lo1 > hi0 - lo0 {
840 k = j;
841 }
842 }
843 line_overlap(p, q, r, s, k, segs)
844}
845
846fn collinear_overlap_2d(
849 p: [f64; 2],
850 q: [f64; 2],
851 r: [f64; 2],
852 s: [f64; 2],
853 pts: [(P3, [f64; 2]); 4],
854 segs: &mut [Seg64; 6],
855) -> usize {
856 let k = if (p[0] - q[0]).abs().max((r[0] - s[0]).abs())
858 >= (p[1] - q[1]).abs().max((r[1] - s[1]).abs())
859 {
860 0
861 } else {
862 1
863 };
864 let key = |x: [f64; 2]| x[k];
865 let (lo, hi) = (
866 key(p).min(key(q)).max(key(r).min(key(s))),
867 key(p).max(key(q)).min(key(r).max(key(s))),
868 );
869 if lo >= hi {
870 return 0;
871 }
872 let at = |t: f64| pts.iter().find(|(_, x)| key(*x) == t).map(|(p, _)| *p);
874 match (at(lo), at(hi)) {
875 (Some(a), Some(b)) if a != b => {
876 segs[0] = Seg64 { from: a, to: b };
877 1
878 }
879 _ => 0,
880 }
881}
882
883fn line_overlap(p: P3, q: P3, r: P3, s: P3, k: usize, segs: &mut [Seg64; 6]) -> usize {
885 let (lo, hi) = (
886 p[k].min(q[k]).max(r[k].min(s[k])),
887 p[k].max(q[k]).min(r[k].max(s[k])),
888 );
889 if lo >= hi {
890 return 0;
891 }
892 let at = |t: f64| [p, q, r, s].into_iter().find(|x| x[k] == t);
893 match (at(lo), at(hi)) {
894 (Some(a), Some(b)) if a != b => {
895 segs[0] = Seg64 { from: a, to: b };
896 1
897 }
898 _ => 0,
899 }
900}
901
902fn lift(x: [f64; 2], axis: usize, plane: [P3; 3], corners: &[([f64; 2], P3)]) -> P3 {
906 if let Some((_, c)) = corners.iter().find(|(q, _)| *q == x) {
907 return *c;
908 }
909 let p = plane.map(|q| drop_axis(q, axis));
912 let d = (p[1][0] - p[0][0]) * (p[2][1] - p[0][1]) - (p[2][0] - p[0][0]) * (p[1][1] - p[0][1]);
913 let c = if d == 0.0 {
914 plane[0][axis]
915 } else {
916 let u =
917 ((x[0] - p[0][0]) * (p[2][1] - p[0][1]) - (x[1] - p[0][1]) * (p[2][0] - p[0][0])) / d;
918 let v =
919 ((x[1] - p[0][1]) * (p[1][0] - p[0][0]) - (x[0] - p[0][0]) * (p[1][1] - p[0][1])) / d;
920 plane[0][axis]
921 + u * (plane[1][axis] - plane[0][axis])
922 + v * (plane[2][axis] - plane[0][axis])
923 };
924 let mut out = [c; 3];
925 out[(axis + 1) % 3] = x[0];
926 out[(axis + 2) % 3] = x[1];
927 out
928}
929
930#[cfg(test)]
931mod tests {
932 use super::*;
933 use crate::test_util::{box_mesh, build, fin};
934 use fast_mesh_bvh::Quality;
935 use fast_mesh_store::Mesh;
936
937 fn clip_set(pa: [P3; 3], pb: [P3; 3]) -> Vec<([u64; 3], [u64; 3])> {
939 let mut buf = [Seg64::EMPTY; 6];
940 let n = clip_pair(pa, pb, &mut buf);
941 assert!(n <= 6);
942 let mut out: Vec<_> = buf[..n]
943 .iter()
944 .map(|s| {
945 let (a, b) = (s.from.map(f64::to_bits), s.to.map(f64::to_bits));
946 if a <= b { (a, b) } else { (b, a) }
947 })
948 .collect();
949 out.sort_unstable();
950 out
951 }
952
953 fn bits(p: [f64; 3]) -> [u64; 3] {
954 p.map(f64::to_bits)
955 }
956
957 const A: [P3; 3] = [[0.0, 0.0, 0.0], [2.0, 0.0, 0.0], [0.0, 2.0, 0.0]];
958
959 #[test]
960 fn clip_crossing_pair() {
961 let b = [[0.5, -1.0, -1.0], [0.5, -1.0, 1.0], [0.5, 2.0, 0.0]];
963 assert!(tri_tri_intersect(A, b));
964 assert_eq!(
965 clip_set(A, b),
966 vec![(bits([0.5, 0.0, 0.0]), bits([0.5, 1.5, 0.0]))]
967 );
968 assert_eq!(clip_set(b, A), clip_set(A, b));
969 }
970
971 #[test]
972 fn clip_coplanar_overlap() {
973 let b = [[1.0, 0.0, 0.0], [3.0, 0.0, 0.0], [1.0, 2.0, 0.0]];
975 assert!(tri_tri_intersect(A, b));
976 let (p, q, r) = (
977 bits([1.0, 0.0, 0.0]),
978 bits([2.0, 0.0, 0.0]),
979 bits([1.0, 1.0, 0.0]),
980 );
981 assert_eq!(clip_set(A, b), vec![(p, r), (p, q), (r, q)]);
982 }
983
984 #[test]
985 fn clip_stage_bails_out_past_six_vertices() {
986 let poly = [[0.0, 1.0], [1.0, -1.0], [2.0, 1.0], [3.0, -1.0], [4.0, 1.0]];
992 let mut next = [[0.0; 2]; 6];
993 assert_eq!(
994 clip_stage(&poly, [-10.0, 0.0], [10.0, 0.0], 1, &mut next),
995 0
996 );
997 }
998
999 #[test]
1000 fn clip_point_touch_gives_nothing() {
1001 let b = [[2.0, 0.0, 0.0], [3.0, 0.0, 1.0], [3.0, 1.0, 1.0]];
1002 assert!(tri_tri_intersect(A, b));
1003 assert!(clip_set(A, b).is_empty());
1004 }
1005
1006 #[test]
1007 fn clip_degenerate_pairs() {
1008 let pierce = [[0.5, 0.5, -1.0], [0.5, 0.5, 1.0], [0.5, 0.5, 1.0]];
1010 assert!(tri_tri_intersect(A, pierce));
1011 assert!(clip_set(A, pierce).is_empty());
1012 let flat = [[0.25, 0.25, 0.0], [0.75, 0.75, 0.0], [0.5, 0.5, 0.0]];
1014 assert!(tri_tri_intersect(A, flat));
1015 assert_eq!(
1016 clip_set(A, flat),
1017 vec![(bits([0.25, 0.25, 0.0]), bits([0.75, 0.75, 0.0]))]
1018 );
1019 }
1020
1021 fn meshes(pa: &[f32], ia: &[u32], pb: &[f32], ib: &[u32]) -> (Mesh, Bvh, Mesh, Bvh) {
1023 let ma = Mesh::from_buffers(pa, ia).unwrap();
1024 let mb = Mesh::from_buffers(pb, ib).unwrap();
1025 let (ba, bb) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1026 (ma, ba, mb, bb)
1027 }
1028
1029 fn run(ma: &Mesh, ba: &Bvh, mb: &Mesh, bb: &Bvh, xf: &Rigid) -> Vec<Polyline> {
1030 let mut out = Vec::new();
1031 let n = intersection_polylines(ma, ba, mb, bb, xf, &mut out).unwrap();
1032 assert_eq!(n, out.len());
1033 out
1034 }
1035
1036 #[test]
1037 fn box_cut_by_sheet_is_a_closed_loop() {
1038 let ma = box_mesh([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]);
1039 let mb = Mesh::from_buffers(
1040 &[
1041 -2.0, -2.0, 0.0, 2.0, -2.0, 0.0, 2.0, 2.0, 0.0, -2.0, 2.0, 0.0,
1042 ],
1043 &[0, 1, 2, 0, 2, 3],
1044 )
1045 .unwrap();
1046 let (ma_bvh, mb_bvh) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1047 let out = run(&ma, &ma_bvh, &mb, &mb_bvh, &Rigid::IDENTITY);
1048 assert_eq!(out.len(), 1);
1049 assert!(out[0].closed && out[0].manifold);
1050 let mut pts = out[0].points.clone();
1053 pts.sort_by_key(|p| p.map(f32::to_bits));
1054 assert_eq!(
1055 pts,
1056 vec![
1057 [0.0, 1.0, 0.0],
1058 [0.0, -1.0, 0.0],
1059 [1.0, 0.0, 0.0],
1060 [1.0, 1.0, 0.0],
1061 [1.0, -1.0, 0.0],
1062 [-1.0, 0.0, 0.0],
1063 [-1.0, 1.0, 0.0],
1064 [-1.0, -1.0, 0.0],
1065 ]
1066 );
1067 }
1068
1069 #[test]
1070 fn coplanar_overlap_chains_its_boundary() {
1071 let (ma, ba, mb, bb) = meshes(
1074 &[0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 2.0, 2.0, 0.0, 0.0, 2.0, 0.0],
1075 &[0, 1, 2, 0, 2, 3],
1076 &[1.0, 1.0, 0.0, 3.0, 1.0, 0.0, 3.0, 3.0, 0.0, 1.0, 3.0, 0.0],
1077 &[0, 1, 2, 0, 2, 3],
1078 );
1079 let out = run(&ma, &ba, &mb, &bb, &Rigid::IDENTITY);
1080 assert_eq!(out.len(), 1);
1081 assert!(out[0].closed && out[0].manifold);
1082 let mut pts = out[0].points.clone();
1083 pts.sort_by_key(|p| p.map(f32::to_bits));
1084 assert_eq!(
1085 pts,
1086 vec![
1087 [1.0, 1.0, 0.0],
1088 [1.0, 2.0, 0.0],
1089 [2.0, 1.0, 0.0],
1090 [2.0, 2.0, 0.0],
1091 ]
1092 );
1093 }
1094
1095 #[test]
1096 fn nonmanifold_junction_splits_chains() {
1097 let ma = fin();
1100 let mb = Mesh::from_buffers(
1101 &[
1102 -2.0, -2.0, 0.5, 2.0, -2.0, 0.5, 2.0, 2.0, 0.5, -2.0, 2.0, 0.5,
1103 ],
1104 &[0, 1, 2, 0, 2, 3],
1105 )
1106 .unwrap();
1107 let (ba, bb) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1108 let out = run(&ma, &ba, &mb, &bb, &Rigid::IDENTITY);
1109 assert_eq!(out.len(), 3);
1110 for c in &out {
1111 assert!(!c.closed && !c.manifold);
1112 assert_eq!(c.points[0], [0.0, 0.0, 0.5]);
1113 assert_eq!(c.points.len(), 3);
1114 }
1115 }
1116
1117 #[test]
1118 fn cancel_dupes_keeps_one_per_direction_group() {
1119 let (u, v, w) = ([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]);
1120 let seg = |from, to| Seg64 { from, to };
1121 let mut segs = vec![
1122 seg(u, v),
1123 seg(v, u), seg(u, w),
1125 seg(u, w), seg(v, w), ];
1128 cancel_dupes(&mut segs);
1129 let mut got: Vec<_> = segs.iter().map(|s| (bits(s.from), bits(s.to))).collect();
1130 got.sort_unstable();
1131 assert_eq!(got, vec![(bits(u), bits(w)), (bits(v), bits(w))]);
1132 }
1133
1134 #[test]
1135 fn chain_lists_open_polylines_before_closed_loops() {
1136 let seg = |from, to| Seg32 { from, to };
1140 let segs = vec![
1141 seg([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
1142 seg([1.0, 0.0, 0.0], [1.0, 1.0, 0.0]),
1143 seg([1.0, 1.0, 0.0], [0.0, 1.0, 0.0]),
1144 seg([0.0, 1.0, 0.0], [0.0, 0.0, 0.0]),
1145 seg([5.0, 0.0, 0.0], [6.0, 0.0, 0.0]),
1146 seg([6.0, 0.0, 0.0], [7.0, 0.0, 0.0]),
1147 ];
1148 let mut out = Vec::new();
1149 let n = chain(&segs, &mut out).unwrap();
1150 assert_eq!(n, 2);
1151 assert!(!out[0].closed, "open chain sorts first, got {out:?}");
1152 assert!(out[1].closed);
1153 assert_eq!(out[0].points[0], [5.0, 0.0, 0.0]);
1154 assert_eq!(out[1].points[0], [0.0, 0.0, 0.0]);
1155 }
1156
1157 #[test]
1158 fn coplanar_edge_touch_gives_nothing() {
1159 let b = [[2.0, 0.0, 0.0], [3.0, 0.0, 0.0], [2.0, 1.0, 0.0]];
1162 assert!(tri_tri_intersect(A, b));
1163 assert!(clip_set(A, b).is_empty());
1164 }
1165
1166 #[test]
1167 fn segments_match_brute_force_and_lie_on_both_meshes() {
1168 use crate::test_util::Lcg;
1169 use crate::tri::closest_on_triangle;
1170 let ma = box_mesh([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]);
1171 let mut rng = Lcg(0x24);
1172 let mut total = 0;
1173 for round in 0..6 {
1174 let xf = if round < 4 {
1177 Rigid::IDENTITY
1178 } else {
1179 Rigid {
1180 rotation: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]],
1181 translation: [0.5, -0.25, 0.0],
1182 }
1183 };
1184 let c = if round < 4 {
1185 [
1186 (rng.next() * 2.0 - 1.0) * 0.8,
1187 (rng.next() * 2.0 - 1.0) * 0.8,
1188 (rng.next() * 2.0 - 1.0) * 0.8,
1189 ]
1190 } else {
1191 [0.0, 0.0, 0.0]
1192 };
1193 let mb = box_mesh(
1194 [c[0] - 0.4, c[1] - 0.4, c[2] - 0.4],
1195 [c[0] + 0.4, c[1] + 0.4, c[2] + 0.4],
1196 );
1197 let (ma_bvh, mb_bvh) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1198 let out = run(&ma, &ma_bvh, &mb, &mb_bvh, &xf);
1199 let (pta, pta_t, ptb, ptb_t) = (
1201 ma.positions(),
1202 ma.triangles(),
1203 mb.positions(),
1204 mb.triangles(),
1205 );
1206 let mut brute: Vec<Seg64> = Vec::new();
1207 let mut buf = [Seg64::EMPTY; 6];
1208 for ta in pta_t {
1209 let pa = ta.map(|v| {
1210 let p = pta[v as usize];
1211 p.map(f64::from)
1212 });
1213 for tb in ptb_t {
1214 let pb = tb.map(|v| xf.apply(ptb[v as usize]));
1215 if !tri_tri_intersect(pa, pb) {
1216 continue;
1217 }
1218 let n = clip_pair(pa, pb, &mut buf);
1219 brute.extend(buf[..n].iter().filter(|s| s.from != s.to).copied());
1220 }
1221 }
1222 cancel_dupes(&mut brute);
1223 let mut want: Vec<_> = brute
1224 .iter()
1225 .map(|s| {
1226 let (a, b) = (
1227 s.from.map(|x| (x as f32).to_bits()),
1228 s.to.map(|x| (x as f32).to_bits()),
1229 );
1230 if a <= b { (a, b) } else { (b, a) }
1231 })
1232 .filter(|(a, b)| a != b)
1233 .collect();
1234 want.sort_unstable();
1235 let mut got = Vec::new();
1236 for c in &out {
1237 let n = c.points.len();
1239 let end = if c.closed { n } else { n - 1 };
1240 for i in 0..end {
1241 let (p, q) = (c.points[i], c.points[(i + 1) % n]);
1242 let (a, b) = (p.map(f32::to_bits), q.map(f32::to_bits));
1243 got.push(if a <= b { (a, b) } else { (b, a) });
1244 let m = std::array::from_fn(|k| f64::from(p[k] + q[k]) / 2.0);
1245 let on_a = pta_t.iter().any(|t| {
1246 let tri = t.map(|v| pta[v as usize].map(f64::from));
1247 closest_on_triangle(m, tri).dist2 < 1e-10
1248 });
1249 let on_b = ptb_t.iter().any(|t| {
1250 let tri = t.map(|v| xf.apply(ptb[v as usize]));
1251 closest_on_triangle(m, tri).dist2 < 1e-10
1252 });
1253 assert!(on_a, "midpoint {m:?} off A");
1254 assert!(on_b, "midpoint {m:?} off B");
1255 }
1256 }
1257 got.sort_unstable();
1258 assert_eq!(got, want);
1259 total += got.len();
1260 }
1261 assert!(total > 0, "no segments over all rounds");
1262 }
1263
1264 #[test]
1265 fn vertex_touch_has_no_polylines() {
1266 let (ma, ba, mb, bb) = meshes(
1267 &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
1268 &[0, 1, 2],
1269 &[0.25, 0.25, 0.0, 1.0, 0.25, 1.0, 0.25, 1.0, 1.0],
1270 &[0, 1, 2],
1271 );
1272 assert!(run(&ma, &ba, &mb, &bb, &Rigid::IDENTITY).is_empty());
1273 }
1274
1275 #[test]
1276 fn validation_errors_leave_out_alone() {
1277 let (ma, ba, mb, bb) = meshes(
1278 &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
1279 &[0, 1, 2],
1280 &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
1281 &[0, 1, 2],
1282 );
1283 let bad = Rigid {
1284 rotation: [[2.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
1285 translation: [0.0; 3],
1286 };
1287 let mut out = vec![Polyline::default()];
1288 let is_invalid = |e: QueryError| matches!(e, QueryError::InvalidInput(_));
1289 assert!(is_invalid(
1290 intersection_polylines(&ma, &ba, &mb, &bb, &bad, &mut out).unwrap_err()
1291 ));
1292 assert_eq!(out.len(), 1);
1293 assert!(is_invalid(
1295 intersection_polylines(&ma, &bb, &mb, &bb, &bad, &mut out).unwrap_err()
1296 ));
1297 assert_eq!(
1298 intersection_polylines(&mb, &ba, &ma, &ba, &Rigid::IDENTITY, &mut out),
1299 Err(QueryError::MeshMismatch)
1300 );
1301 assert_eq!(out.len(), 1);
1302 }
1303
1304 #[test]
1305 fn out_of_memory_restores_out() {
1306 use crate::fail_allocations_after;
1307 let ma = box_mesh([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]);
1308 let mb = box_mesh([0.4, -0.5, -0.5], [1.4, 0.5, 0.5]);
1309 let (ba, bb) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1310 assert!(!run(&ma, &ba, &mb, &bb, &Rigid::IDENTITY).is_empty());
1312 fail_allocations_after(Some(0));
1313 let mut out = Vec::new();
1314 assert_eq!(
1315 intersection_polylines(&ma, &ba, &mb, &bb, &Rigid::IDENTITY, &mut out),
1316 Err(QueryError::OutOfMemory)
1317 );
1318 assert!(out.is_empty());
1319 fail_allocations_after(None);
1320 }
1321
1322 fn segments_of(ma: &Mesh, ba: &Bvh, mb: &Mesh, bb: &Bvh) -> Vec<PairSegment> {
1323 let mut out = Vec::new();
1324 let n = intersecting_segments(ma, ba, mb, bb, &Rigid::IDENTITY, &mut out).unwrap();
1325 assert_eq!(n, out.len());
1326 out
1327 }
1328
1329 #[test]
1330 fn pair_segments_cover_overlapping_boxes() {
1331 let ma = box_mesh([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0]);
1332 let mb = box_mesh([0.4, -0.5, -0.5], [1.4, 0.5, 0.5]);
1333 let (ma_bvh, mb_bvh) = (build(&ma, Quality::Sah), build(&mb, Quality::Sah));
1334 let segs = segments_of(&ma, &ma_bvh, &mb, &mb_bvh);
1335 assert!(!segs.is_empty());
1336 for s in &segs {
1338 assert_ne!(s.from, s.to);
1339 assert!(ma.triangle(s.a).is_some());
1340 assert!(mb.triangle(s.b).is_some());
1341 }
1342 let far = box_mesh([5.0, 5.0, 5.0], [6.0, 6.0, 6.0]);
1344 let far_bvh = build(&far, Quality::Sah);
1345 assert!(segments_of(&ma, &ma_bvh, &far, &far_bvh).is_empty());
1346 }
1347
1348 #[test]
1349 fn pair_segments_include_coplanar_boundaries() {
1350 let (ma, ba, mb, bb) = meshes(
1353 &[0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 2.0, 2.0, 0.0, 0.0, 2.0, 0.0],
1354 &[0, 1, 2, 0, 2, 3],
1355 &[1.0, 1.0, 0.0, 3.0, 1.0, 0.0, 3.0, 3.0, 0.0, 1.0, 3.0, 0.0],
1356 &[0, 1, 2, 0, 2, 3],
1357 );
1358 let segs = segments_of(&ma, &ba, &mb, &bb);
1359 assert!(!segs.is_empty());
1363 for s in &segs {
1364 assert_ne!(s.from, s.to);
1365 }
1366 }
1367
1368 #[test]
1369 fn pair_segments_leave_out_alone_on_error() {
1370 let (ma, ba, mb, bb) = meshes(
1371 &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
1372 &[0, 1, 2],
1373 &[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0],
1374 &[0, 1, 2],
1375 );
1376 let bad = Rigid {
1377 rotation: [[2.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
1378 translation: [0.0; 3],
1379 };
1380 let mut out = vec![PairSegment {
1381 a: 0,
1382 b: 0,
1383 from: [0.0; 3],
1384 to: [1.0, 0.0, 0.0],
1385 }];
1386 assert!(intersecting_segments(&ma, &ba, &mb, &bb, &bad, &mut out).is_err());
1387 assert_eq!(out.len(), 1);
1388 }
1389}