mod mesh;
mod motion;
mod pose;
pub use mesh::{
inside, interfere, nearest, point_tri_distance, seg_seg_distance, shells, tri_bbs,
tri_tri_distance, tri_tri_intersect, winding,
};
pub use motion::{classify, recover_screw, ParamMotion, Screw};
pub use pose::{align, engagement, engagement_with, floating, floating_with, Body, Engagement};
pub type Tri = [[f64; 3]; 3];
pub fn from_geometry(geometry: &crate::core::BufferGeometry) -> Vec<Tri> {
let Some(pos) = geometry.get_attribute("position") else {
return Vec::new();
};
let point = |i: usize| -> [f64; 3] {
let b = i * 3;
[
pos.array[b] as f64,
pos.array[b + 1] as f64,
pos.array[b + 2] as f64,
]
};
let count = pos.array.len() / 3;
match &geometry.index {
Some(index) => index
.chunks_exact(3)
.filter(|t| t.iter().all(|&i| (i as usize) < count))
.map(|t| {
[
point(t[0] as usize),
point(t[1] as usize),
point(t[2] as usize),
]
})
.collect(),
None => (0..count / 3)
.map(|t| [point(t * 3), point(t * 3 + 1), point(t * 3 + 2)])
.collect(),
}
}
pub fn from_geometry_at(
geometry: &crate::core::BufferGeometry,
transform: &crate::math::Matrix4,
) -> Vec<Tri> {
from_geometry(geometry)
.into_iter()
.map(|t| {
t.map(|p| {
let v = crate::math::Vector3::new(p[0] as f32, p[1] as f32, p[2] as f32)
.apply_matrix4(transform);
[v.x as f64, v.y as f64, v.z as f64]
})
})
.collect()
}
pub fn aabb(tris: &[Tri]) -> ([f64; 3], [f64; 3]) {
let mut lo = [f64::INFINITY; 3];
let mut hi = [f64::NEG_INFINITY; 3];
for t in tris {
for v in t {
for k in 0..3 {
lo[k] = lo[k].min(v[k]);
hi[k] = hi[k].max(v[k]);
}
}
}
(lo, hi)
}
pub fn centroid(tris: &[Tri]) -> [f64; 3] {
let n = (tris.len() * 3).max(1) as f64;
let mut c = [0.0; 3];
for t in tris {
for v in t {
for k in 0..3 {
c[k] += v[k] / n;
}
}
}
c
}
pub fn centroid_area(tris: &[Tri]) -> [f64; 3] {
let mut total = 0.0;
let mut acc = [0.0; 3];
for t in tris {
let a = tri_area(t);
total += a;
for k in 0..3 {
acc[k] += a * (t[0][k] + t[1][k] + t[2][k]) / 3.0;
}
}
if total <= 0.0 {
return centroid(tris);
}
[acc[0] / total, acc[1] / total, acc[2] / total]
}
pub fn area(tris: &[Tri]) -> f64 {
tris.iter().map(tri_area).sum()
}
pub fn volume(tris: &[Tri]) -> f64 {
let c = centroid_area(tris);
let mut v = 0.0;
for t in tris {
let a = [t[0][0] - c[0], t[0][1] - c[1], t[0][2] - c[2]];
let b = [t[1][0] - c[0], t[1][1] - c[1], t[1][2] - c[2]];
let d = [t[2][0] - c[0], t[2][1] - c[1], t[2][2] - c[2]];
v += (a[0] * (b[1] * d[2] - b[2] * d[1]) - a[1] * (b[0] * d[2] - b[2] * d[0])
+ a[2] * (b[0] * d[1] - b[1] * d[0]))
/ 6.0;
}
v
}
fn tri_area(t: &Tri) -> f64 {
let u = [t[1][0] - t[0][0], t[1][1] - t[0][1], t[1][2] - t[0][2]];
let v = [t[2][0] - t[0][0], t[2][1] - t[0][1], t[2][2] - t[0][2]];
let c = [
u[1] * v[2] - u[2] * v[1],
u[2] * v[0] - u[0] * v[2],
u[0] * v[1] - u[1] * v[0],
];
(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]).sqrt() / 2.0
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RigidKey {
pub area_scale: f64,
pub volume_scale: f64,
pub moments: [f64; 3],
pub handedness: i8,
}
impl RigidKey {
pub const TOLERANCE: f64 = 1e-4;
pub fn matches(&self, other: &Self) -> bool {
self.matches_within(other, Self::TOLERANCE)
}
pub fn matches_within(&self, other: &Self, tol: f64) -> bool {
let scale = self.area_scale.max(other.area_scale);
let close = |a: f64, b: f64| (a - b).abs() <= tol * scale.max(a.abs()).max(b.abs());
let same_hand =
self.handedness == other.handedness || self.handedness == 0 || other.handedness == 0;
same_hand
&& close(self.area_scale, other.area_scale)
&& close(self.volume_scale, other.volume_scale)
&& (0..3).all(|k| close(self.moments[k], other.moments[k]))
}
}
pub fn rigid_key(tris: &[Tri]) -> RigidKey {
let c = centroid_area(tris);
let mut total_area = 0.0;
let mut m = [[0.0f64; 3]; 3];
for t in tris {
let a = tri_area(t);
if a <= 0.0 {
continue;
}
total_area += a;
let v: [[f64; 3]; 3] = [
[t[0][0] - c[0], t[0][1] - c[1], t[0][2] - c[2]],
[t[1][0] - c[0], t[1][1] - c[1], t[1][2] - c[2]],
[t[2][0] - c[0], t[2][1] - c[1], t[2][2] - c[2]],
];
let s = [
v[0][0] + v[1][0] + v[2][0],
v[0][1] + v[1][1] + v[2][1],
v[0][2] + v[1][2] + v[2][2],
];
for i in 0..3 {
for j in 0..3 {
let sum: f64 = (0..3).map(|k| v[k][i] * v[k][j]).sum();
m[i][j] += (a / 12.0) * (sum + s[i] * s[j]);
}
}
}
let trace = m[0][0] + m[1][1] + m[2][2];
let second = (m[0][0] * m[1][1] - m[0][1] * m[1][0])
+ (m[0][0] * m[2][2] - m[0][2] * m[2][0])
+ (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
let det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1])
- m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+ m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
RigidKey {
area_scale: total_area.max(0.0).sqrt(),
volume_scale: cbrt(volume(tris).abs()),
moments: [
root(trace.abs(), 4.0),
root(second.abs(), 8.0),
root(det.abs(), 12.0),
],
handedness: handedness(tris, c, m, total_area),
}
}
fn handedness(tris: &[Tri], c: [f64; 3], m: [[f64; 3]; 3], total_area: f64) -> i8 {
if total_area <= 0.0 {
return 0;
}
let (values, mut axes) = motion::symmetric_eigen(m);
let scale = values[0].abs();
if scale <= 0.0 {
return 0;
}
const DISTINCT: f64 = 1e-6;
if (values[0] - values[1]).abs() <= DISTINCT * scale
|| (values[1] - values[2]).abs() <= DISTINCT * scale
{
return 0;
}
let length = root(scale, 4.0);
let floor = total_area * length * length * length * 1e-6;
for axis in axes.iter_mut() {
let m3 = third_moment(tris, c, *axis);
if m3.abs() <= floor {
return 0; }
if m3 < 0.0 {
*axis = [-axis[0], -axis[1], -axis[2]];
}
}
let det = axes[0][0] * (axes[1][1] * axes[2][2] - axes[1][2] * axes[2][1])
- axes[0][1] * (axes[1][0] * axes[2][2] - axes[1][2] * axes[2][0])
+ axes[0][2] * (axes[1][0] * axes[2][1] - axes[1][1] * axes[2][0]);
if det > 0.0 {
1
} else if det < 0.0 {
-1
} else {
0
}
}
fn third_moment(tris: &[Tri], c: [f64; 3], e: [f64; 3]) -> f64 {
let mut acc = 0.0;
for t in tris {
let a = tri_area(t);
if a <= 0.0 {
continue;
}
let f: [f64; 3] = std::array::from_fn(|i| {
(t[i][0] - c[0]) * e[0] + (t[i][1] - c[1]) * e[1] + (t[i][2] - c[2]) * e[2]
});
let (x, y, z) = (f[0], f[1], f[2]);
let h3 = x * x * x
+ y * y * y
+ z * z * z
+ x * x * y
+ x * x * z
+ y * y * x
+ y * y * z
+ z * z * x
+ z * z * y
+ x * y * z;
acc += a / 10.0 * h3;
}
acc
}
fn root(x: f64, n: f64) -> f64 {
if x <= 0.0 {
0.0
} else {
x.powf(1.0 / n)
}
}
fn cbrt(x: f64) -> f64 {
root(x, 3.0)
}
pub fn correspond(a: &[Vec<Tri>], b: &[Vec<Tri>]) -> Vec<(usize, usize)> {
let ka: Vec<_> = a.iter().map(|s| (rigid_key(s), centroid_area(s))).collect();
let kb: Vec<_> = b.iter().map(|s| (rigid_key(s), centroid_area(s))).collect();
let mut used = vec![false; b.len()];
let mut out = Vec::new();
for (i, (k, c)) in ka.iter().enumerate() {
let mut best: Option<(f64, usize)> = None;
for (j, (k2, c2)) in kb.iter().enumerate() {
if used[j] || !k.matches(k2) {
continue;
}
let d = (0..3).map(|m| (c[m] - c2[m]).powi(2)).sum::<f64>();
if best.is_none_or(|(bd, _)| d < bd) {
best = Some((d, j));
}
}
if let Some((_, j)) = best {
used[j] = true;
out.push((i, j));
}
}
out
}
const MAX_RESIDUAL: f64 = 1e-2;
pub fn recover_axis(before: &[Tri], after: &[Tri], normal: [f64; 3]) -> Option<([f64; 3], f64)> {
if before.is_empty() || before.len() != after.len() {
return None;
}
let n = {
let l = (normal[0].powi(2) + normal[1].powi(2) + normal[2].powi(2)).sqrt();
if l <= 0.0 {
return None;
}
[normal[0] / l, normal[1] / l, normal[2] / l]
};
let ca = centroid_area(before);
let cb = centroid_area(after);
let planar = motion::recover_in_plane(before, after, ca, cb, n)?;
if planar.angle.abs() < 1e-3 {
return None; }
let scale = motion::radius(before, ca);
if scale > 0.0 && planar.residual > scale * MAX_RESIDUAL {
return None; }
Some((planar.point, planar.angle))
}
#[allow(clippy::too_many_arguments)]
pub fn free_annuli(
poses: &[Vec<Tri>],
axis_at: [f64; 3],
normal: [f64; 3],
r_max: f64,
bins: usize,
z_lo: f64,
z_bins: usize,
z_step: f64,
) -> Vec<(f64, f64, f64)> {
if bins == 0 || z_bins == 0 || z_step <= 0.0 || r_max <= 0.0 || !r_max.is_finite() {
return Vec::new();
}
let n = {
let l = (normal[0].powi(2) + normal[1].powi(2) + normal[2].powi(2)).sqrt();
if l <= 0.0 {
return Vec::new();
}
[normal[0] / l, normal[1] / l, normal[2] / l]
};
let (u, v) = plane_basis(n);
let bw = r_max / bins as f64;
let mut occ = vec![vec![false; bins]; z_bins];
for pose in poses {
for t in pose {
let (mut r0, mut r1) = (f64::MAX, 0.0f64);
let (mut a0, mut a1) = (f64::MAX, f64::MIN);
for p in t {
let d = [p[0] - axis_at[0], p[1] - axis_at[1], p[2] - axis_at[2]];
let r = dot3(&d, &u).hypot(dot3(&d, &v));
let h = dot3(&d, &n);
r0 = r0.min(r);
r1 = r1.max(r);
a0 = a0.min(h);
a1 = a1.max(h);
}
if r0 > r_max {
continue;
}
let zi0 = (((a0 - z_lo) / z_step).floor().max(0.0)) as usize;
let zi1 = (((a1 - z_lo) / z_step).ceil().max(0.0)) as usize;
let ri0 = ((r0 / bw).floor().max(0.0)) as usize;
let ri1 = ((r1 / bw).ceil().max(0.0)) as usize;
for row in occ.iter_mut().take(zi1.min(z_bins - 1) + 1).skip(zi0) {
for slot in row.iter_mut().take(ri1.min(bins - 1) + 1).skip(ri0) {
*slot = true;
}
}
}
}
let mut out = Vec::new();
for (zi, row) in occ.iter().enumerate() {
if !row.iter().any(|x| *x) {
continue;
}
let (mut best, mut bs, mut run, mut rs) = (0usize, 0usize, 0usize, 0usize);
for (ri, o) in row.iter().enumerate() {
if *o {
run = 0;
} else {
if run == 0 {
rs = ri;
}
run += 1;
if run > best {
best = run;
bs = rs;
}
}
}
if best > 0 {
out.push((
z_lo + zi as f64 * z_step,
bs as f64 * bw,
(bs + best) as f64 * bw,
));
}
}
out
}
fn dot3(a: &[f64; 3], b: &[f64; 3]) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
fn plane_basis(n: [f64; 3]) -> ([f64; 3], [f64; 3]) {
let t = if n[0].abs() < 0.9 {
[1.0, 0.0, 0.0]
} else {
[0.0, 1.0, 0.0]
};
let u = unit3(cross3(t, n));
(u, unit3(cross3(n, u)))
}
fn cross3(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],
]
}
fn unit3(a: [f64; 3]) -> [f64; 3] {
let l = (a[0] * a[0] + a[1] * a[1] + a[2] * a[2]).sqrt();
if l > 0.0 {
[a[0] / l, a[1] / l, a[2] / l]
} else {
[1.0, 0.0, 0.0]
}
}
#[cfg(test)]
mod tests;
pub struct Occupancy {
lo: [f64; 3],
cell: f64,
n: [usize; 3],
bits: Vec<bool>,
}
impl Occupancy {
pub fn new(lo: [f64; 3], hi: [f64; 3], cell: f64) -> Self {
let n = [
(((hi[0] - lo[0]) / cell).ceil().max(1.0)) as usize,
(((hi[1] - lo[1]) / cell).ceil().max(1.0)) as usize,
(((hi[2] - lo[2]) / cell).ceil().max(1.0)) as usize,
];
Occupancy {
lo,
cell,
n,
bits: vec![false; n[0] * n[1] * n[2]],
}
}
fn idx(&self, i: usize, j: usize, k: usize) -> usize {
(k * self.n[1] + j) * self.n[0] + i
}
pub fn add_pose(&mut self, tris: &[Tri]) {
for t in tris {
let mut mn = [f64::INFINITY; 3];
let mut mx = [f64::NEG_INFINITY; 3];
for v in t {
for c in 0..3 {
mn[c] = mn[c].min(v[c]);
mx[c] = mx[c].max(v[c]);
}
}
let mut a = [0usize; 3];
let mut b = [0usize; 3];
let mut skip = false;
for c in 0..3 {
let f0 = ((mn[c] - self.lo[c]) / self.cell).floor();
let f1 = ((mx[c] - self.lo[c]) / self.cell).ceil();
if f1 < 0.0 || f0 >= self.n[c] as f64 {
skip = true;
break;
}
a[c] = f0.max(0.0) as usize;
b[c] = (f1 as usize).min(self.n[c] - 1);
}
if skip {
continue;
}
for k in a[2]..=b[2] {
for j in a[1]..=b[1] {
for i in a[0]..=b[0] {
let x = self.idx(i, j, k);
self.bits[x] = true;
}
}
}
}
}
pub fn free_at(&self, p: [f64; 3]) -> bool {
let mut c = [0usize; 3];
for a in 0..3 {
let f = ((p[a] - self.lo[a]) / self.cell).floor();
if f < 0.0 || f >= self.n[a] as f64 {
return true; }
c[a] = f as usize;
}
!self.bits[self.idx(c[0], c[1], c[2])]
}
pub fn free_fraction(&self) -> f64 {
let f = self.bits.iter().filter(|b| !**b).count();
f as f64 / self.bits.len().max(1) as f64
}
pub fn largest_free_box(&self) -> Option<([f64; 3], [f64; 3])> {
let mut best: Option<(usize, [usize; 3], [usize; 3])> = None;
for k in 0..self.n[2] {
for j in 0..self.n[1] {
for i in 0..self.n[0] {
if self.bits[self.idx(i, j, k)] {
continue;
}
let (mut ei, mut ej, mut ek) = (i, j, k);
loop {
let mut grew = false;
if ei + 1 < self.n[0] && self.clear(i, ei + 1, j, ej, k, ek) {
ei += 1;
grew = true;
}
if ej + 1 < self.n[1] && self.clear(i, ei, j, ej + 1, k, ek) {
ej += 1;
grew = true;
}
if ek + 1 < self.n[2] && self.clear(i, ei, j, ej, k, ek + 1) {
ek += 1;
grew = true;
}
if !grew {
break;
}
}
let vol = (ei - i + 1) * (ej - j + 1) * (ek - k + 1);
if best.is_none_or(|(v, _, _)| vol > v) {
best = Some((vol, [i, j, k], [ei, ej, ek]));
}
}
}
}
best.map(|(_, a, b)| {
(
[
self.lo[0] + a[0] as f64 * self.cell,
self.lo[1] + a[1] as f64 * self.cell,
self.lo[2] + a[2] as f64 * self.cell,
],
[
self.lo[0] + (b[0] + 1) as f64 * self.cell,
self.lo[1] + (b[1] + 1) as f64 * self.cell,
self.lo[2] + (b[2] + 1) as f64 * self.cell,
],
)
})
}
fn clear(&self, i0: usize, i1: usize, j0: usize, j1: usize, k0: usize, k1: usize) -> bool {
for k in k0..=k1 {
for j in j0..=j1 {
for i in i0..=i1 {
if self.bits[self.idx(i, j, k)] {
return false;
}
}
}
}
true
}
}