1use super::exact::backend::{rat_zero, Int, Signed};
16
17use crate::linalg::Vec3;
18
19use super::exact::approx::orient2d_a;
20use super::exact::filtered::orient3d;
21use super::exact::predicates::{
22 homog2_of, line_line_intersect_2d, line_plane_intersect, orient2d_h, tri_normal_r,
23 Homog2,
24};
25use super::exact::rational::{r2_eq, rat_to_f64, R2, R3};
26use super::exact::Sign;
27
28#[derive(Clone, Debug, PartialEq)]
30pub enum TriTriIsect {
31 None,
32 Point(R3),
35 Segment(R3, R3),
37 Coplanar {
42 polygon: Vec<R3>,
43 same_orientation: bool,
44 },
45}
46
47pub fn dominant_axis(n: &R3) -> usize {
52 let ax = n.x.abs();
53 let ay = n.y.abs();
54 let az = n.z.abs();
55 if az >= ax && az >= ay {
56 2
57 } else if ay >= ax {
58 1
59 } else {
60 0
61 }
62}
63
64pub use super::exact::predicates::lift_to_plane;
67
68pub mod stats {
71 use std::sync::atomic::{AtomicU64, Ordering::Relaxed};
72
73 pub static PLANE_REJECT: AtomicU64 = AtomicU64::new(0);
74 pub static COPLANAR: AtomicU64 = AtomicU64::new(0);
75 pub static COPLANAR_SAT: AtomicU64 = AtomicU64::new(0);
76 pub static SAT_REJECT: AtomicU64 = AtomicU64::new(0);
77 pub static INTERVAL: AtomicU64 = AtomicU64::new(0);
78 pub static COPLANAR_NS: AtomicU64 = AtomicU64::new(0);
79 pub static COPLANAR_CLIP_NS: AtomicU64 = AtomicU64::new(0);
83 pub static PLANE_NS: AtomicU64 = AtomicU64::new(0);
84 pub static INTERVAL_NS: AtomicU64 = AtomicU64::new(0);
85
86 pub fn snapshot_and_reset() -> String {
87 let take = |a: &AtomicU64| a.swap(0, Relaxed);
88 format!(
89 "plane-reject {} ({:.3}s signs), coplanar {} (sat {}, {:.3}s of which clip {:.3}s), sat-reject {}, interval {} ({:.3}s)",
90 take(&PLANE_REJECT),
91 take(&PLANE_NS) as f64 * 1e-9,
92 take(&COPLANAR),
93 take(&COPLANAR_SAT),
94 take(&COPLANAR_NS) as f64 * 1e-9,
95 take(&COPLANAR_CLIP_NS) as f64 * 1e-9,
96 take(&SAT_REJECT),
97 take(&INTERVAL),
98 take(&INTERVAL_NS) as f64 * 1e-9,
99 )
100 }
101}
102
103pub fn tri_tri_intersect(t1: [Vec3; 3], t2: [Vec3; 3]) -> TriTriIsect {
106 use std::sync::atomic::Ordering::Relaxed;
107 let t_signs = crate::timing::Stopwatch::start();
108 let s2 = [
110 orient3d(t1[0], t1[1], t1[2], t2[0]),
111 orient3d(t1[0], t1[1], t1[2], t2[1]),
112 orient3d(t1[0], t1[1], t1[2], t2[2]),
113 ];
114 if all_same_strict(&s2) {
115 stats::PLANE_REJECT.fetch_add(1, Relaxed);
116 stats::PLANE_NS.fetch_add(t_signs.elapsed_ns(), Relaxed);
117 return TriTriIsect::None;
118 }
119 if s2.iter().all(|s| *s == Sign::Zero) {
120 stats::COPLANAR.fetch_add(1, Relaxed);
121 let out = coplanar_overlap(t1, t2);
122 stats::COPLANAR_NS.fetch_add(t_signs.elapsed_ns(), Relaxed);
123 return out;
124 }
125 let s1 = [
127 orient3d(t2[0], t2[1], t2[2], t1[0]),
128 orient3d(t2[0], t2[1], t2[2], t1[1]),
129 orient3d(t2[0], t2[1], t2[2], t1[2]),
130 ];
131 if all_same_strict(&s1) {
132 stats::PLANE_REJECT.fetch_add(1, Relaxed);
133 stats::PLANE_NS.fetch_add(t_signs.elapsed_ns(), Relaxed);
134 return TriTriIsect::None;
135 }
136 stats::PLANE_NS.fetch_add(t_signs.elapsed_ns(), Relaxed);
137 debug_assert!(
138 !s1.iter().all(|s| *s == Sign::Zero),
139 "t1 coplanar with t2's plane implies t2 coplanar with t1's — handled above"
140 );
141
142 {
147 let f1 = [
148 [t1[0].x, t1[0].y, t1[0].z],
149 [t1[1].x, t1[1].y, t1[1].z],
150 [t1[2].x, t1[2].y, t1[2].z],
151 ];
152 let f2 = [
153 [t2[0].x, t2[0].y, t2[0].z],
154 [t2[1].x, t2[1].y, t2[1].z],
155 [t2[2].x, t2[2].y, t2[2].z],
156 ];
157 if super::exact::approx::sat_edge_axes_disjoint(&f1, &f2) {
158 stats::SAT_REJECT.fetch_add(1, Relaxed);
159 return TriTriIsect::None;
160 }
161 }
162 stats::INTERVAL.fetch_add(1, Relaxed);
163 let t_interval = crate::timing::Stopwatch::start();
164
165 let out = interval_overlap(t1, t2, &s1, &s2);
175 stats::INTERVAL_NS.fetch_add(t_interval.elapsed_ns(), Relaxed);
176 out
177}
178
179#[derive(Clone, Copy)]
182enum EndPt {
183 Vert(u8, u8),
185 Cross(u8, u8),
187}
188
189fn interval_overlap(t1: [Vec3; 3], t2: [Vec3; 3], s1: &[Sign; 3], s2: &[Sign; 3]) -> TriTriIsect {
190 let degenerate_at = |s: &[Sign; 3]| -> Option<usize> {
197 (0..3).find(|&i| {
198 s[i] == Sign::Zero
199 && s[(i + 1) % 3] != Sign::Zero
200 && s[(i + 1) % 3] == s[(i + 2) % 3]
201 })
202 };
203 if let (Some(i), Some(j)) = (degenerate_at(s1), degenerate_at(s2)) {
204 return if t1[i] == t2[j] {
206 TriTriIsect::Point(R3::from_vec3(t1[i]))
207 } else {
208 TriTriIsect::None
209 };
210 }
211
212 let sx = super::exact::intpred::scaled_big([
215 t1[0].x, t1[1].x, t1[2].x, t2[0].x, t2[1].x, t2[2].x,
216 ]);
217 let sy = super::exact::intpred::scaled_big([
218 t1[0].y, t1[1].y, t1[2].y, t2[0].y, t2[1].y, t2[2].y,
219 ]);
220 let sz = super::exact::intpred::scaled_big([
221 t1[0].z, t1[1].z, t1[2].z, t2[0].z, t2[1].z, t2[2].z,
222 ]);
223 let v = |k: usize| [&sx[k], &sy[k], &sz[k]];
224 let sub = |a: [∬ 3], b: [∬ 3]| [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
225 let cross = |a: &[Int; 3], b: &[Int; 3]| {
226 [
227 &a[1] * &b[2] - &a[2] * &b[1],
228 &a[2] * &b[0] - &a[0] * &b[2],
229 &a[0] * &b[1] - &a[1] * &b[0],
230 ]
231 };
232 let dot = |a: &[Int; 3], b: [∬ 3]| &a[0] * b[0] + &a[1] * b[1] + &a[2] * b[2];
233
234 let n1 = cross(&sub(v(1), v(0)), &sub(v(2), v(0)));
235 let n2 = cross(&sub(v(4), v(3)), &sub(v(5), v(3)));
236 let dir = cross(&n1, &n2);
237 debug_assert!(
238 dir.iter().any(|c| !c.is_zero()),
239 "non-coplanar intersecting planes"
240 );
241
242 let du = |k: usize| dot(&dir, v(k));
247 let h = |k: usize, n: &[Int; 3], origin: usize| dot(n, v(k)) - dot(n, v(origin));
248 #[cfg(debug_assertions)]
249 for i in 0..3 {
250 debug_assert_eq!(int_sign(&h(i, &n2, 3)), s1[i], "scaled height disagrees with s1");
251 debug_assert_eq!(int_sign(&h(3 + i, &n1, 0)), s2[i], "scaled height disagrees with s2");
252 }
253
254 let endpoints = |which: u8, s: &[Sign; 3]| -> Vec<(Frac, EndPt)> {
259 let base = if which == 0 { 0 } else { 3 };
260 let (n, origin) = if which == 0 { (&n2, 3) } else { (&n1, 0) };
261 let mut pts = Vec::with_capacity(2);
262 for i in 0..3 {
263 if s[i] == Sign::Zero {
264 pts.push((
265 (du(base + i), Int::from(1)),
266 EndPt::Vert(which, i as u8),
267 ));
268 }
269 }
270 for i in 0..3 {
271 let j = (i + 1) % 3;
272 if s[i] != Sign::Zero && s[j] != Sign::Zero && s[i] != s[j] {
273 let hu = h(base + i, n, origin);
276 let hv = h(base + j, n, origin);
277 let du_u = du(base + i);
278 let du_v = du(base + j);
279 let mut den = &hu - &hv;
280 let mut num = &den * &du_u + &hu * (&du_v - &du_u);
281 if den.is_negative() {
282 den = -den;
283 num = -num;
284 }
285 pts.push(((num, den), EndPt::Cross(which, i as u8)));
286 }
287 }
288 debug_assert!(!pts.is_empty() && pts.len() <= 2);
289 pts
290 };
291 let pts1 = endpoints(0, s1);
292 let pts2 = endpoints(1, s2);
293
294 let minmax = |pts: Vec<(Frac, EndPt)>| -> ((Frac, EndPt), (Frac, EndPt)) {
297 let mut lo = pts[0].clone();
298 let mut hi = pts[0].clone();
299 for p in &pts[1..] {
300 if cmp_frac(&p.0, &lo.0) == std::cmp::Ordering::Less {
301 lo = p.clone();
302 }
303 if cmp_frac(&p.0, &hi.0) == std::cmp::Ordering::Greater {
304 hi = p.clone();
305 }
306 }
307 (lo, hi)
308 };
309 let i1 = minmax(pts1);
310 let i2 = minmax(pts2);
311 let (lo, lo_pt) = if cmp_frac(&i1.0 .0, &i2.0 .0) != std::cmp::Ordering::Less { i1.0 } else { i2.0 };
312 let (hi, hi_pt) = if cmp_frac(&i1.1 .0, &i2.1 .0) != std::cmp::Ordering::Greater { i1.1 } else { i2.1 };
313
314 match cmp_frac(&lo, &hi) {
315 std::cmp::Ordering::Greater => TriTriIsect::None,
316 std::cmp::Ordering::Equal => TriTriIsect::Point(build_endpoint(lo_pt, &t1, &t2)),
317 std::cmp::Ordering::Less => TriTriIsect::Segment(
318 build_endpoint(lo_pt, &t1, &t2),
319 build_endpoint(hi_pt, &t1, &t2),
320 ),
321 }
322}
323
324#[cfg(debug_assertions)]
325fn int_sign(v: &Int) -> Sign {
326 if v.is_zero() {
327 Sign::Zero
328 } else if v.is_negative() {
329 Sign::Neg
330 } else {
331 Sign::Pos
332 }
333}
334
335fn build_endpoint(e: EndPt, t1: &[Vec3; 3], t2: &[Vec3; 3]) -> R3 {
338 let tri = |which: u8| if which == 0 { t1 } else { t2 };
339 match e {
340 EndPt::Vert(w, i) => R3::from_vec3(tri(w)[i as usize]),
341 EndPt::Cross(w, i) => {
342 let own = tri(w);
343 let other = tri(1 - w);
344 let a = R3::from_vec3(own[i as usize]);
345 let b = R3::from_vec3(own[(i as usize + 1) % 3]);
346 let p: [R3; 3] = [
347 R3::from_vec3(other[0]),
348 R3::from_vec3(other[1]),
349 R3::from_vec3(other[2]),
350 ];
351 line_plane_intersect(&a, &b, &p[0], &p[1], &p[2])
352 .expect("strictly straddling edge cannot be parallel to the plane")
353 }
354 }
355}
356
357type Frac = (Int, Int);
359
360fn cmp_frac(a: &Frac, b: &Frac) -> std::cmp::Ordering {
361 (&a.0 * &b.1).cmp(&(&b.0 * &a.1))
363}
364
365fn all_same_strict(s: &[Sign; 3]) -> bool {
366 s[0] != Sign::Zero && s[0] == s[1] && s[1] == s[2]
367}
368
369fn coplanar_separated_2d(t1: [Vec3; 3], t2: [Vec3; 3], axis: usize) -> bool {
378 use super::exact::filtered::orient2d;
379 let proj = |v: Vec3| match axis {
381 0 => crate::linalg::Vec2::new(v.y, v.z),
382 1 => crate::linalg::Vec2::new(v.z, v.x),
383 _ => crate::linalg::Vec2::new(v.x, v.y),
384 };
385 let p1 = t1.map(proj);
386 let p2 = t2.map(proj);
387 let separates = |tri: &[crate::linalg::Vec2; 3], other: &[crate::linalg::Vec2; 3]| {
388 (0..3).any(|i| {
389 let a = tri[i];
390 let b = tri[(i + 1) % 3];
391 let s_ref = orient2d(a, b, tri[(i + 2) % 3]);
392 s_ref != Sign::Zero
393 && other.iter().all(|&q| {
394 let s = orient2d(a, b, q);
395 s != Sign::Zero && s != s_ref
396 })
397 })
398 };
399 separates(&p1, &p2) || separates(&p2, &p1)
400}
401
402fn coplanar_overlap(t1: [Vec3; 3], t2: [Vec3; 3]) -> TriTriIsect {
406 {
407 let n = crate::linalg::cross(t1[1] - t1[0], t1[2] - t1[0]);
408 let (ax, ay, az) = (n.x.abs(), n.y.abs(), n.z.abs());
409 let axis = if az >= ax && az >= ay { 2 } else if ay >= ax { 1 } else { 0 };
410 if coplanar_separated_2d(t1, t2, axis) {
411 stats::COPLANAR_SAT.fetch_add(1, std::sync::atomic::Ordering::Relaxed);
412 return TriTriIsect::None;
413 }
414 }
415 let t_clip = crate::timing::Stopwatch::start();
416 let out = coplanar_clip(t1, t2);
417 stats::COPLANAR_CLIP_NS.fetch_add(t_clip.elapsed_ns(), std::sync::atomic::Ordering::Relaxed);
418 out
419}
420
421#[derive(Clone)]
428struct ClipPt {
429 r: R2,
430 a: [f64; 2],
431 h: std::cell::OnceCell<Homog2>,
435}
436
437impl ClipPt {
438 fn new(r: R2) -> Self {
439 let a = [rat_to_f64(&r.x), rat_to_f64(&r.y)];
440 ClipPt {
441 r,
442 a,
443 h: std::cell::OnceCell::new(),
444 }
445 }
446
447 fn h(&self) -> &Homog2 {
448 self.h.get_or_init(|| homog2_of(&self.r))
449 }
450}
451
452#[inline]
456fn o2p(a: &ClipPt, b: &ClipPt, c: &ClipPt) -> Sign {
457 orient2d_a(a.a, b.a, c.a).unwrap_or_else(|| orient2d_h(a.h(), b.h(), c.h()))
458}
459
460#[inline]
464fn clip_pt_eq(a: &ClipPt, b: &ClipPt) -> bool {
465 r2_eq(&a.r, &b.r)
466}
467
468fn coplanar_clip(t1: [Vec3; 3], t2: [Vec3; 3]) -> TriTriIsect {
471 let r1: [R3; 3] = [
472 R3::from_vec3(t1[0]),
473 R3::from_vec3(t1[1]),
474 R3::from_vec3(t1[2]),
475 ];
476 let r2: [R3; 3] = [
477 R3::from_vec3(t2[0]),
478 R3::from_vec3(t2[1]),
479 R3::from_vec3(t2[2]),
480 ];
481 let n1 = tri_normal_r(&r1[0], &r1[1], &r1[2]);
482 debug_assert!(!n1.is_zero(), "degenerate input triangle");
483
484 let axis = dominant_axis(&n1);
485 let mut clip: Vec<ClipPt> = r1
486 .iter()
487 .map(|p| ClipPt::new(p.project_drop(axis)))
488 .collect();
489 if o2p(&clip[0], &clip[1], &clip[2]) == Sign::Neg {
491 clip.swap(1, 2);
492 }
493 let mut subject: Vec<ClipPt> = r2
494 .iter()
495 .map(|p| ClipPt::new(p.project_drop(axis)))
496 .collect();
497 if o2p(&subject[0], &subject[1], &subject[2]) == Sign::Neg {
498 subject.swap(1, 2);
499 }
500
501 let mut poly = subject;
503 for i in 0..3 {
504 if poly.is_empty() {
505 break;
506 }
507 let (c0, c1) = (&clip[i], &clip[(i + 1) % 3]);
508 let mut out: Vec<ClipPt> = Vec::with_capacity(poly.len() + 2);
509 let sides: Vec<bool> = poly.iter().map(|p| o2p(c0, c1, p) != Sign::Neg).collect();
512 for k in 0..poly.len() {
513 let kn = (k + 1) % poly.len();
514 let (s, e) = (&poly[k], &poly[kn]);
515 match (sides[k], sides[kn]) {
516 (true, true) => out.push(e.clone()),
520 (true, false) => {
521 let x = line_line_intersect_2d(&c0.r, &c1.r, &s.r, &e.r)
522 .expect("strictly crossing edge is not parallel to clip line");
523 out.push(ClipPt::new(x));
524 }
525 (false, true) => {
526 let x = line_line_intersect_2d(&c0.r, &c1.r, &s.r, &e.r)
527 .expect("strictly crossing edge is not parallel to clip line");
528 out.push(ClipPt::new(x));
529 out.push(e.clone());
530 }
531 (false, false) => {}
532 }
533 }
534 poly = out;
535 }
536
537 let poly = canonical_polygon(poly);
540 match poly.len() {
541 0 => TriTriIsect::None,
542 1 => TriTriIsect::Point(lift_to_plane(&poly[0].r, axis, &r1[0], &n1)),
543 2 => TriTriIsect::Segment(
544 lift_to_plane(&poly[0].r, axis, &r1[0], &n1),
545 lift_to_plane(&poly[1].r, axis, &r1[0], &n1),
546 ),
547 _ => {
552 let n2 = tri_normal_r(&r2[0], &r2[1], &r2[2]);
553 debug_assert!(!n2.is_zero(), "degenerate input triangle");
554 let same_orientation = match Sign::of_rat(&n1.dot(&n2)) {
555 Sign::Pos => true,
556 Sign::Neg => false,
557 Sign::Zero => unreachable!("coplanar triangles have parallel normals"),
558 };
559 TriTriIsect::Coplanar {
560 polygon: poly
561 .iter()
562 .map(|p| lift_to_plane(&p.r, axis, &r1[0], &n1))
563 .collect(),
564 same_orientation,
565 }
566 }
567 }
568}
569
570fn canonical_polygon(poly: Vec<ClipPt>) -> Vec<ClipPt> {
574 let mut pts: Vec<ClipPt> = Vec::with_capacity(poly.len());
576 for p in poly {
577 if pts.last().map_or(true, |last| !clip_pt_eq(last, &p)) {
578 pts.push(p);
579 }
580 }
581 while pts.len() > 1 && clip_pt_eq(&pts[0], &pts[pts.len() - 1]) {
582 pts.pop();
583 }
584 if pts.len() <= 2 {
585 return pts;
586 }
587 let all_collinear = (0..pts.len()).all(|i| {
590 let a = &pts[i];
591 let b = &pts[(i + 1) % pts.len()];
592 let c = &pts[(i + 2) % pts.len()];
593 o2p(a, b, c) == Sign::Zero
594 });
595 if all_collinear {
596 let dir = pts
599 .iter()
600 .skip(1)
601 .map(|p| p.r.sub(&pts[0].r))
602 .find(|d| !d.is_zero())
603 .expect("at least two distinct points");
604 let param = |p: &R2| p.sub(&pts[0].r).dot(&dir);
605 let (mut lo, mut hi) = (0usize, 0usize);
606 let (mut lo_t, mut hi_t) = (rat_zero(), rat_zero());
607 for (i, p) in pts.iter().enumerate() {
608 let t = param(&p.r);
609 if t < lo_t {
610 lo_t = t.clone();
611 lo = i;
612 }
613 if t > hi_t {
614 hi_t = t;
615 hi = i;
616 }
617 }
618 if clip_pt_eq(&pts[lo], &pts[hi]) {
619 return vec![pts[lo].clone()];
620 }
621 return vec![pts[lo].clone(), pts[hi].clone()];
622 }
623 let n = pts.len();
625 let keep: Vec<ClipPt> = (0..n)
626 .filter(|&i| {
627 let prev = &pts[(i + n - 1) % n];
628 let next = &pts[(i + 1) % n];
629 o2p(prev, &pts[i], next) != Sign::Zero
630 })
631 .map(|i| pts[i].clone())
632 .collect();
633 keep
634}
635
636#[cfg(test)]
637#[path = "tri_tri_tests.rs"]
638mod tests;