From c650d1867f8f2caf7ae8ee0ce29650cad85705cd Mon Sep 17 00:00:00 2001 From: bherbruck Date: Mon, 12 Oct 2026 13:54:14 +0000 Subject: [PATCH] Geometry comparison helpers: STL reader, mesh inertia, sampled Hausdorff distance spatial.rs in solvecraft-cli: read binary or ASCII STL; bounds, volume, centre of mass and principal second moments of a closed triangle mesh (signed tetrahedra, Jacobi eigenvalues); sampled symmetric Hausdorff distance between two meshes using a uniform grid with an expanding-shell search. Unit test on a cube (moments and a 0.3 mm offset). Not called from CLI commands yet. Co-Authored-By: Claude Opus 5.5 --- apps/solvecraft-cli/src/main.rs | 3 + apps/solvecraft-cli/src/spatial.rs | 333 +++++++++++++++++++++++++++++ 2 files changed, 336 insertions(+) create mode 100644 apps/solvecraft-cli/src/spatial.rs diff --git a/apps/solvecraft-cli/src/main.rs b/apps/solvecraft-cli/src/main.rs index 91da5e22..c504770a 100644 --- a/apps/solvecraft-cli/src/main.rs +++ b/apps/solvecraft-cli/src/main.rs @@ -21,6 +21,9 @@ mod fuzz; mod oracle; mod recipe; mod seams; +// Geometry comparison helpers; not called from the CLI commands yet. +#[cfg_attr(not(test), allow(dead_code))] +mod spatial; use std::process::ExitCode; diff --git a/apps/solvecraft-cli/src/spatial.rs b/apps/solvecraft-cli/src/spatial.rs new file mode 100644 index 00000000..95ed5ef4 --- /dev/null +++ b/apps/solvecraft-cli/src/spatial.rs @@ -0,0 +1,333 @@ +//! Geometry comparison helpers: an STL reader, bounding boxes, volume, centre of mass and principal +//! second moments of a closed triangle mesh, and a sampled symmetric Hausdorff distance between two +//! triangle meshes. + +use std::collections::HashMap; + +/// A triangle soup in mm. +pub type Tri = [[f64; 3]; 3]; + +fn sub(a: [f64; 3], b: [f64; 3]) -> [f64; 3] { + [a[0] - b[0], a[1] - b[1], a[2] - b[2]] +} +fn add(a: [f64; 3], b: [f64; 3]) -> [f64; 3] { + [a[0] + b[0], a[1] + b[1], a[2] + b[2]] +} +fn scale(a: [f64; 3], s: f64) -> [f64; 3] { + [a[0] * s, a[1] * s, a[2] * s] +} +fn dot(a: [f64; 3], b: [f64; 3]) -> f64 { + a[0] * b[0] + a[1] * b[1] + a[2] * b[2] +} +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]] +} + +/// Read a binary (or ASCII) STL file. +pub fn read_stl(path: &str) -> Result, String> { + let b = std::fs::read(path).map_err(|e| format!("{path}: {e}"))?; + if b.len() >= 84 { + let n = u32::from_le_bytes([b[80], b[81], b[82], b[83]]) as usize; + if b.len() == 84 + 50 * n { + let f = |o: usize| f64::from(f32::from_le_bytes([b[o], b[o + 1], b[o + 2], b[o + 3]])); + return Ok((0..n) + .map(|i| { + let o = 84 + 50 * i + 12; + let v = |k: usize| [f(o + 12 * k), f(o + 12 * k + 4), f(o + 12 * k + 8)]; + [v(0), v(1), v(2)] + }) + .collect()); + } + } + let text = String::from_utf8_lossy(&b); + let mut verts = Vec::new(); + for line in text.lines() { + let mut it = line.split_whitespace(); + if it.next() == Some("vertex") { + let c: Vec = it.filter_map(|x| x.parse().ok()).collect(); + if c.len() == 3 { + verts.push([c[0], c[1], c[2]]); + } + } + } + if verts.is_empty() || verts.len() % 3 != 0 { + return Err(format!("{path}: not an STL file")); + } + Ok(verts.chunks(3).map(|c| [c[0], c[1], c[2]]).collect()) +} + +/// Volume, centre of mass and the principal second moments of volume (mm^5, sorted ascending) of a +/// closed, outward-oriented triangle soup, by signed tetrahedra from the origin. +pub fn mass_properties(tris: &[Tri]) -> (f64, [f64; 3], [f64; 3]) { + let (mut v6, mut m1) = (0.0, [0.0; 3]); + let mut c = [[0.0; 3]; 3]; // ∫ x xᵀ dV + for t in tris { + let d = dot(t[0], cross(t[1], t[2])); + v6 += d; + let s = add(add(t[0], t[1]), t[2]); + m1 = add(m1, scale(s, d / 24.0)); + for (i, row) in c.iter_mut().enumerate() { + for (j, cell) in row.iter_mut().enumerate() { + let vv: f64 = t.iter().map(|p| p[i] * p[j]).sum::() + s[i] * s[j]; + *cell += d / 120.0 * vv; + } + } + } + let vol = v6 / 6.0; + if vol.abs() < 1e-12 { + return (0.0, [0.0; 3], [0.0; 3]); + } + let com = scale(m1, 1.0 / vol); + // Central covariance and inertia tensor I = tr(C)·1 − C. + let mut cc = c; + for (i, row) in cc.iter_mut().enumerate() { + for (j, cell) in row.iter_mut().enumerate() { + *cell -= vol * com[i] * com[j]; + } + } + let tr = cc[0][0] + cc[1][1] + cc[2][2]; + let mut inertia = [[0.0; 3]; 3]; + for i in 0..3 { + for j in 0..3 { + inertia[i][j] = if i == j { tr - cc[i][j] } else { -cc[i][j] }; + } + } + let mut ev = eigen_sym3(inertia); + ev.sort_by(f64::total_cmp); + (vol, com, ev) +} + +/// Eigenvalues of a symmetric 3×3 matrix (Jacobi rotations). +fn eigen_sym3(mut a: [[f64; 3]; 3]) -> [f64; 3] { + for _ in 0..50 { + let (mut p, mut q, mut big) = (0, 1, 0.0); + for (i, j) in [(0, 1), (0, 2), (1, 2)] { + if a[i][j].abs() > big { + (p, q, big) = (i, j, a[i][j].abs()); + } + } + if big <= 1e-15 * (a[0][0].abs() + a[1][1].abs() + a[2][2].abs()).max(1e-300) { + break; + } + let theta = (a[q][q] - a[p][p]) / (2.0 * a[p][q]); + let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt()); + let t = if theta == 0.0 { 1.0 } else { t }; + let (cs, sn) = (1.0 / (t * t + 1.0).sqrt(), t / (t * t + 1.0).sqrt()); + let mut b = a; + for k in 0..3 { + b[k][p] = cs * a[k][p] - sn * a[k][q]; + b[k][q] = sn * a[k][p] + cs * a[k][q]; + } + let mut c2 = b; + for k in 0..3 { + c2[p][k] = cs * b[p][k] - sn * b[q][k]; + c2[q][k] = sn * b[p][k] + cs * b[q][k]; + } + a = c2; + } + [a[0][0], a[1][1], a[2][2]] +} + +/// Axis-aligned bounds of a triangle soup. +pub fn bounds(tris: &[Tri]) -> ([f64; 3], [f64; 3]) { + let (mut lo, mut hi) = ([f64::INFINITY; 3], [f64::NEG_INFINITY; 3]); + for p in tris.iter().flatten() { + for k in 0..3 { + lo[k] = lo[k].min(p[k]); + hi[k] = hi[k].max(p[k]); + } + } + (lo, hi) +} + +/// Distance from a point to a triangle (Ericson, Real-Time Collision Detection 5.1.5). +fn point_tri(p: [f64; 3], t: &Tri) -> f64 { + let (a, b, c) = (t[0], t[1], t[2]); + let (ab, ac, ap) = (sub(b, a), sub(c, a), sub(p, a)); + let (d1, d2) = (dot(ab, ap), dot(ac, ap)); + let q = if d1 <= 0.0 && d2 <= 0.0 { + a + } else { + let bp = sub(p, b); + let (d3, d4) = (dot(ab, bp), dot(ac, bp)); + if d3 >= 0.0 && d4 <= d3 { + b + } else { + let vc = d1 * d4 - d3 * d2; + if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 { + add(a, scale(ab, d1 / (d1 - d3))) + } else { + let cp = sub(p, c); + let (d5, d6) = (dot(ab, cp), dot(ac, cp)); + if d6 >= 0.0 && d5 <= d6 { + c + } else { + let vb = d5 * d2 - d1 * d6; + if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 { + add(a, scale(ac, d2 / (d2 - d6))) + } else { + let va = d3 * d6 - d5 * d4; + if va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0 { + add(b, scale(sub(c, b), (d4 - d3) / ((d4 - d3) + (d5 - d6)))) + } else { + let den = va + vb + vc; + if den.abs() < 1e-300 { a } else { add(a, add(scale(ab, vb / den), scale(ac, vc / den))) } + } + } + } + } + } + }; + let d = sub(p, q); + dot(d, d).sqrt() +} + +/// Uniform grid of triangles for distance queries up to `cap`. +struct Grid<'a> { + tris: &'a [Tri], + cell: f64, + cells: HashMap<(i64, i64, i64), Vec>, +} + +impl<'a> Grid<'a> { + fn new(tris: &'a [Tri], cell: f64) -> Self { + let mut cells: HashMap<(i64, i64, i64), Vec> = HashMap::new(); + let key = |x: f64| (x / cell).floor() as i64; + for (i, t) in tris.iter().enumerate() { + let (lo, hi) = bounds(std::slice::from_ref(t)); + for x in key(lo[0])..=key(hi[0]) { + for y in key(lo[1])..=key(hi[1]) { + for z in key(lo[2])..=key(hi[2]) { + cells.entry((x, y, z)).or_default().push(i as u32); + } + } + } + } + Grid { tris, cell, cells } + } + + /// Distance from `p` to the nearest triangle, or `cap` if none is that close. Searches shells + /// of cells outwards and stops once the nearest hit is closer than the unsearched shells. + fn distance(&self, p: [f64; 3], cap: f64, seen: &mut [u32], stamp: u32) -> f64 { + let rmax = (cap / self.cell).ceil() as i64; + let k = |x: f64| (x / self.cell).floor() as i64; + let (cx, cy, cz) = (k(p[0]), k(p[1]), k(p[2])); + let mut best = cap; + for r in 0..=rmax { + for x in cx - r..=cx + r { + for y in cy - r..=cy + r { + for z in cz - r..=cz + r { + // only the shell at Chebyshev distance r + if (x - cx).abs() != r && (y - cy).abs() != r && (z - cz).abs() != r { + continue; + } + let Some(list) = self.cells.get(&(x, y, z)) else { continue }; + for &i in list { + let Some(slot) = seen.get_mut(i as usize) else { continue }; + if *slot == stamp { + continue; + } + *slot = stamp; + if let Some(t) = self.tris.get(i as usize) { + best = best.min(point_tri(p, t)); + } + } + } + } + } + // Everything not yet searched is at least r·cell away from p. + if best <= r as f64 * self.cell { + break; + } + } + best + } +} + +/// At most this many sample points per side (larger meshes are sampled with an even stride). +const MAX_SAMPLES: usize = 250_000; + +/// Sample points of a triangle soup: its vertices and triangle centroids (every k-th triangle for +/// very large meshes). +fn samples(tris: &[Tri]) -> Vec<[f64; 3]> { + let stride = (tris.len() * 4).div_ceil(MAX_SAMPLES).max(1); + let mut out: Vec<[f64; 3]> = Vec::with_capacity(tris.len() * 4 / stride + 4); + for t in tris.iter().step_by(stride) { + out.extend_from_slice(t); + out.push(scale(add(add(t[0], t[1]), t[2]), 1.0 / 3.0)); + } + out +} + +/// One-sided sampled distance from `from` to `to`: (max, mean, worst sample point). Distances are +/// capped at `cap`. +fn one_sided(from: &[Tri], to: &[Tri], cap: f64) -> (f64, f64, [f64; 3]) { + let (lo, hi) = bounds(to); + let diag = dot(sub(hi, lo), sub(hi, lo)).sqrt(); + // Cells about twice the typical triangle size (so a cell holds a handful of triangles), but no + // finer than diag/400 (so big flat triangles don't fill too many cells). + let area: f64 = to.iter().map(|t| dot(cross(sub(t[1], t[0]), sub(t[2], t[0])), cross(sub(t[1], t[0]), sub(t[2], t[0]))).sqrt() / 2.0).sum(); + let typical = (area / to.len().max(1) as f64).sqrt(); + let grid = Grid::new(to, (2.0 * typical).max(diag / 400.0).max(1e-3)); + let mut seen = vec![0u32; to.len()]; + let (mut max, mut sum, mut worst, mut n) = (0.0f64, 0.0, [0.0; 3], 0usize); + for (i, p) in samples(from).into_iter().enumerate() { + let d = grid.distance(p, cap, &mut seen, i as u32 + 1); + sum += d; + n += 1; + if d > max { + max = d; + worst = p; + } + } + (max, if n > 0 { sum / n as f64 } else { 0.0 }, worst) +} + +/// Sampled symmetric Hausdorff distance between two triangle soups (distances capped at `cap`). +pub struct Hausdorff { + pub a_to_b: f64, + pub b_to_a: f64, + pub mean: f64, + /// Where the larger one-sided distance was found (mm, world). + pub worst_at: [f64; 3], +} + +pub fn hausdorff(a_mesh: &[Tri], b_mesh: &[Tri], cap: f64) -> Hausdorff { + let (a, am, ap) = one_sided(a_mesh, b_mesh, cap); + let (b, bm, bp) = one_sided(b_mesh, a_mesh, cap); + Hausdorff { a_to_b: a, b_to_a: b, mean: (am + bm) / 2.0, worst_at: if a >= b { ap } else { bp } } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn cube(s: f64, o: [f64; 3]) -> Vec { + let v = |x: f64, y: f64, z: f64| [o[0] + x * s, o[1] + y * s, o[2] + z * s]; + let q = |a: [f64; 3], b: [f64; 3], c: [f64; 3], d: [f64; 3]| vec![[a, b, c], [a, c, d]]; + let mut t = Vec::new(); + t.extend(q(v(0., 0., 0.), v(0., 1., 0.), v(1., 1., 0.), v(1., 0., 0.))); + t.extend(q(v(0., 0., 1.), v(1., 0., 1.), v(1., 1., 1.), v(0., 1., 1.))); + t.extend(q(v(0., 0., 0.), v(1., 0., 0.), v(1., 0., 1.), v(0., 0., 1.))); + t.extend(q(v(0., 1., 0.), v(0., 1., 1.), v(1., 1., 1.), v(1., 1., 0.))); + t.extend(q(v(0., 0., 0.), v(0., 0., 1.), v(0., 1., 1.), v(0., 1., 0.))); + t.extend(q(v(1., 0., 0.), v(1., 1., 0.), v(1., 1., 1.), v(1., 0., 1.))); + t + } + + #[test] + fn box_moments_and_distance() { + let c = cube(10.0, [5.0, -3.0, 2.0]); + let (v, com, pm) = mass_properties(&c); + assert!((v - 1000.0).abs() < 1e-9 && (com[0] - 10.0).abs() < 1e-9 && (com[2] - 7.0).abs() < 1e-9); + // V·(a²+b²)/12 for a cube of side 10: 1000·200/12. + for m in pm { + assert!((m - 1000.0 * 200.0 / 12.0).abs() < 1e-6, "{pm:?}"); + } + let moved = cube(10.0, [5.0, -3.0, 2.3]); + let h = hausdorff(&c, &moved, 1.0); + assert!((h.a_to_b - 0.3).abs() < 1e-9 && (h.b_to_a - 0.3).abs() < 1e-9); + let same = hausdorff(&c, &c, 1.0); + assert!(same.a_to_b < 1e-12); + } +}