use super::Tri;
use std::collections::HashMap;
pub fn tri_bbs(tris: &[Tri]) -> Vec<([f64; 3], [f64; 3])> {
tris.iter().map(tri_bb).collect()
}
fn tri_bb(t: &Tri) -> ([f64; 3], [f64; 3]) {
let mut lo = t[0];
let mut hi = t[0];
for v in &t[1..] {
for k in 0..3 {
lo[k] = lo[k].min(v[k]);
hi[k] = hi[k].max(v[k]);
}
}
(lo, hi)
}
fn bb_dist_sq(a: &([f64; 3], [f64; 3]), b: &([f64; 3], [f64; 3])) -> f64 {
let mut d = 0.0;
for k in 0..3 {
let gap = (a.0[k] - b.1[k]).max(b.0[k] - a.1[k]).max(0.0);
d += gap * gap;
}
d
}
pub fn shells(tris: &[Tri], weld: f64) -> Vec<Vec<Tri>> {
if tris.is_empty() {
return Vec::new();
}
let (lo, hi) = super::aabb(tris);
let diag = ((hi[0] - lo[0]).powi(2) + (hi[1] - lo[1]).powi(2) + (hi[2] - lo[2]).powi(2)).sqrt();
let tol = if weld > 0.0 {
weld
} else if diag > 0.0 {
diag * 1e-6
} else {
1e-9
};
let cell = |p: &[f64; 3]| -> [i64; 3] {
[
(p[0] / tol).floor() as i64,
(p[1] / tol).floor() as i64,
(p[2] / tol).floor() as i64,
]
};
let mut lookup: HashMap<[i64; 3], Vec<u32>> = HashMap::new();
let mut points: Vec<[f64; 3]> = Vec::new();
let tol_sq = tol * tol;
let mut ids: Vec<[u32; 3]> = Vec::with_capacity(tris.len());
for t in tris {
let mut tri_ids = [0u32; 3];
for (k, v) in t.iter().enumerate() {
let c = cell(v);
let mut found = None;
'search: for dx in -1..=1 {
for dy in -1..=1 {
for dz in -1..=1 {
let key = [c[0] + dx, c[1] + dy, c[2] + dz];
let Some(bucket) = lookup.get(&key) else {
continue;
};
for &id in bucket {
let q = &points[id as usize];
let d = (q[0] - v[0]).powi(2)
+ (q[1] - v[1]).powi(2)
+ (q[2] - v[2]).powi(2);
if d <= tol_sq {
found = Some(id);
break 'search;
}
}
}
}
}
tri_ids[k] = match found {
Some(id) => id,
None => {
let id = points.len() as u32;
points.push(*v);
lookup.entry(c).or_default().push(id);
id
}
};
}
ids.push(tri_ids);
}
let mut parent: Vec<u32> = (0..points.len() as u32).collect();
fn find(parent: &mut [u32], mut x: u32) -> u32 {
while parent[x as usize] != x {
parent[x as usize] = parent[parent[x as usize] as usize];
x = parent[x as usize];
}
x
}
for t in &ids {
let a = find(&mut parent, t[0]);
for &v in &t[1..] {
let b = find(&mut parent, v);
if a != b {
parent[b as usize] = a;
}
}
}
let mut order: HashMap<u32, usize> = HashMap::new();
let mut out: Vec<Vec<Tri>> = Vec::new();
for (i, t) in tris.iter().enumerate() {
let root = find(&mut parent, ids[i][0]);
let slot = *order.entry(root).or_insert_with(|| {
out.push(Vec::new());
out.len() - 1
});
out[slot].push(*t);
}
out
}
pub fn inside(tris: &[Tri], p: [f64; 3]) -> bool {
winding(tris, p).abs() > 0.5
}
pub fn winding(tris: &[Tri], p: [f64; 3]) -> f64 {
let mut total = 0.0;
for t in tris {
let a = sub(t[0], p);
let b = sub(t[1], p);
let c = sub(t[2], p);
let (la, lb, lc) = (len(a), len(b), len(c));
let numer = dot(a, cross(b, c));
let denom = la * lb * lc + dot(a, b) * lc + dot(a, c) * lb + dot(b, c) * la;
total += 2.0 * numer.atan2(denom);
}
total / (4.0 * std::f64::consts::PI)
}
const PROBES: usize = 256;
fn shares_volume(body: &[Tri], probe: &[Tri], probes: usize) -> bool {
if body.is_empty() || probe.is_empty() {
return false;
}
let reach = super::aabb(body);
let candidates: Vec<&Tri> = probe
.iter()
.filter(|t| bb_dist_sq(&tri_bb(t), &reach) <= 0.0)
.collect();
if candidates.is_empty() {
return false;
}
let step = candidates.len().div_ceil(probes.max(1)).max(1);
for t in candidates.into_iter().step_by(step).take(probes.max(1)) {
let Some(p) = just_inside(t) else { continue };
if winding(body, p).abs() > 0.5 {
return true;
}
}
false
}
fn just_inside(t: &Tri) -> Option<[f64; 3]> {
let n = cross(sub(t[1], t[0]), sub(t[2], t[0]));
let l = len(n);
if l <= 0.0 {
return None; }
let inward = scale(n, -(l.sqrt() * 1e-6) / l);
Some([
(t[0][0] + t[1][0] + t[2][0]) / 3.0 + inward[0],
(t[0][1] + t[1][1] + t[2][1]) / 3.0 + inward[1],
(t[0][2] + t[1][2] + t[2][2]) / 3.0 + inward[2],
])
}
pub fn nearest(a: &[Tri], b: &[Tri]) -> f64 {
if a.is_empty() || b.is_empty() {
return f64::INFINITY;
}
let abb = tri_bbs(a);
let bbb = tri_bbs(b);
let whole_b = super::aabb(b);
let mut order: Vec<(f64, usize)> = abb
.iter()
.map(|bb| bb_dist_sq(bb, &whole_b))
.zip(0..a.len())
.collect();
order.sort_unstable_by(|x, y| x.0.partial_cmp(&y.0).unwrap_or(std::cmp::Ordering::Equal));
let mut best = f64::INFINITY;
for &(reach, i) in &order {
if reach >= best * best {
break; }
for (j, bj) in bbb.iter().enumerate() {
if bb_dist_sq(&abb[i], bj) >= best * best {
continue;
}
let d = tri_tri_distance(&a[i], &b[j]);
if d < best {
best = d;
if best == 0.0 {
return 0.0;
}
}
}
}
best
}
pub fn interfere(a: &[Tri], b: &[Tri]) -> bool {
if a.is_empty() || b.is_empty() {
return false;
}
let bbb = tri_bbs(b);
let whole_b = super::aabb(b);
let mut touched = false;
for ai in a {
let abi = tri_bb(ai);
if bb_dist_sq(&abi, &whole_b) > 0.0 {
continue;
}
for (j, bj) in bbb.iter().enumerate() {
if bb_dist_sq(&abi, bj) > 0.0 {
continue;
}
if tri_tri_intersect(ai, &b[j]) {
return true;
}
if !touched && tri_tri_distance(ai, &b[j]) == 0.0 {
touched = true;
}
}
}
let probes = if touched { PROBES } else { 8 };
shares_volume(b, a, probes) || shares_volume(a, b, probes)
}
pub fn tri_tri_intersect(a: &Tri, b: &Tri) -> bool {
for k in 0..3 {
if seg_tri(a[k], a[(k + 1) % 3], b) || seg_tri(b[k], b[(k + 1) % 3], a) {
return true;
}
}
false
}
fn seg_tri(p0: [f64; 3], p1: [f64; 3], t: &Tri) -> bool {
const EDGE: f64 = 1e-9;
let dir = sub(p1, p0);
let e1 = sub(t[1], t[0]);
let e2 = sub(t[2], t[0]);
let h = cross(dir, e2);
let det = dot(e1, h);
if det.abs() <= 1e-12 * len(dir) * len(e1) * len(e2) {
return false; }
let inv = 1.0 / det;
let s = sub(p0, t[0]);
let u = dot(s, h) * inv;
if u <= EDGE || u >= 1.0 - EDGE {
return false;
}
let q = cross(s, e1);
let v = dot(dir, q) * inv;
if v <= EDGE || u + v >= 1.0 - EDGE {
return false;
}
let time = dot(e2, q) * inv;
time > EDGE && time < 1.0 - EDGE
}
pub fn tri_tri_distance(a: &Tri, b: &Tri) -> f64 {
if tri_tri_intersect(a, b) {
return 0.0;
}
let mut best = f64::INFINITY;
for v in a {
best = best.min(point_tri_distance(*v, b));
}
for v in b {
best = best.min(point_tri_distance(*v, a));
}
for i in 0..3 {
for j in 0..3 {
best = best.min(seg_seg_distance(a[i], a[(i + 1) % 3], b[j], b[(j + 1) % 3]));
}
}
best
}
pub fn point_tri_distance(p: [f64; 3], t: &Tri) -> f64 {
len(sub(p, closest_on_tri(p, t)))
}
fn closest_on_tri(p: [f64; 3], t: &Tri) -> [f64; 3] {
let (a, b, c) = (t[0], t[1], t[2]);
let ab = sub(b, a);
let ac = sub(c, a);
let ap = sub(p, a);
let d1 = dot(ab, ap);
let d2 = dot(ac, ap);
if d1 <= 0.0 && d2 <= 0.0 {
return a;
}
let bp = sub(p, b);
let d3 = dot(ab, bp);
let d4 = dot(ac, bp);
if d3 >= 0.0 && d4 <= d3 {
return b;
}
let vc = d1 * d4 - d3 * d2;
if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 {
let v = if d1 - d3 != 0.0 { d1 / (d1 - d3) } else { 0.0 };
return add(a, scale(ab, v));
}
let cp = sub(p, c);
let d5 = dot(ab, cp);
let d6 = dot(ac, cp);
if d6 >= 0.0 && d5 <= d6 {
return c;
}
let vb = d5 * d2 - d1 * d6;
if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 {
let w = if d2 - d6 != 0.0 { d2 / (d2 - d6) } else { 0.0 };
return add(a, scale(ac, w));
}
let va = d3 * d6 - d5 * d4;
if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 {
let denom = (d4 - d3) + (d5 - d6);
let w = if denom != 0.0 { (d4 - d3) / denom } else { 0.0 };
return add(b, scale(sub(c, b), w));
}
let denom = va + vb + vc;
if denom == 0.0 {
return a; }
let v = vb / denom;
let w = vc / denom;
add(add(a, scale(ab, v)), scale(ac, w))
}
pub fn seg_seg_distance(p1: [f64; 3], q1: [f64; 3], p2: [f64; 3], q2: [f64; 3]) -> f64 {
const EPS: f64 = 1e-15;
let d1 = sub(q1, p1);
let d2 = sub(q2, p2);
let r = sub(p1, p2);
let a = dot(d1, d1);
let e = dot(d2, d2);
let f = dot(d2, r);
let (mut s, mut t);
if a <= EPS && e <= EPS {
return len(r); }
if a <= EPS {
s = 0.0;
t = (f / e).clamp(0.0, 1.0);
} else {
let c = dot(d1, r);
if e <= EPS {
t = 0.0;
s = (-c / a).clamp(0.0, 1.0);
} else {
let b = dot(d1, d2);
let denom = a * e - b * b;
s = if denom > EPS {
((b * f - c * e) / denom).clamp(0.0, 1.0)
} else {
0.0 };
t = (b * s + f) / e;
if t < 0.0 {
t = 0.0;
s = (-c / a).clamp(0.0, 1.0);
} else if t > 1.0 {
t = 1.0;
s = ((b - c) / a).clamp(0.0, 1.0);
}
}
}
len(sub(add(p1, scale(d1, s)), add(p2, scale(d2, t))))
}
#[inline]
fn sub(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[a[0] - b[0], a[1] - b[1], a[2] - b[2]]
}
#[inline]
fn add(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[a[0] + b[0], a[1] + b[1], a[2] + b[2]]
}
#[inline]
fn scale(a: [f64; 3], s: f64) -> [f64; 3] {
[a[0] * s, a[1] * s, a[2] * s]
}
#[inline]
fn dot(a: [f64; 3], b: [f64; 3]) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
#[inline]
fn cross(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
[
a[1] * b[2] - a[2] * b[1],
a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0],
]
}
#[inline]
fn len(a: [f64; 3]) -> f64 {
dot(a, a).sqrt()
}