From 6dbda8ecf1118161a2c8e581e1479653f84fcdd8 Mon Sep 17 00:00:00 2001 From: Brennen Herbruck Date: Wed, 21 Oct 2026 05:25:53 -0400 Subject: [PATCH] smlib tests: SSI scorer geometry (truth distances, polyline sets) Test-support port of the parts of the SSI corpus's ssi_geom.py and score.py the scorer needs: exact distances to the implicit truths, the Gauss-Newton closest point on traced param patches, B-spline/NURBS evaluation of the case surfaces, and score.py's segment-hash polyline set, polyline distance, components, free ends and topology signature, following numpy's arithmetic order. Co-Authored-By: Claude Opus 5.5 --- crates/smlib/tests/ssi_score/geom.rs | 519 ++++++++++++++++++++++++ crates/smlib/tests/ssi_score/polyset.rs | 350 ++++++++++++++++ 2 files changed, 869 insertions(+) create mode 100644 crates/smlib/tests/ssi_score/geom.rs create mode 100644 crates/smlib/tests/ssi_score/polyset.rs diff --git a/crates/smlib/tests/ssi_score/geom.rs b/crates/smlib/tests/ssi_score/geom.rs new file mode 100644 index 0000000000..13c8ecf715 --- /dev/null +++ b/crates/smlib/tests/ssi_score/geom.rs @@ -0,0 +1,519 @@ +//! Exact distances to the corpus truths and B-spline/NURBS evaluation: a port of the parts of +//! the corpus's `scoring/ssi_geom.py` the scorer uses (`truth_distance`, `param_distance`, +//! `bspline_basis`, `Nurbs.eval`). Arithmetic follows numpy's order of operations so the +//! numbers match the Python scorer to the last few bits. + +use serde_json::Value; + +pub type V3 = [f64; 3]; + +pub fn sub(a: V3, b: V3) -> V3 { + [a[0] - b[0], a[1] - b[1], a[2] - b[2]] +} + +pub fn dot(a: V3, b: V3) -> f64 { + a[0] * b[0] + a[1] * b[1] + a[2] * b[2] +} + +pub fn norm(a: V3) -> f64 { + dot(a, a).sqrt() +} + +fn unit(a: V3) -> V3 { + let n = norm(a); + [a[0] / n, a[1] / n, a[2] / n] +} + +/// numpy's `x ** i` for a float array and a small integer exponent. +pub fn powi_np(x: f64, i: usize) -> f64 { + match i { + 0 => 1.0, + 1 => x, + 2 => x * x, + _ => x.powf(i as f64), + } +} + +/// numpy's `np.clip` (NaN propagates). +pub fn clip(x: f64, lo: f64, hi: f64) -> f64 { + if x.is_nan() { + return x; + } + let y = if x < lo { lo } else { x }; + if y > hi { hi } else { y } +} + +/// numpy's `ndarray.max()`: NaN wins; `-inf` for an empty slice (numpy raises, never reached). +pub fn np_max(xs: &[f64]) -> f64 { + let mut m = f64::NEG_INFINITY; + for &x in xs { + if x.is_nan() { + return x; + } + if x > m { + m = x; + } + } + m +} + +/// numpy's `np.minimum` on two scalars (NaN wins). +pub fn np_minimum(a: f64, b: f64) -> f64 { + if a.is_nan() || b.is_nan() { + f64::NAN + } else if b < a { + b + } else { + a + } +} + +/// Python's builtin `max(a, b)`: keeps `a` unless `b > a`. +pub fn py_max(a: f64, b: f64) -> f64 { + if b > a { b } else { a } +} + +/// Python's builtin `min(a, b)`: keeps `a` unless `b < a`. +pub fn py_min(a: f64, b: f64) -> f64 { + if b < a { b } else { a } +} + +/// numpy's `linspace(start, stop, num)`. +pub fn linspace(start: f64, stop: f64, num: usize) -> Vec { + if num == 0 { + return Vec::new(); + } + if num == 1 { + return vec![start]; + } + let step = (stop - start) / (num - 1) as f64; + let mut out: Vec = (0..num).map(|i| i as f64 * step + start).collect(); + if let Some(last) = out.last_mut() { + *last = stop; + } + out +} + +// ------------------------------------------------------------------------------------------ +// JSON helpers + +pub fn num(v: &Value) -> f64 { + v.as_f64().unwrap_or(f64::NAN) +} + +pub fn v3(v: &Value) -> Result { + match v { + Value::Null => Ok([f64::NAN; 3]), + Value::Array(a) if a.len() == 3 => Ok([num(&a[0]), num(&a[1]), num(&a[2])]), + _ => Err(format!("not a 3-vector: {v}")), + } +} + +pub fn v3_list(v: Option<&Value>) -> Result, String> { + match v { + None | Some(Value::Null) => Ok(Vec::new()), + Some(Value::Array(a)) => a.iter().map(v3).collect(), + Some(other) => Err(format!("not a point list: {other}")), + } +} + +fn field<'a>(v: &'a Value, key: &str) -> Result<&'a Value, String> { + v.get(key).ok_or_else(|| format!("missing field {key}")) +} + +fn f64_field(v: &Value, key: &str) -> Result { + field(v, key)?.as_f64().ok_or_else(|| format!("{key} is not a number")) +} + +fn f64_list(v: &Value) -> Result, String> { + v.as_array().ok_or_else(|| format!("not a list: {v}"))?.iter().map(|x| x.as_f64().ok_or_else(|| format!("not a number: {x}"))).collect() +} + +/// A 2D coefficient array `c[i][j]` (numpy `atleast_2d`: a scalar is 1x1, a flat list one row). +pub type Poly2 = Vec>; + +fn poly2(v: &Value) -> Result { + match v { + Value::Number(_) => Ok(vec![vec![num(v)]]), + Value::Array(a) if a.iter().all(Value::is_number) => Ok(vec![f64_list(v)?]), + Value::Array(a) => { + let rows: Vec> = a.iter().map(f64_list).collect::>()?; + let w = rows.first().map_or(0, Vec::len); + if rows.iter().any(|r| r.len() != w) { + return Err("ragged coefficient array".into()); + } + Ok(rows) + } + _ => Err(format!("not a coefficient array: {v}")), + } +} + +// ------------------------------------------------------------------------------------------ +// truths + +/// A rational polynomial patch `(hx, hy, hz) / hw` on a parameter box: the `param` truth. +#[derive(Clone, Debug)] +pub struct ParamPatch { + hom: [Poly2; 4], + dhu: [Poly2; 4], + dhv: [Poly2; 4], + domain: [f64; 4], + /// Seeds: the 41 x 41 grid of the domain (u major), with the patch evaluated there. + seeds: Vec<(f64, f64, V3)>, +} + +/// One exact surface description of a case (`truth` entries). +#[derive(Clone, Debug)] +pub enum Truth { + Plane { point: V3, normal: V3 }, + Sphere { center: V3, radius: f64 }, + Cylinder { point: V3, axis: V3, radius: f64 }, + Cone { point: V3, axis: V3, half_angle_deg: f64 }, + Torus { point: V3, axis: V3, major: f64, minor: f64 }, + HeightField { coeffs: Poly2, axes: [usize; 3] }, + Param(Box), + Quadric { center: V3, axes: [V3; 3], abc: V3, signs: V3 }, + Ellipsoid { center: V3, axes: [V3; 3], abc: V3 }, +} + +fn axes3(v: &Value) -> Result<[V3; 3], String> { + let a = v.as_array().ok_or("axes: not a list")?; + if a.len() != 3 { + return Err("axes: need 3 rows".into()); + } + Ok([v3(&a[0])?, v3(&a[1])?, v3(&a[2])?]) +} + +impl Truth { + pub fn from_json(t: &Value) -> Result { + let kind = field(t, "kind")?.as_str().ok_or("kind: not a string")?; + Ok(match kind { + "plane" => Truth::Plane { point: v3(field(t, "point")?)?, normal: v3(field(t, "normal")?)? }, + "sphere" => Truth::Sphere { center: v3(field(t, "center")?)?, radius: f64_field(t, "radius")? }, + "cylinder" => Truth::Cylinder { point: v3(field(t, "point")?)?, axis: v3(field(t, "axis")?)?, radius: f64_field(t, "radius")? }, + "cone" => Truth::Cone { point: v3(field(t, "point")?)?, axis: v3(field(t, "axis")?)?, half_angle_deg: f64_field(t, "half_angle_deg")? }, + "torus" => { + Truth::Torus { point: v3(field(t, "point")?)?, axis: v3(field(t, "axis")?)?, major: f64_field(t, "R")?, minor: f64_field(t, "r")? } + } + "heightfield" => { + let axes = match t.get("axes") { + None => [0, 1, 2], + Some(a) => { + let a = f64_list(a)?; + if a.len() != 3 || a.iter().any(|&x| !(0.0..=2.0).contains(&x)) { + return Err("heightfield axes".into()); + } + [a[0] as usize, a[1] as usize, a[2] as usize] + } + }; + Truth::HeightField { coeffs: poly2(field(t, "coeffs")?)?, axes } + } + "param" => { + let hom = field(t, "hom")?.as_array().ok_or("hom: not a list")?; + if hom.len() != 4 { + return Err("hom: need 4 coefficient arrays".into()); + } + let hom = [poly2(&hom[0])?, poly2(&hom[1])?, poly2(&hom[2])?, poly2(&hom[3])?]; + let d = f64_list(field(t, "domain")?)?; + if d.len() != 4 { + return Err("param domain".into()); + } + Truth::Param(Box::new(ParamPatch::new(hom, [d[0], d[1], d[2], d[3]]))) + } + "quadric" => { + let abc = f64_list(field(t, "abc")?)?; + let sg = f64_list(field(t, "signs")?)?; + if abc.len() != 3 || sg.len() != 3 { + return Err("quadric abc/signs".into()); + } + Truth::Quadric { + center: v3(field(t, "center")?)?, + axes: axes3(field(t, "axes")?)?, + abc: [abc[0], abc[1], abc[2]], + signs: [sg[0], sg[1], sg[2]], + } + } + "ellipsoid" => { + let abc = f64_list(field(t, "abc")?)?; + if abc.len() != 3 { + return Err("ellipsoid abc".into()); + } + Truth::Ellipsoid { center: v3(field(t, "center")?)?, axes: axes3(field(t, "axes")?)?, abc: [abc[0], abc[1], abc[2]] } + } + other => return Err(format!("unknown truth kind {other}")), + }) + } + + /// Distance from each point to the surface (`truth_distance`): exact for the implicit kinds + /// except the height field, quadric and ellipsoid (first order), closest point for `param`. + pub fn distance(&self, pts: &[V3]) -> Vec { + pts.iter().map(|&p| self.distance1(p)).collect() + } + + pub fn distance1(&self, p: V3) -> f64 { + match self { + Truth::Plane { point, normal } => dot(sub(p, *point), unit(*normal)).abs(), + Truth::Sphere { center, radius } => (norm(sub(p, *center)) - radius).abs(), + Truth::Cylinder { point, axis, radius } => { + let (_, rho) = axial(p, *point, *axis); + (rho - radius).abs() + } + Truth::Cone { point, axis, half_angle_deg } => { + let (h, rho) = axial(p, *point, *axis); + let al = half_angle_deg.to_radians(); + (rho * al.cos() - h * al.sin()).abs() + } + Truth::Torus { point, axis, major, minor } => { + let (h, rho) = axial(p, *point, *axis); + ((rho - major).hypot(h) - minor).abs() + } + Truth::HeightField { coeffs, axes } => { + let (x, y, z) = (p[axes[0]], p[axes[1]], p[axes[2]]); + let (mut h, mut hx, mut hy) = (0.0, 0.0, 0.0); + for (i, row) in coeffs.iter().enumerate() { + for (j, &c) in row.iter().enumerate() { + if c == 0.0 { + continue; + } + h += c * powi_np(x, i) * powi_np(y, j); + if i > 0 { + hx += c * i as f64 * powi_np(x, i - 1) * powi_np(y, j); + } + if j > 0 { + hy += c * j as f64 * powi_np(x, i) * powi_np(y, j - 1); + } + } + } + (z - h).abs() / (1.0 + hx * hx + hy * hy).sqrt() + } + Truth::Param(patch) => patch.distance(p), + Truth::Quadric { center, axes, abc, signs } => quadric_distance(p, *center, axes, *abc, Some(*signs)), + Truth::Ellipsoid { center, axes, abc } => quadric_distance(p, *center, axes, *abc, None), + } + } +} + +/// (height along the unit axis, distance from the axis). +fn axial(p: V3, point: V3, axis: V3) -> (f64, f64) { + let a = unit(axis); + let q = sub(p, point); + let h = dot(q, a); + (h, norm([q[0] - h * a[0], q[1] - h * a[1], q[2] - h * a[2]])) +} + +fn quadric_distance(p: V3, center: V3, axes: &[V3; 3], abc: V3, signs: Option) -> f64 { + let q = sub(p, center); + let l: V3 = [0, 1, 2].map(|k| dot(q, axes[k]) / abc[k]); + let (e, w) = match signs { + Some(sg) => { + let e = (sg[0] * l[0] * l[0] + sg[1] * l[1] * l[1] + sg[2] * l[2] * l[2]) - 1.0; + (e, [0, 1, 2].map(|k| 2.0 * (sg[k] * l[k] / abc[k]))) + } + None => { + let e = (l[0] * l[0] + l[1] * l[1] + l[2] * l[2]) - 1.0; + (e, [0, 1, 2].map(|k| 2.0 * (l[k] / abc[k]))) + } + }; + let grad: V3 = [0, 1, 2].map(|m| w[0] * axes[0][m] + w[1] * axes[1][m] + w[2] * axes[2][m]); + e.abs() / norm(grad) +} + +/// `c(u, w)` with the powers `up[i] = u**i`, `wp[j] = w**j` precomputed (the same `pow` calls +/// numpy makes, once per exponent instead of once per term). +fn p2_eval_pow(c: &Poly2, up: &[f64], wp: &[f64]) -> f64 { + let mut out = 0.0; + for (row, &ui) in c.iter().zip(up) { + for (&cij, &wj) in row.iter().zip(wp) { + if cij != 0.0 { + out += cij * ui * wj; + } + } + } + out +} + +fn pow_table(x: f64, n: usize) -> Vec { + (0..n).map(|i| powi_np(x, i)).collect() +} + +fn p2_du(c: &Poly2) -> Poly2 { + let cols = c.first().map_or(1, Vec::len); + if c.len() <= 1 { + return vec![vec![0.0; cols]]; + } + c.iter().enumerate().skip(1).map(|(i, row)| row.iter().map(|&x| x * i as f64).collect()).collect() +} + +fn p2_dv(c: &Poly2) -> Poly2 { + c.iter().map(|row| if row.len() <= 1 { vec![0.0] } else { row.iter().enumerate().skip(1).map(|(j, &x)| x * j as f64).collect() }).collect() +} + +const N_SEED: usize = 41; + +impl ParamPatch { + fn new(hom: [Poly2; 4], domain: [f64; 4]) -> ParamPatch { + let dhu = [p2_du(&hom[0]), p2_du(&hom[1]), p2_du(&hom[2]), p2_du(&hom[3])]; + let dhv = [p2_dv(&hom[0]), p2_dv(&hom[1]), p2_dv(&hom[2]), p2_dv(&hom[3])]; + let mut patch = ParamPatch { hom, dhu, dhv, domain, seeds: Vec::new() }; + let us = linspace(domain[0], domain[1], N_SEED); + let vs = linspace(domain[2], domain[3], N_SEED); + let mut seeds = Vec::with_capacity(N_SEED * N_SEED); + for &u in &us { + for &v in &vs { + seeds.push((u, v, patch.eval(u, v))); + } + } + patch.seeds = seeds; + patch + } + + fn pows(&self, u: f64, v: f64) -> (Vec, Vec) { + let rows = self.hom.iter().map(Vec::len).max().unwrap_or(1); + let cols = self.hom.iter().flat_map(|c| c.iter().map(Vec::len)).max().unwrap_or(1); + (pow_table(u, rows), pow_table(v, cols)) + } + + pub fn eval(&self, u: f64, v: f64) -> V3 { + let (up, vp) = self.pows(u, v); + let h = [0, 1, 2, 3].map(|k| p2_eval_pow(&self.hom[k], &up, &vp)); + [h[0] / h[3], h[1] / h[3], h[2] / h[3]] + } + + fn derivs(&self, u: f64, v: f64) -> (V3, V3, V3) { + let (up, vp) = self.pows(u, v); + let h = [0, 1, 2, 3].map(|k| p2_eval_pow(&self.hom[k], &up, &vp)); + let hu = [0, 1, 2, 3].map(|k| p2_eval_pow(&self.dhu[k], &up, &vp)); + let hv = [0, 1, 2, 3].map(|k| p2_eval_pow(&self.dhv[k], &up, &vp)); + let s: V3 = [h[0] / h[3], h[1] / h[3], h[2] / h[3]]; + let su: V3 = [0, 1, 2].map(|k| (hu[k] - s[k] * hu[3]) / h[3]); + let sv: V3 = [0, 1, 2].map(|k| (hv[k] - s[k] * hv[3]) / h[3]); + (s, su, sv) + } + + /// `param_distance`: Gauss-Newton closest point from the 12 nearest grid seeds in turn, + /// clamped to the domain, until one lands within 1e-12. + pub fn distance(&self, x: V3) -> f64 { + let d2: Vec = self + .seeds + .iter() + .map(|&(_, _, g)| { + let d = sub(x, g); + d[0] * d[0] + d[1] * d[1] + d[2] * d[2] + }) + .collect(); + let mut order: Vec = (0..d2.len()).collect(); + order.sort_by(|&a, &b| d2[a].total_cmp(&d2[b])); + let d0 = self.domain; + let mut out = f64::INFINITY; + for &seed in order.iter().take(12) { + if !(out > 1e-12) { + break; + } + let (mut u, mut v, _) = self.seeds[seed]; + for _ in 0..40 { + let (s, su, sv) = self.derivs(u, v); + let r = sub(s, x); + let a11 = dot(su, su); + let a12 = dot(su, sv); + let a22 = dot(sv, sv); + let b1 = -dot(su, r); + let b2 = -dot(sv, r); + let mut det = a11 * a22 - a12 * a12; + if det.abs() < 1e-300 { + det = 1e-300; + } + let du = (a22 * b1 - a12 * b2) / det; + let dv = (a11 * b2 - a12 * b1) / det; + u = clip(u + du, d0[0], d0[1]); + v = clip(v + dv, d0[2], d0[3]); + } + out = np_minimum(out, norm(sub(self.eval(u, v), x))); + } + out + } +} + +// ------------------------------------------------------------------------------------------ +// B-spline / NURBS surfaces of the case specs + +/// `bspline_basis`: the knot span containing `t` and the `p + 1` nonzero basis functions there. +pub fn bspline_basis(knots: &[f64], p: usize, t: f64) -> (usize, Vec) { + let n = knots.len().saturating_sub(p + 2); + let span = if knots.get(n + 1).is_some_and(|&k| t >= k) { + n + } else { + let right = knots.partition_point(|&k| k <= t); + (right.saturating_sub(1)).clamp(p.min(n), n) + }; + let mut nb = vec![0.0; p + 1]; + nb[0] = 1.0; + let mut left = vec![0.0; p + 1]; + let mut rightv = vec![0.0; p + 1]; + for j in 1..=p { + left[j] = t - knots.get((span + 1).wrapping_sub(j)).copied().unwrap_or(f64::NAN); + rightv[j] = knots.get(span + j).copied().unwrap_or(f64::NAN) - t; + let mut saved = 0.0; + for r in 0..j { + let tmp = nb[r] / (rightv[r + 1] + left[j - r]); + nb[r] = saved + rightv[r + 1] * tmp; + saved = left[j - r] * tmp; + } + nb[j] = saved; + } + (span, nb) +} + +/// A tensor-product NURBS surface from a case's `{"type": "nurbs", ...}` spec. +#[derive(Clone, Debug)] +pub struct Nurbs { + pub p: usize, + pub q: usize, + pub knots_u: Vec, + pub knots_v: Vec, + /// Homogeneous control net (wx, wy, wz, w), `nu x nv`, row-major along u. + net: Vec<[f64; 4]>, + nv: usize, +} + +impl Nurbs { + pub fn from_spec(s: &Value) -> Result { + let deg = f64_list(field(s, "degree")?)?; + if deg.len() != 2 || deg.iter().any(|&d| !(1.0..=30.0).contains(&d)) { + return Err("nurbs degree".into()); + } + let (p, q) = (deg[0] as usize, deg[1] as usize); + let knots_u = f64_list(field(s, "knots_u")?)?; + let knots_v = f64_list(field(s, "knots_v")?)?; + let nu = f64_field(s, "nu")? as usize; + let nv = f64_field(s, "nv")? as usize; + let pts = v3_list(Some(field(s, "points")?))?; + let w = f64_list(field(s, "weights")?)?; + if nu <= p || nv <= q || pts.len() != nu * nv || w.len() != nu * nv || knots_u.len() != nu + p + 1 || knots_v.len() != nv + q + 1 { + return Err("nurbs: inconsistent sizes".into()); + } + let net = pts.iter().zip(&w).map(|(pt, &wt)| [pt[0] * wt, pt[1] * wt, pt[2] * wt, wt]).collect(); + Ok(Nurbs { p, q, knots_u, knots_v, net, nv }) + } + + /// The point at (u, v) of the natural parameter box. + pub fn eval(&self, u: f64, v: f64) -> V3 { + let (su, nu) = bspline_basis(&self.knots_u, self.p, u); + let (sv, nvb) = bspline_basis(&self.knots_v, self.q, v); + let mut h = [0.0; 4]; + for (i, bu) in nu.iter().enumerate() { + for (j, bv) in nvb.iter().enumerate() { + let Some(c) = self.net.get((su - self.p + i) * self.nv + (sv - self.q + j)) else { continue }; + for k in 0..4 { + h[k] += bu * bv * c[k]; + } + } + } + [h[0] / h[3], h[1] / h[3], h[2] / h[3]] + } + + /// The natural parameter box `[u0, u1, v0, v1]`. + pub fn domain(&self) -> [f64; 4] { + let f = |k: &[f64], first: bool| if first { k.first().copied() } else { k.last().copied() }.unwrap_or(f64::NAN); + [f(&self.knots_u, true), f(&self.knots_u, false), f(&self.knots_v, true), f(&self.knots_v, false)] + } +} diff --git a/crates/smlib/tests/ssi_score/polyset.rs b/crates/smlib/tests/ssi_score/polyset.rs new file mode 100644 index 0000000000..e9e0c8ad4a --- /dev/null +++ b/crates/smlib/tests/ssi_score/polyset.rs @@ -0,0 +1,350 @@ +//! Point-to-polyline distances and the topology signature: a port of `PolySet`, +//! `polyline_distance`, `components`, `free_ends` and `signature` of the corpus's +//! `scoring/score.py`, with the same segment hash (exact below the cell size, the nearest +//! decimated vertex above it), so the numbers the verdicts depend on are the same. + +use std::collections::HashMap; + +use super::geom::{V3, linspace, norm, py_max, py_min, sub}; + +/// numpy's `np.percentile(xs, q)` (linear interpolation). +fn percentile(xs: &[f64], q: f64) -> f64 { + let mut s = xs.to_vec(); + s.sort_by(f64::total_cmp); + let Some(&last) = s.last() else { return f64::NAN }; + let vi = (s.len() - 1) as f64 * (q / 100.0); + let lo = vi.floor(); + let i = lo as usize; + let a = s.get(i).copied().unwrap_or(last); + let b = s.get(i + 1).copied().unwrap_or(last); + let t = vi - lo; + let d = b - a; + if t >= 0.5 { b - d * (1.0 - t) } else { a + d * t } +} + +fn key(p: V3, cell: f64) -> [i64; 3] { + [(p[0] / cell).floor() as i64, (p[1] / cell).floor() as i64, (p[2] / cell).floor() as i64] +} + +/// A set of polylines and points, hashed by segment for nearest-distance queries. +pub struct PolySet { + a: Vec, + b: Vec, + cell: f64, + table: HashMap<[i64; 3], Vec>, + coarse: Vec, +} + +impl PolySet { + pub fn new(polylines: &[&[V3]], points: &[V3]) -> PolySet { + let (mut a, mut b) = (Vec::new(), Vec::new()); + for p in polylines { + if p.len() == 1 { + a.push(p[0]); + b.push(p[0]); + } + for w in p.windows(2) { + a.push(w[0]); + b.push(w[1]); + } + } + for &p in points { + a.push(p); + b.push(p); + } + let mut set = PolySet { a, b, cell: 0.0, table: HashMap::new(), coarse: Vec::new() }; + if set.a.is_empty() { + return set; + } + let lens: Vec = set.a.iter().zip(&set.b).map(|(&a, &b)| norm(sub(b, a))).collect(); + let lmax = lens.iter().copied().fold(f64::NEG_INFINITY, f64::max); + let mut cell = percentile(&lens, 95.0) * 2.0; + if 1e-300 > cell { + cell = 1e-300; + } + let alt = if lmax > 0.0 { lmax / 50.0 } else { cell }; + if alt > cell { + cell = alt; + } + set.cell = cell; + for (i, (&a, &b)) in set.a.iter().zip(&set.b).enumerate() { + let n = ((lens[i] / cell).ceil() as usize * 2).max(1); + let d = sub(b, a); + for t in linspace(0.0, 1.0, n + 1) { + let k = key([a[0] + t * d[0], a[1] + t * d[1], a[2] + t * d[2]], cell); + let e = set.table.entry(k).or_default(); + if e.last() != Some(&(i as u32)) { + e.push(i as u32); + } + } + } + let step = (set.a.len() / 4000).max(1); + set.coarse = set.a.iter().step_by(step).copied().collect(); + set + } + + pub fn is_empty(&self) -> bool { + self.a.is_empty() + } + + fn seg_dist(&self, q: V3, i: usize) -> f64 { + let (a, b) = (self.a[i], self.b[i]); + let d = sub(b, a); + let dd = d[0] * d[0] + d[1] * d[1] + d[2] * d[2]; + let qa = sub(q, a); + let t = if dd > 0.0 { (qa[0] * d[0] + qa[1] * d[1] + qa[2] * d[2]) / dd } else { 0.0 }; + let t = super::geom::clip(t, 0.0, 1.0); + norm(sub(q, [a[0] + t * d[0], a[1] + t * d[1], a[2] + t * d[2]])) + } + + /// Distance from one point to the set (infinite for an empty set). + pub fn query1(&self, q: V3) -> f64 { + if self.is_empty() { + return f64::INFINITY; + } + let k = key(q, self.cell); + let mut best = f64::INFINITY; + let mut any = false; + for i in -1..=1 { + for j in -1..=1 { + for l in -1..=1 { + if let Some(c) = self.table.get(&[k[0].wrapping_add(i), k[1].wrapping_add(j), k[2].wrapping_add(l)]) { + for &s in c { + let d = self.seg_dist(q, s as usize); + if !any || d < best { + best = d; + } + any = true; + } + } + } + } + } + if best > self.cell { + let m = self.coarse.iter().map(|&c| norm(sub(c, q))).fold(f64::INFINITY, f64::min); + best = py_min(best, m); + } + best + } + + pub fn query(&self, qs: &[V3]) -> Vec { + qs.iter().map(|&q| self.query1(q)).collect() + } +} + +/// Every `step`-th point (`P[::step]`). +pub fn stride(p: &[V3], step: usize) -> Vec { + p.iter().step_by(step.max(1)).copied().collect() +} + +/// The largest extent along x, y or z (`np.ptp(P, axis=0).max()`). +pub fn diameter(p: &[V3]) -> f64 { + if p.is_empty() { + return 0.0; + } + (0..3) + .map(|k| { + let lo = p.iter().map(|q| q[k]).fold(f64::INFINITY, f64::min); + let hi = p.iter().map(|q| q[k]).fold(f64::NEG_INFINITY, f64::max); + hi - lo + }) + .fold(f64::NEG_INFINITY, f64::max) +} + +/// Exact distance between segments p1q1 and p2q2. +pub fn seg_seg(p1: V3, q1: V3, p2: V3, q2: V3) -> f64 { + let dt = |x: V3, y: V3| x[0] * y[0] + x[1] * y[1] + x[2] * y[2]; + let (d1, d2, r) = (sub(q1, p1), sub(q2, p2), sub(p1, p2)); + let (a, e, f) = (dt(d1, d1), dt(d2, d2), dt(d2, r)); + if a <= 1e-300 && e <= 1e-300 { + return norm(r); + } + let cl = |x: f64| py_min(py_max(x, 0.0), 1.0); + let (s, t); + if a <= 1e-300 { + s = 0.0; + t = cl(f / e); + } else { + let c = dt(d1, r); + if e <= 1e-300 { + t = 0.0; + s = cl(-c / a); + } else { + let b = dt(d1, d2); + let den = a * e - b * b; + let s0 = if den > 1e-300 { cl((b * f - c * e) / den) } else { 0.0 }; + let t0 = (b * s0 + f) / e; + if t0 < 0.0 { + t = 0.0; + s = cl(-c / a); + } else if t0 > 1.0 { + t = 1.0; + s = cl((b - c) / a); + } else { + t = t0; + s = s0; + } + } + } + norm(sub([p1[0] + d1[0] * s, p1[1] + d1[1] * s, p1[2] + d1[2] * s], [p2[0] + d2[0] * t, p2[1] + d2[1] * t, p2[2] + d2[2] * t])) +} + +fn max_seg(p: &[V3]) -> f64 { + p.windows(2).map(|w| norm(sub(w[1], w[0]))).fold(0.0, |m, x| if x > m { x } else { m }) +} + +/// Min distance between polylines A and B: vertex query first, exact segment-segment distance +/// around the closest approach when it matters. +pub fn polyline_distance(a: &[V3], b: &[V3], set_b: &PolySet, tol: f64) -> f64 { + let dv = set_b.query(a); + let mut k = 0; + for (i, &d) in dv.iter().enumerate() { + if d.is_nan() { + k = i; + break; + } + if d < dv[k] { + k = i; + } + } + let Some(&d) = dv.get(k) else { return f64::INFINITY }; + if d <= tol { + return d; + } + let (sa, sb) = (max_seg(a), max_seg(b)); + if d > tol + sa.max(sb) { + return d; + } + let mut best = d; + let lim = tol + sa + sb; + // the nearest vertex of B to each vertex of A, computed once (np.argmin: the first minimum) + let mut nearest: Vec> = vec![None; a.len()]; + for (i, &di) in dv.iter().enumerate() { + if !(di <= lim) { + continue; + } + for ia in [i as isize - 1, i as isize] { + if ia < 0 || ia as usize + 1 >= a.len() { + continue; + } + let ia = ia as usize; + let jb = *nearest[ia].get_or_insert_with(|| { + let (mut jb, mut dj) = (0, f64::INFINITY); + for (j, &q) in b.iter().enumerate() { + let x = norm(sub(q, a[ia])); + if x.is_nan() { + jb = j; + break; + } + if j == 0 || x < dj { + dj = x; + jb = j; + } + } + jb + }); + for j in jb.saturating_sub(3)..(jb + 3).min(b.len().saturating_sub(1)) { + best = py_min(best, seg_seg(a[ia], a[ia + 1], b[j], b[j + 1])); + } + } + } + best +} + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum Item { + Curve(usize), + Point(usize), +} + +/// Connected components of curves and points joined within `join_tol`. +pub fn components(curves: &[Vec], points: &[V3], join_tol: f64) -> Vec> { + let items: Vec = (0..curves.len()).map(Item::Curve).chain((0..points.len()).map(Item::Point)).collect(); + let mut parent: Vec = (0..items.len()).collect(); + fn find(parent: &mut [usize], mut x: usize) -> usize { + while parent[x] != x { + parent[x] = parent[parent[x]]; + x = parent[x]; + } + x + } + let sets: Vec = curves.iter().map(|c| PolySet::new(&[c], &[])).collect(); + for i in 0..items.len() { + for j in i + 1..items.len() { + let d = match (items[i], items[j]) { + (Item::Point(a), Item::Point(b)) => norm(sub(points[a], points[b])), + (Item::Curve(a), Item::Curve(b)) => polyline_distance(&curves[a], &curves[b], &sets[b], join_tol), + (Item::Curve(c), Item::Point(p)) | (Item::Point(p), Item::Curve(c)) => sets[c].query1(points[p]), + }; + if d <= join_tol { + let (ri, rj) = (find(&mut parent, i), find(&mut parent, j)); + parent[ri] = rj; + } + } + } + let mut groups: Vec<(usize, Vec)> = Vec::new(); + for (i, &it) in items.iter().enumerate() { + let r = find(&mut parent, i); + match groups.iter_mut().find(|(g, _)| *g == r) { + Some((_, v)) => v.push(it), + None => groups.push((r, vec![it])), + } + } + groups.into_iter().map(|(_, v)| v).collect() +} + +/// Number of free endpoints of each curve. +pub fn free_ends(curves: &[Vec], join_tol: f64) -> Vec { + let sets: Vec = curves.iter().map(|c| PolySet::new(&[c], &[])).collect(); + let mut res = Vec::with_capacity(curves.len()); + for (i, c) in curves.iter().enumerate() { + let (Some(&first), Some(&last)) = (c.first(), c.last()) else { + res.push(0); + continue; + }; + if norm(sub(first, last)) <= join_tol && diameter(c) > 2.0 * join_tol { + res.push(0); + continue; + } + let mut s = Vec::with_capacity(c.len()); + let mut acc = 0.0; + s.push(0.0); + for w in c.windows(2) { + acc += norm(sub(w[1], w[0])); + s.push(acc); + } + let mut n = 0; + for (end, si) in [(first, 0.0), (last, acc)] { + let mut glued = (0..curves.len()).any(|j| j != i && sets[j].query1(end) <= join_tol); + if !glued { + let m = c + .iter() + .zip(&s) + .filter(|(_, sk)| (*sk - si).abs() > 3.0 * join_tol) + .map(|(&p, _)| norm(sub(p, end))) + .fold(None, |m: Option, x| Some(m.map_or(x, |m| m.min(x)))); + glued = m.is_some_and(|m| m <= join_tol); + } + if !glued { + n += 1; + } + } + res.push(n); + } + res +} + +/// The sorted component signature: `point` or `curve(free_ends=N)` per component. +pub fn signature(curves: &[Vec], points: &[V3], join_tol: f64) -> Vec { + let comps = components(curves, points, join_tol); + let fe = if curves.is_empty() { Vec::new() } else { free_ends(curves, join_tol) }; + let mut sig: Vec = comps + .iter() + .map(|comp| { + let cs: Vec = comp.iter().filter_map(|it| if let Item::Curve(k) = it { Some(*k) } else { None }).collect(); + if cs.is_empty() { "point".to_string() } else { format!("curve(free_ends={})", cs.iter().map(|&k| fe[k]).sum::()) } + }) + .collect(); + sig.sort(); + sig +}