diff --git a/crates/geom/src/ball.rs b/crates/geom/src/ball.rs new file mode 100644 index 0000000000..46db2293b9 --- /dev/null +++ b/crates/geom/src/ball.rs @@ -0,0 +1,232 @@ +//! The smallest ball holding a set of points (Welzl's algorithm): a measure of a point set's size +//! that does not move with the points, whatever rigid motion carries them. + +use crate::Vec3; + +/// A ball: every point within `radius` of `center`. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Ball { + pub center: Vec3, + pub radius: f64, +} + +/// How much more than `radius` a point may lie from the centre and still count as inside: the +/// rounding of the centre's and radius' arithmetic, so a point the ball was made through is never +/// taken for one outside it. +const ROUNDING: f64 = 1e-12; + +impl Ball { + /// The smallest ball holding every finite point of `points` (points that are not finite are + /// skipped), or `None` when there are none. Welzl's algorithm in its incremental form, the + /// points taken in a fixed shuffled order: expected `O(n)`. The ball is unique, so the order + /// only changes the last bits of its centre and radius. + pub fn enclosing(points: &[Vec3]) -> Option { + let mut p: Vec = points.iter().copied().filter(|q| q.is_finite()).collect(); + shuffle(&mut p); + let mut ball = Ball { center: *p.first()?, radius: 0.0 }; + // Each loop keeps the smallest ball holding the points before it with the points of the + // loops outside it on its boundary (Welzl 1991): at most four are, in three dimensions. + for i in 1..p.len() { + if ball.holds(p[i]) { + continue; + } + ball = Ball { center: p[i], radius: 0.0 }; + for j in 0..i { + if ball.holds(p[j]) { + continue; + } + ball = through(&[p[i], p[j]]); + for k in 0..j { + if ball.holds(p[k]) { + continue; + } + ball = through(&[p[i], p[j], p[k]]); + for l in 0..k { + if !ball.holds(p[l]) { + ball = through(&[p[i], p[j], p[k], p[l]]); + } + } + } + } + } + Some(ball) + } + + pub fn diameter(&self) -> f64 { + 2.0 * self.radius + } + + /// Whether `p` lies in the ball, to the rounding of its arithmetic. + pub fn holds(&self, p: Vec3) -> bool { + p.dist(self.center) <= self.radius + ROUNDING * (self.radius + self.center.len()) + } +} + +/// The smallest ball with every one of `r` (two to four points) on its boundary; where they are +/// degenerate (three in a line, four in a plane, which exact arithmetic never asks for but +/// rounding can), the smallest ball through some of them that holds them all. +fn through(r: &[Vec3]) -> Ball { + let exact = match r { + [a, b] => Some((*a + *b) * 0.5), + [o, b, c] => circumcentre3(*o, *b, *c), + [o, b, c, d] => circumcentre4(*o, *b, *c, *d), + _ => None, + }; + match exact { + Some(center) => with_center(center, r), + None => smallest_holding(r), + } +} + +/// The ball about `center` reaching the farthest of `r`. +fn with_center(center: Vec3, r: &[Vec3]) -> Ball { + Ball { center, radius: r.iter().map(|q| q.dist(center)).fold(0.0, f64::max) } +} + +/// The centre of the circle through `o`, `b` and `c`, or `None` when they are (nearly) in a line. +fn circumcentre3(o: Vec3, b: Vec3, c: Vec3) -> Option { + let (a, b) = (b - o, c - o); + let n = a.cross(b); + let n2 = n.len2(); + if !(n2 > 1e-14 * a.len2() * b.len2()) { + return None; + } + Some(o + (b.cross(n) * a.len2() + n.cross(a) * b.len2()) / (2.0 * n2)) +} + +/// The centre of the sphere through `o`, `b`, `c` and `d`, or `None` when they are (nearly) in a +/// plane. +fn circumcentre4(o: Vec3, b: Vec3, c: Vec3, d: Vec3) -> Option { + let (a, b, c) = (b - o, c - o, d - o); + let det = a.dot(b.cross(c)); + if !(det.abs() > 1e-7 * a.len() * b.len() * c.len()) { + return None; + } + Some(o + (b.cross(c) * a.len2() + c.cross(a) * b.len2() + a.cross(b) * c.len2()) / (2.0 * det)) +} + +/// The smallest of the balls through two or three of `r` that holds all of them; a ball about +/// their mean where none does (never, for two to four points). +fn smallest_holding(r: &[Vec3]) -> Ball { + let mut best: Option = None; + let mut consider = |ball: Ball| { + if r.iter().all(|q| ball.holds(*q)) && best.is_none_or(|b| ball.radius < b.radius) { + best = Some(ball); + } + }; + for i in 0..r.len() { + for j in i + 1..r.len() { + consider(with_center((r[i] + r[j]) * 0.5, r)); + for k in j + 1..r.len() { + if let Some(c) = circumcentre3(r[i], r[j], r[k]) { + consider(with_center(c, r)); + } + } + } + } + best.unwrap_or_else(|| { + let mean = r.iter().fold(Vec3::ZERO, |s, q| s + *q) / (r.len().max(1) as f64); + with_center(mean, r) + }) +} + +/// A Fisher–Yates shuffle from a fixed seed (SplitMix64), so the same points give the same ball. +fn shuffle(p: &mut [Vec3]) { + let mut state: u64 = 0x9E37_79B9_7F4A_7C15; + for i in (1..p.len()).rev() { + state = state.wrapping_add(0x9E37_79B9_7F4A_7C15); + let mut z = state; + z = (z ^ (z >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); + z = (z ^ (z >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); + z ^= z >> 31; + p.swap(i, (z % (i as u64 + 1)) as usize); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Points from a fixed generator in the cube [-1, 1]³. + fn cloud(n: usize, seed: u64) -> Vec { + let mut s = seed; + let mut next = || { + s = s.wrapping_mul(6364136223846793005).wrapping_add(1442695040888963407); + (s >> 11) as f64 / (1u64 << 53) as f64 * 2.0 - 1.0 + }; + (0..n).map(|_| Vec3::new(next(), next(), next())).collect() + } + + /// The smallest ball by brute force: of the balls through two, three or four of the points, + /// the smallest that holds them all. + fn brute(p: &[Vec3]) -> f64 { + let mut best = f64::INFINITY; + let n = p.len(); + let mut try_ball = |b: Ball| { + if p.iter().all(|q| q.dist(b.center) <= b.radius * (1.0 + 1e-9)) { + best = best.min(b.radius); + } + }; + for i in 0..n { + for j in i + 1..n { + try_ball(through(&[p[i], p[j]])); + for k in j + 1..n { + try_ball(through(&[p[i], p[j], p[k]])); + for l in k + 1..n { + try_ball(through(&[p[i], p[j], p[k], p[l]])); + } + } + } + } + best + } + + #[test] + fn the_smallest_ball_holds_every_point_and_matches_brute_force() { + for seed in 1..40 { + let p = cloud(3 + seed as usize % 9, seed); + let ball = Ball::enclosing(&p).unwrap(); + assert!(p.iter().all(|q| ball.holds(*q)), "seed {seed}: a point outside"); + let want = brute(&p); + assert!((ball.radius - want).abs() <= 1e-12 * want, "seed {seed}: {} against {want}", ball.radius); + } + } + + #[test] + fn known_balls() { + let cube: Vec = (0..8).map(|i| Vec3::new((i & 1) as f64, (i >> 1 & 1) as f64, (i >> 2 & 1) as f64)).collect(); + let b = Ball::enclosing(&cube).unwrap(); + assert!(b.center.dist(Vec3::new(0.5, 0.5, 0.5)) < 1e-15 && (b.radius - 3f64.sqrt() / 2.0).abs() < 1e-15); + let s = 3f64.sqrt(); + let triangle = [Vec3::new(1.0, 0.0, 0.0), Vec3::new(-0.5, s / 2.0, 0.0), Vec3::new(-0.5, -s / 2.0, 0.0)]; + assert!((Ball::enclosing(&triangle).unwrap().radius - 1.0).abs() < 1e-15); + // An obtuse triangle's ball is its longest side's. + let obtuse = [Vec3::new(-1.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0), Vec3::new(0.0, 0.1, 0.0)]; + assert!((Ball::enclosing(&obtuse).unwrap().radius - 1.0).abs() < 1e-15); + let tetra = [Vec3::new(1.0, 1.0, 1.0), Vec3::new(1.0, -1.0, -1.0), Vec3::new(-1.0, 1.0, -1.0), Vec3::new(-1.0, -1.0, 1.0)]; + assert!((Ball::enclosing(&tetra).unwrap().radius - s).abs() < 1e-15); + // Points in a line and in a plane, repeated points, one point, none. + let line: Vec = (0..10).map(|i| Vec3::new(i as f64, 2.0 * i as f64, 0.0)).collect(); + assert!((Ball::enclosing(&line).unwrap().diameter() - 9.0 * 5f64.sqrt()).abs() < 1e-12); + let square: Vec = (0..4).flat_map(|i| [Vec3::new((i & 1) as f64, (i >> 1) as f64, 0.0); 3]).collect(); + assert!((Ball::enclosing(&square).unwrap().diameter() - 2f64.sqrt()).abs() < 1e-15); + assert_eq!(Ball::enclosing(&[Vec3::X]), Some(Ball { center: Vec3::X, radius: 0.0 })); + assert_eq!(Ball::enclosing(&[]), None); + assert_eq!(Ball::enclosing(&[Vec3::new(f64::NAN, 0.0, 0.0)]), None); + } + + /// The ball's size is the set's own: a rigid motion leaves it, and points crowding one end + /// do not stretch it (twice the farthest point's distance from the centroid would be nearly + /// twice the diameter here). + #[test] + fn the_size_does_not_move_with_the_points_or_their_crowding() { + let p = cloud(200, 7); + let d = Ball::enclosing(&p).unwrap().diameter(); + let (c, s) = (0.7f64.cos(), 0.7f64.sin()); + let moved: Vec = p.iter().map(|q| Vec3::new(c * q.x - s * q.y + 30.0, s * q.x + c * q.y - 12.0, q.z + 5.0)).collect(); + assert!((Ball::enclosing(&moved).unwrap().diameter() - d).abs() <= 1e-12 * d); + let mut crowded = vec![Vec3::ZERO; 99]; + crowded.push(Vec3::new(1.0, 0.0, 0.0)); + assert!((Ball::enclosing(&crowded).unwrap().diameter() - 1.0).abs() < 1e-15); + } +} diff --git a/crates/geom/src/lib.rs b/crates/geom/src/lib.rs index 3c328146cd..622be959b2 100644 --- a/crates/geom/src/lib.rs +++ b/crates/geom/src/lib.rs @@ -11,6 +11,7 @@ mod aabb2; mod arcfit; +mod ball; pub mod blend; pub mod brep; pub mod bspline; @@ -40,6 +41,7 @@ mod vec; pub use aabb2::Aabb2; pub use arcfit::MAX_FIT_PIECES; +pub use ball::Ball; pub use blend::hermite_bezier; pub use bvh::{Bvh, MeshIndex}; pub use chain::{Chain, Closed, Ends, Open, Path, Planar, Ring}; diff --git a/crates/kernel/src/fill.rs b/crates/kernel/src/fill.rs index 4bf60a9fa8..003061f280 100644 --- a/crates/kernel/src/fill.rs +++ b/crates/kernel/src/fill.rs @@ -968,10 +968,23 @@ fn middle(vertices: &mut Vec<[f64; 3]>, made: &mut std::collections::HashMap<(us /// tolerance, since a planar face is exact and other operations treat it as containing its /// edges to that tolerance. A fitted face gets [`face_tolerance`], ten times looser. pub(crate) fn planarity_tolerance(body: &Body) -> f64 { - (body.size() * 1e-7).max(solidcraft_geom::LINEAR_TOL) + (placed_size(body) * 1e-7).max(solidcraft_geom::LINEAR_TOL) } -/// How far the fill's surface may be from the loop's edges: `10⁻⁶` of the body's size, at least +/// The body's size as the fill tolerances take it, the same wherever the body is and however it +/// is turned (the bounding box's diagonal is not: a cup turned 0.7 rad about (1, 2, 3) grows from +/// 54.7 to 61.6, and its fill was sampled to another tolerance): the diameter of the smallest ball +/// holding its vertices. (Twice the farthest vertex's distance from their centroid is not the +/// body's size where the vertices crowd one end: up to twice its diameter, and a rounded box's G2 +/// corner patch, its vertices crowding that corner, then fitted past the box.) A body with no +/// vertices takes its box's diagonal. +fn placed_size(body: &Body) -> f64 { + let vertices: Vec = body.record().vertices.iter().map(|v| Vec3::new(v.point[0], v.point[1], v.point[2])).collect(); + solidcraft_geom::Ball::enclosing(&vertices).map_or_else(|| body.size(), |ball| ball.diameter()) +} + +/// How far the fill's surface may be from the loop's edges: `10⁻⁶` of the body's size +/// ([`placed_size`]), at least /// `10⁻⁵` mm — ten times [`planarity_tolerance`]. Ten times the linear tolerance because a /// fitted surface approaches curved edges like a cubic spline, so each further factor of ten /// costs about twice the spans each way, and imported edges may already lie `10⁻⁵` mm or more off @@ -979,7 +992,7 @@ pub(crate) fn planarity_tolerance(body: &Body) -> f64 { /// tolerance, not the linear one: the face's edges are the body's exact edges, the surface /// between them is the fit (the patch record's edges and vertices hold this gap: [`hold_gap`]). pub(crate) fn face_tolerance(body: &Body) -> f64 { - (body.size() * 1e-6).max(1e-5) + (placed_size(body) * 1e-6).max(1e-5) } #[cfg(test)] @@ -1064,13 +1077,23 @@ mod tests { } /// Whether every edge of `made` is one of `of`'s own: the same kind of curve, on the same - /// curve record it is parameterised on (its NURBS form where it has one), on the same - /// interval. (A circle's or ellipse's frame is SMLib's again after a round trip, its axes - /// renormalised to the last bit.) + /// interval, on the curve it is parameterised on (its NURBS form where it has one) to + /// rounding: the same knots and control points within 10⁻¹² of their size. (SMLib makes a + /// conic's frame and NURBS form again on a round trip, the last bit of a coordinate apart.) fn own_edges(made: &Body, of: &Body) -> bool { let own = record(of).edges; let kind = |e: &b::Edge| std::mem::discriminant(&e.curve); - record(made).edges.iter().all(|e| own.iter().any(|o| kind(o) == kind(e) && o.parameters() == e.parameters() && o.interval == e.interval)) + let close = |p: &[f64], q: &[f64]| p.len() == q.len() && p.iter().zip(q).all(|(x, y)| (x - y).abs() <= 1e-12 * (1.0 + x.abs().max(y.abs()))); + let same = |c: &b::Curve, d: &b::Curve| match (c, d) { + (b::Curve::Spline { knots: k, points: p }, b::Curve::Spline { knots: l, points: q }) => { + k == l && p.len() == q.len() && p.iter().zip(q).all(|(x, y)| close(x, y)) + } + (b::Curve::RationalSpline { knots: k, homogeneous: p }, b::Curve::RationalSpline { knots: l, homogeneous: q }) => { + k == l && p.len() == q.len() && p.iter().zip(q).all(|(x, y)| close(x, y)) + } + _ => c == d, + }; + record(made).edges.iter().all(|e| own.iter().any(|o| kind(o) == kind(e) && same(o.parameters(), e.parameters()) && o.interval == e.interval)) } /// The fill's mesh: every normal finite, and no triangle folded over (all face the same way