use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet};
use num_rational::BigRational;
use num_traits::{One, Zero};
use crate::linalg::Vec3;
use crate::types::Box;
use super::arrangement::{self, ArrangementInput};
use super::exact::rational::{r3_eq, R3, R3Key};
use super::exact::Sign;
use super::tri_tri::{tri_tri_intersect, TriTriIsect};
pub type EdgeKey = (u32, u32);
pub fn edge_key(a: u32, b: u32) -> EdgeKey {
if a <= b {
(a, b)
} else {
(b, a)
}
}
type GeoEdgeKey = (R3, R3);
fn geo_edge_key(a: &R3, b: &R3) -> GeoEdgeKey {
if a <= b {
(a.clone(), b.clone())
} else {
(b.clone(), a.clone())
}
}
type BitEdgeKey = ([u64; 3], [u64; 3]);
fn bit_edge_key(a: Vec3, b: Vec3) -> BitEdgeKey {
let (ka, kb) = (f64_key(a), f64_key(b));
if ka <= kb {
(ka, kb)
} else {
(kb, ka)
}
}
#[derive(Clone, Copy, Debug)]
pub struct Piece {
pub mesh: u8,
pub tri: usize,
pub vi: [u32; 3],
}
pub struct IntersectionGraph {
pub pieces: Vec<Piece>,
pub verts: Vec<R3>,
pub verts_f64: Vec<Vec3>,
pub isect_edges: HashSet<EdgeKey>,
pub any_intersections: bool,
}
impl IntersectionGraph {
pub fn piece_verts(&self, pi: usize) -> [&R3; 3] {
let vi = self.pieces[pi].vi;
[
&self.verts[vi[0] as usize],
&self.verts[vi[1] as usize],
&self.verts[vi[2] as usize],
]
}
}
#[derive(Default)]
pub struct VertInterner {
map: HashMap<R3Key, u32>,
fmap: HashMap<[u64; 3], u32>,
pub verts: Vec<R3>,
pub verts_f64: Vec<Vec3>,
}
fn f64_key(v: Vec3) -> [u64; 3] {
let norm = |x: f64| if x == 0.0 { 0.0f64 } else { x }.to_bits();
[norm(v.x), norm(v.y), norm(v.z)]
}
impl VertInterner {
pub fn intern_f64(&mut self, v: Vec3) -> u32 {
let key = f64_key(v);
if let Some(&id) = self.fmap.get(&key) {
return id;
}
let id = self.verts.len() as u32;
self.fmap.insert(key, id);
self.verts.push(R3::from_vec3(v));
self.verts_f64.push(v);
id
}
pub fn intern(&mut self, p: &R3) -> u32 {
let rounded = p.to_vec3_rounded();
if r3_eq(&R3::from_vec3(rounded), p) {
return self.intern_f64(rounded);
}
let next = self.verts.len() as u32;
match self.map.entry(R3Key(p.clone())) {
std::collections::hash_map::Entry::Occupied(e) => *e.get(),
std::collections::hash_map::Entry::Vacant(e) => {
e.insert(next);
self.verts.push(p.clone());
self.verts_f64.push(rounded);
next
}
}
}
}
#[derive(Clone, Debug, Default)]
struct TriPrims {
points: Vec<(R3, usize)>,
segments: Vec<(R3, R3, usize)>,
}
fn tri_box(t: &[Vec3; 3]) -> Box {
let mut b = Box::from_points(t[0], t[1]);
b.union_point(t[2]);
b
}
fn is_degenerate(t: &[Vec3; 3]) -> bool {
const EPS: f64 = f64::EPSILON * 0.5;
let u = t[1] - t[0];
let v = t[2] - t[0];
let n = crate::linalg::cross(u, v);
let m = |k: usize| t[0][k].abs() + t[1][k].abs() + t[2][k].abs();
let (mx, my, mz) = (m(0), m(1), m(2));
if n.x.abs() > 16.0 * EPS * my * mz
|| n.y.abs() > 16.0 * EPS * mz * mx
|| n.z.abs() > 16.0 * EPS * mx * my
{
return false;
}
use super::exact::predicates::tri_normal_r;
tri_normal_r(
&R3::from_vec3(t[0]),
&R3::from_vec3(t[1]),
&R3::from_vec3(t[2]),
)
.is_zero()
}
fn point_on_segment(p: &R3, a: &R3, b: &R3) -> bool {
super::exact::predicates::point_on_segment_r(p, a, b)
}
fn approx3(p: &R3) -> [f64; 3] {
use super::exact::rational::rat_to_f64;
[rat_to_f64(&p.x), rat_to_f64(&p.y), rat_to_f64(&p.z)]
}
fn point_on_segment_f(p_approx: [f64; 3], p: &R3, a_approx: [f64; 3], a: &R3, b_approx: [f64; 3], b: &R3) -> bool {
match super::exact::approx::not_on_segment_a(p_approx, a_approx, b_approx) {
Some(false) => false,
_ => point_on_segment(p, a, b),
}
}
fn clip_segment_to_polygon(a: &R3, b: &R3, poly: &[R3]) -> Option<(R3, R3)> {
use super::exact::predicates::{orient2d_r, tri_normal_r};
use super::exact::rational::R2;
use super::exact::Sign;
use super::tri_tri::dominant_axis;
debug_assert!(poly.len() >= 3);
let n = tri_normal_r(&poly[0], &poly[1], &poly[2]);
let axis = dominant_axis(&n);
let mut pts2: Vec<R2> = poly.iter().map(|p| p.project_drop(axis)).collect();
if orient2d_r(&pts2[0], &pts2[1], &pts2[2]) == Sign::Neg {
pts2.reverse();
}
let a2 = a.project_drop(axis);
let b2 = b.project_drop(axis);
let dir = b2.sub(&a2);
let mut t0 = BigRational::zero();
let mut t1 = BigRational::one();
for i in 0..pts2.len() {
let e0 = &pts2[i];
let e1 = &pts2[(i + 1) % pts2.len()];
let edge = e1.sub(e0);
let fa = edge.cross(&a2.sub(e0));
let fd = edge.cross(&dir);
if fd.is_zero() {
if fa < BigRational::zero() {
return None; }
continue;
}
let t_hit = -&fa / &fd;
if fd > BigRational::zero() {
if t_hit > t0 {
t0 = t_hit;
}
} else if t_hit < t1 {
t1 = t_hit;
}
if t0 >= t1 {
return None;
}
}
if t0 >= t1 {
return None;
}
let seg = |t: &BigRational| a.add(&b.sub(a).scale(t));
Some((seg(&t0), seg(&t1)))
}
pub fn build_graph(p: &[[Vec3; 3]], q: &[[Vec3; 3]]) -> IntersectionGraph {
let t_all = crate::timing::start();
let meshes: [&[[Vec3; 3]]; 2] = [p, q];
let live: [Vec<bool>; 2] = [
p.iter().map(|t| !is_degenerate(t)).collect(),
q.iter().map(|t| !is_degenerate(t)).collect(),
];
let p_boxes: Vec<Box> = p.iter().map(tri_box).collect();
let q_boxes: Vec<Box> = q.iter().map(tri_box).collect();
let scene_box = q_boxes
.iter()
.enumerate()
.filter(|(qi, _)| live[1][*qi])
.fold(Box::new(), |acc, (_, b)| acc.union_box(b));
let mut q_order: Vec<usize> = (0..q.len()).filter(|&qi| live[1][qi]).collect();
q_order.sort_by_key(|&qi| crate::sort::morton_code(q_boxes[qi].center(), &scene_box));
let leaf_boxes: Vec<Box> = q_order.iter().map(|&qi| q_boxes[qi]).collect();
let leaf_morton: Vec<u32> = q_order
.iter()
.map(|&qi| crate::sort::morton_code(q_boxes[qi].center(), &scene_box))
.collect();
let collider = crate::collider::Collider::new(leaf_boxes, leaf_morton);
let mut prims: [Vec<TriPrims>; 2] = [
vec![TriPrims::default(); p.len()],
vec![TriPrims::default(); q.len()],
];
let mut coplanar_regions: Vec<(usize, usize, Vec<R3>)> = Vec::new();
let mut any_intersections = false;
let mut pair_count = 0usize;
let mut candidates_q: Vec<usize> = Vec::new();
for (pi, pt) in p.iter().enumerate() {
if !live[0][pi] {
continue;
}
candidates_q.clear();
collider.collisions_one(&p_boxes[pi], pi, |_, leaf| {
candidates_q.push(q_order[leaf]);
});
candidates_q.sort_unstable();
for &qi in &candidates_q {
let qt = &q[qi];
if !p_boxes[pi].does_overlap_box(&q_boxes[qi]) {
continue;
}
let isect = tri_tri_intersect(*pt, *qt);
let pair = pair_count;
match isect {
TriTriIsect::None => continue,
TriTriIsect::Point(x) => {
prims[0][pi].points.push((x.clone(), pair));
prims[1][qi].points.push((x, pair));
}
TriTriIsect::Segment(x, y) => {
prims[0][pi].segments.push((x.clone(), y.clone(), pair));
prims[1][qi].segments.push((x, y, pair));
}
TriTriIsect::Coplanar { polygon, .. } => {
for i in 0..polygon.len() {
let a = polygon[i].clone();
let b = polygon[(i + 1) % polygon.len()].clone();
prims[0][pi].segments.push((a.clone(), b.clone(), pair));
prims[1][qi].segments.push((a, b, pair));
}
coplanar_regions.push((pi, qi, polygon));
}
}
any_intersections = true;
pair_count += 1;
}
}
crate::timing::print("robust: pair narrow phase", t_all);
let t_self = crate::timing::start();
for m in 0..2 {
let (tris, boxes) = if m == 0 {
(p, &p_boxes)
} else {
(q, &q_boxes)
};
let self_scene = boxes
.iter()
.enumerate()
.filter(|(i, _)| live[m][*i])
.fold(Box::new(), |acc, (_, b)| acc.union_box(b));
let mut order: Vec<usize> = (0..tris.len()).filter(|&i| live[m][i]).collect();
order.sort_by_key(|&i| crate::sort::morton_code(boxes[i].center(), &self_scene));
let self_collider = crate::collider::Collider::new(
order.iter().map(|&i| boxes[i]).collect(),
order
.iter()
.map(|&i| crate::sort::morton_code(boxes[i].center(), &self_scene))
.collect(),
);
let mut cands: Vec<usize> = Vec::new();
let mut n_pairs = 0usize;
let mut n_cut = 0usize;
let mut stats = SelfCutStats::default();
for i in 0..tris.len() {
if !live[m][i] {
continue;
}
cands.clear();
self_collider.collisions_one(&boxes[i], i, |_, leaf| {
cands.push(order[leaf]);
});
cands.sort_unstable();
for &j in &cands {
if j <= i || !boxes[i].does_overlap_box(&boxes[j]) {
continue;
}
n_pairs += 1;
let Some(segs) = real_self_contact(tris[i], tris[j], &mut stats) else {
continue;
};
n_cut += 1;
for (x, y) in segs {
let pair = pair_count;
prims[m][i].segments.push((x.clone(), y.clone(), pair));
prims[m][j].segments.push((x, y, pair));
pair_count += 1;
}
}
}
crate::timing::print_count(
&format!("robust: self-cut mesh {m}: {n_pairs} box pairs, {n_cut} cutting"),
);
crate::timing::print_count(&format!(
"robust: self-cut mesh {m} tri_tri exits: {}",
super::tri_tri::stats::snapshot_and_reset()
));
crate::timing::print_count(&format!(
"robust: self-cut mesh {m} paths: identical {}, edge-benign {}, vert-benign {}, \
full {} ({:.3}s: none {}, point {}, seg-benign {})",
stats.identical,
stats.edge_benign,
stats.vert_benign,
stats.full,
stats.full_secs,
stats.full_none,
stats.full_point,
stats.full_seg_benign,
));
}
crate::timing::print("robust: self-intersection cuts", t_self);
let t_cross = crate::timing::start();
for (pi, qi, poly) in &coplanar_regions {
let from_p: TriPrims = prims[0][*pi].clone();
let from_q: TriPrims = prims[1][*qi].clone();
let copy = |src: &TriPrims, dst: &mut TriPrims| {
for (a, b, prov) in &src.segments {
if let Some((ca, cb)) = clip_segment_to_polygon(a, b, poly) {
if !dst
.segments
.iter()
.any(|(x, y, pv)| pv == prov && ((x, y) == (&ca, &cb) || (x, y) == (&cb, &ca)))
{
dst.segments.push((ca, cb, *prov));
}
}
}
for (pt, prov) in &src.points {
if clip_segment_to_polygon(pt, pt, poly).is_some()
|| point_in_polygon_coplanar(pt, poly)
{
if !dst.points.iter().any(|(x, pv)| pv == prov && x == pt) {
dst.points.push((pt.clone(), *prov));
}
}
}
};
copy(&from_p, &mut prims[1][*qi]);
copy(&from_q, &mut prims[0][*pi]);
}
crate::timing::print("robust: coplanar cross-copy", t_cross);
let t_cand = crate::timing::start();
let mut candidates: [Vec<Option<Vec<R3>>>; 2] = [
vec![None; p.len()],
vec![None; q.len()],
];
for m in 0..2 {
for ti in 0..meshes[m].len() {
let pr = &prims[m][ti];
if pr.points.is_empty() && pr.segments.is_empty() {
continue;
}
let input = ArrangementInput {
points: pr.points.clone(),
segments: pr.segments.clone(),
};
candidates[m][ti] = Some(arrangement::candidate_points(meshes[m][ti], &input));
}
}
crate::timing::print("robust: candidate points", t_cand);
let t_reg = crate::timing::start();
let mut edge_registry: [HashMap<BitEdgeKey, BTreeSet<R3>>; 2] =
[HashMap::new(), HashMap::new()];
for m in 0..2 {
for ti in 0..meshes[m].len() {
let Some(cands) = &candidates[m][ti] else { continue };
let t = meshes[m][ti];
let corners = [
R3::from_vec3(t[0]),
R3::from_vec3(t[1]),
R3::from_vec3(t[2]),
];
let ca: [[f64; 3]; 3] = [
[t[0].x, t[0].y, t[0].z],
[t[1].x, t[1].y, t[1].z],
[t[2].x, t[2].y, t[2].z],
];
let cands_a: Vec<[f64; 3]> = cands.iter().map(approx3).collect();
for e in 0..3 {
let a = &corners[e];
let b = &corners[(e + 1) % 3];
let key = bit_edge_key(t[e], t[(e + 1) % 3]);
for (pt, pt_a) in cands.iter().zip(&cands_a) {
if !r3_eq(pt, a)
&& !r3_eq(pt, b)
&& point_on_segment_f(*pt_a, pt, ca[e], a, ca[(e + 1) % 3], b)
{
edge_registry[m].entry(key).or_default().insert(pt.clone());
}
}
}
}
}
let mut seg_splits: BTreeMap<GeoEdgeKey, BTreeSet<R3>> = BTreeMap::new();
for m in 0..2 {
for ti in 0..meshes[m].len() {
let Some(cands) = &candidates[m][ti] else { continue };
let cands_a: Vec<[f64; 3]> = cands.iter().map(approx3).collect();
for (a, b, _prov) in &prims[m][ti].segments {
let key = geo_edge_key(a, b);
let (aa, ba) = (approx3(a), approx3(b));
for (pt, pt_a) in cands.iter().zip(&cands_a) {
if !r3_eq(pt, a) && !r3_eq(pt, b) && point_on_segment_f(*pt_a, pt, aa, a, ba, b) {
seg_splits.entry(key.clone()).or_default().insert(pt.clone());
}
}
}
}
}
crate::timing::print("robust: split registries", t_reg);
let t_arr = crate::timing::start();
let mut pieces: Vec<Piece> = Vec::new();
let mut isect_edges: HashSet<EdgeKey> = HashSet::new();
let mut interner = VertInterner::default();
for m in 0..2 {
for ti in 0..meshes[m].len() {
if !live[m][ti] {
continue;
}
let t = meshes[m][ti];
let pr = &prims[m][ti];
let mut extra: BTreeSet<R3> = BTreeSet::new();
for e in 0..3 {
if let Some(set) = edge_registry[m].get(&bit_edge_key(t[e], t[(e + 1) % 3])) {
extra.extend(set.iter().cloned());
}
}
for (a, b, _) in &pr.segments {
if let Some(set) = seg_splits.get(&geo_edge_key(a, b)) {
extra.extend(set.iter().cloned());
}
}
if pr.points.is_empty() && pr.segments.is_empty() && extra.is_empty() {
pieces.push(Piece {
mesh: m as u8,
tri: ti,
vi: [
interner.intern_f64(t[0]),
interner.intern_f64(t[1]),
interner.intern_f64(t[2]),
],
});
continue;
}
let mut input = ArrangementInput {
points: pr.points.clone(),
segments: pr.segments.clone(),
};
for pt in extra {
input.points.push((pt, usize::MAX));
}
let arr = arrangement::build(t, &input);
let ids: Vec<u32> = arr.points3.iter().map(|p| interner.intern(p)).collect();
for (u, w) in arr.constraints.keys() {
isect_edges.insert(edge_key(ids[*u], ids[*w]));
}
for st in &arr.tris {
let (a, b, c) = (st[0], st[1], st[2]);
let vi = if arr.flipped {
[ids[a], ids[c], ids[b]]
} else {
[ids[a], ids[b], ids[c]]
};
pieces.push(Piece {
mesh: m as u8,
tri: ti,
vi,
});
}
}
}
crate::timing::print("robust: arrangements", t_arr);
crate::timing::print_count(&format!(
"robust: arrangement phases: {}",
arrangement::stats::snapshot_and_reset()
));
IntersectionGraph {
pieces,
verts: interner.verts,
verts_f64: interner.verts_f64,
isect_edges,
any_intersections,
}
}
fn orient3d_plane(t: &[Vec3; 3], v: Vec3) -> Sign {
if let Some(s) = super::exact::approx::orient3d_a(
[t[0].x, t[0].y, t[0].z],
[t[1].x, t[1].y, t[1].z],
[t[2].x, t[2].y, t[2].z],
[v.x, v.y, v.z],
) {
return s;
}
super::exact::intpred::orient3d_i(
[t[0].x, t[0].y, t[0].z],
[t[1].x, t[1].y, t[1].z],
[t[2].x, t[2].y, t[2].z],
[v.x, v.y, v.z],
)
}
fn dominant_axis_f64(t: [Vec3; 3]) -> usize {
let n = crate::linalg::cross(t[1] - t[0], t[2] - t[0]);
let (ax, ay, az) = (n.x.abs(), n.y.abs(), n.z.abs());
if az >= ax && az >= ay {
2
} else if ay >= ax {
1
} else {
0
}
}
fn project_f64(v: Vec3, axis: usize) -> crate::linalg::Vec2 {
match axis {
0 => crate::linalg::Vec2::new(v.y, v.z),
1 => crate::linalg::Vec2::new(v.z, v.x),
_ => crate::linalg::Vec2::new(v.x, v.y),
}
}
#[derive(Default)]
struct SelfCutStats {
identical: usize,
edge_benign: usize,
vert_benign: usize,
full: usize,
full_none: usize,
full_point: usize,
full_seg_benign: usize,
full_secs: f64,
}
fn real_self_contact(
t1: [Vec3; 3],
t2: [Vec3; 3],
stats: &mut SelfCutStats,
) -> Option<Vec<(R3, R3)>> {
use super::exact::Sign;
let mut shared_f = [Vec3::default(); 3];
let mut n_shared = 0usize;
for &v in &t1 {
if t2.contains(&v) {
shared_f[n_shared] = v;
n_shared += 1;
}
}
let shared_f = &shared_f[..n_shared];
if shared_f.len() == 3 {
stats.identical += 1;
return None;
}
if shared_f.len() == 2 {
if let Some(&opp) = t2.iter().find(|v| !t1.contains(v)) {
if orient3d_plane(&t1, opp) != Sign::Zero {
stats.edge_benign += 1;
return None;
}
if let Some(&own) = t1.iter().find(|v| !t2.contains(v)) {
let axis = dominant_axis_f64(t1);
let p2 = |v: Vec3| project_f64(v, axis);
let s_own =
super::exact::filtered::orient2d(p2(shared_f[0]), p2(shared_f[1]), p2(own));
let s_opp =
super::exact::filtered::orient2d(p2(shared_f[0]), p2(shared_f[1]), p2(opp));
if s_own != Sign::Zero && s_opp != Sign::Zero && s_own != s_opp {
stats.edge_benign += 1;
return None;
}
}
}
} else if shared_f.len() == 1 {
let mut others = [(Vec3::default(), Sign::Zero); 3];
let mut n_others = 0usize;
for &v in &t2 {
if !t1.contains(&v) {
others[n_others] = (v, orient3d_plane(&t1, v));
n_others += 1;
}
}
let others = &others[..n_others];
if others.len() == 2 && others[0].1 != Sign::Zero && others[0].1 == others[1].1 {
stats.vert_benign += 1;
return None;
}
if others.len() == 2 && others[0].1 == Sign::Zero && others[1].1 == Sign::Zero {
let axis = dominant_axis_f64(t1);
let p2 = |v: Vec3| project_f64(v, axis);
let v0 = shared_f[0];
let mut own = [Vec3::default(); 3];
let mut n_own = 0usize;
for &v in &t1 {
if !t2.contains(&v) {
own[n_own] = v;
n_own += 1;
}
}
let own = &own[..n_own];
let other = [others[0].0, others[1].0];
let separated = |ea: Vec3, third: Vec3, far: [Vec3; 2]| -> bool {
let s_t = super::exact::filtered::orient2d(p2(v0), p2(ea), p2(third));
if s_t == Sign::Zero {
return false;
}
far.iter().all(|&f| {
let s = super::exact::filtered::orient2d(p2(v0), p2(ea), p2(f));
s != Sign::Zero && s != s_t
})
};
if own.len() == 2
&& (separated(own[0], own[1], other)
|| separated(own[1], own[0], other)
|| separated(other[0], other[1], [own[0], own[1]])
|| separated(other[1], other[0], [own[0], own[1]]))
{
stats.vert_benign += 1;
return None;
}
}
}
stats.full += 1;
let t_full = crate::timing::Stopwatch::start();
let isect = tri_tri_intersect(t1, t2);
stats.full_secs += t_full.elapsed_secs();
match isect {
TriTriIsect::None => {
stats.full_none += 1;
None
}
TriTriIsect::Point(_) => {
stats.full_point += 1;
None
}
TriTriIsect::Segment(x, y) => {
let benign = shared_f.len() >= 2 && {
let s0 = R3::from_vec3(shared_f[0]);
let s1 = R3::from_vec3(shared_f[1]);
point_on_segment(&x, &s0, &s1) && point_on_segment(&y, &s0, &s1)
};
if benign {
stats.full_seg_benign += 1;
}
(!benign).then(|| vec![(x, y)])
}
TriTriIsect::Coplanar { polygon, .. } => Some(
(0..polygon.len())
.map(|i| {
(
polygon[i].clone(),
polygon[(i + 1) % polygon.len()].clone(),
)
})
.collect(),
),
}
}
fn point_in_polygon_coplanar(p: &R3, poly: &[R3]) -> bool {
use super::exact::predicates::{orient2d_r, tri_normal_r};
use super::exact::Sign;
use super::tri_tri::dominant_axis;
let n = tri_normal_r(&poly[0], &poly[1], &poly[2]);
let axis = dominant_axis(&n);
let mut pts2: Vec<_> = poly.iter().map(|q| q.project_drop(axis)).collect();
if orient2d_r(&pts2[0], &pts2[1], &pts2[2]) == Sign::Neg {
pts2.reverse();
}
let p2 = p.project_drop(axis);
for i in 0..pts2.len() {
if orient2d_r(&pts2[i], &pts2[(i + 1) % pts2.len()], &p2) == Sign::Neg {
return false;
}
}
true
}