diff --git a/crates/geom/src/fill.rs b/crates/geom/src/fill.rs index 1ce368fa0e..5590ec400e 100644 --- a/crates/geom/src/fill.rs +++ b/crates/geom/src/fill.rs @@ -166,10 +166,25 @@ enum Fit { Exhausted(f64), } -/// One round of [`fill_loop`]: fit the points `fit`, check every point of `all` (the loop in -/// order, `fit` among them). -fn fit_once(fit: &[Vec3], all: &[Vec3], tol: f64, crown: f64) -> SplineResult { +/// The plane a loop's height field stands on (see [`fill_loop`]): through the fitted points' +/// centroid `o`, normal `n` by Newell's method turned so that its largest component is positive, +/// first axis `x` towards the fitted point farthest from the centroid in the plane, `y = n × x`; +/// and whether the points run counter-clockwise about `n`. +#[derive(Clone, Copy, Debug)] +pub(crate) struct LoopFrame { + pub(crate) o: Vec3, + pub(crate) x: Vec3, + pub(crate) y: Vec3, + pub(crate) n: Vec3, + pub(crate) counter_clockwise: bool, +} + +/// The [`LoopFrame`] of the loop of points `fit`. +pub(crate) fn loop_frame(fit: &[Vec3]) -> SplineResult { let m = fit.len(); + if m < 3 { + return invalid("a loop needs three points"); + } let o = fit.iter().fold(Vec3::ZERO, |a, p| a + *p) / m as f64; let at = |k: usize| fit.get(k % m).copied().unwrap_or(Vec3::ZERO); let area = (0..m).fold(Vec3::ZERO, |a, k| a + (at(k) - o).cross(at(k + 1) - o)); @@ -206,6 +221,14 @@ fn fit_once(fit: &[Vec3], all: &[Vec3], tol: f64, crown: f64) -> SplineResult SplineResult { + let m = fit.len(); + let LoopFrame { o, x, y, n, counter_clockwise } = loop_frame(fit)?; let project = |pts: &[Vec3]| -> (Vec<[f64; 2]>, Vec) { (pts.iter().map(|p| [(*p - o).dot(x), (*p - o).dot(y)]).collect(), pts.iter().map(|p| (*p - o).dot(n)).collect()) }; @@ -495,7 +518,7 @@ fn segment_meets_rect(p: [f64; 2], q: [f64; 2], lo: [f64; 2], hi: [f64; 2]) -> b } /// Refuse a loop that does not bound a height field over its plane (see [`fill_loop`]). -fn check_projectable(points: &[Vec3], uv: &[[f64; 2]]) -> SplineResult<()> { +pub(crate) fn check_projectable(points: &[Vec3], uv: &[[f64; 2]]) -> SplineResult<()> { let m = uv.len(); let q = |k: usize| uv.get(k % m).copied().unwrap_or([0.0; 2]); let p = |k: usize| points.get(k % m).copied().unwrap_or(Vec3::ZERO); @@ -707,7 +730,7 @@ impl Grid { /// The surface of the net `heights` over the plane `o`, `x`, `y`, `n`: control point `(i, j)` /// at the knot averages `(ξᵢ, ηⱼ)` in the plane, `heights[j·count[0] + i]` along `n`. - fn surface(&self, heights: &[f64], o: Vec3, x: Vec3, y: Vec3, n: Vec3) -> SplineResult { + pub(crate) fn surface(&self, heights: &[f64], o: Vec3, x: Vec3, y: Vec3, n: Vec3) -> SplineResult { let [cu, cv] = self.count; let mut ctrl = Vec::with_capacity(cu * cv); for j in 0..cv { @@ -755,7 +778,7 @@ impl Band { } /// Solve `A·x = b` by banded Cholesky factorisation `A = Uᵀ·U`; an error when `A` is not /// positive definite. - fn solve(self, b: Vec) -> SplineResult> { + pub(crate) fn solve(self, b: Vec) -> SplineResult> { self.factor()?.substitute(b) } diff --git a/crates/geom/src/fill_matched.rs b/crates/geom/src/fill_matched.rs new file mode 100644 index 0000000000..a8b6e8c745 --- /dev/null +++ b/crates/geom/src/fill_matched.rs @@ -0,0 +1,504 @@ +//! Filling a closed loop with one height field that meets the faces beside the loop with G1 or +//! G2: the surface of [`crate::fill::fill_loop`] (a bicubic B-spline height field over the loop's +//! plane, so its parameters are plane coordinates and every edge's pcurve is the edge projected +//! onto that plane, exactly), fitted to the faces' derivatives as well as to the loop. +//! +//! The formulation (Piegl & Tiller, *The NURBS Book*, §9.4.1 least squares; do Carmo, +//! *Differential Geometry of Curves and Surfaces*, §3-3 for the second fundamental form of a +//! graph): +//! +//! - **The plane and grid** are [`crate::fill::fill_loop`]'s: the surface is +//! `S(u, v) = o + u·x + v·y + h(u, v)·n` with `h` a bicubic B-spline on a uniform grid over the +//! projected loop's box, the heights its unknowns. +//! - **The conditions** at each station (a point of the loop with the face beside it) are linear +//! in the heights, since every quantity of a graph is a derivative of `h`: +//! - G0: `h(u, v)` is the station's height. +//! - G1: the face's unit normal `N` (turned to `n`'s side) is the graph's when +//! `h_u = −N·x / N·n` and `h_v = −N·y / N·n`. +//! - G2: with G1, the surfaces share the curve and its tangent plane, so they agree to the +//! second order exactly when their normal curvatures across the curve agree: along the +//! direction `w` the face leaves the loop in (`a = w·x`, `b = w·y`), +//! `(a² h_uu + 2ab h_uv + b² h_vv) · N·n = κ`, `κ` the face's normal curvature along `w` +//! (its curvature vector `bend` against `N`). +//! +//! Each row is weighed to a length on the grid's scale (the G1 rows times a cell, the G2 rows +//! times a cell squared), and the heights minimise their squared misfit plus a small multiple +//! of the thin-plate energy ([`FAIRNESS`]), which decides what the conditions leave free. +//! - **At a corner** of the loop the two faces' conditions meet, and where their curvatures +//! across the corner differ (a round ending on a plane: the round's section has the round's +//! curvature where it reaches the plane, which is flat across its own curve) no surface is G2 +//! to both there: the surface's curvature is continuous, and has to pass from one face's to the +//! other's. A plane's G2 with G1 makes the surface flat to the second order all along its +//! curve, so the passage is a stiff one, and asked over a few cells it pulls the surface off +//! the loop along the whole side. The corner's own station holds the position only; the G2 +//! rows fade in over [`FADE_SHARE`] of the run from each corner to the next (a smoothstep of +//! the distance along the loop), a stretch of the loop's own, which finer grids then resolve, +//! and hold everywhere else. G1 rows fade likewise from a corner whose faces' tangent planes +//! disagree (`clash`), where no surface is G1 to both. +//! - **Grids** of 4 to [`MAX_MATCHED_SPANS`] spans across the box's longer side are tried in turn, +//! with denser sampling when a grid's cells are finer than twice the stations' step: the first +//! whose surface is within `tol` of every station, whose normal is within `angle_tol` of the +//! face's at every G1/G2 station clear of clashing corners, and whose normal curvature across +//! the loop is within `curvature_tol` of the face's at every G2 station clear of the corners' +//! fades, is the fill. Otherwise "not supported yet", with how close the best grid came. +//! +//! A loop that is not the boundary of a height field over its plane, or a face beside it that +//! stands along the plane's normal, is refused, never filled with wrong geometry. Input is +//! hostile: sizes are capped and every evaluation checked; nothing here panics. + +use crate::Vec3; +use crate::bspline::{SplineError, SplineResult, invalid}; +use crate::fill::{Band, Grid, LoopFill, LoopFrame, MAX_LOOP_POINTS, SMOOTHNESS, check_projectable, loop_frame}; +use crate::fill_general::Station; + +/// Most spans of the height field across the box's longer side. +pub const MAX_MATCHED_SPANS: usize = 128; +/// Fewest spans (the first grid tried). +const MIN_SPANS: usize = 4; +/// The share of the run from a corner to the next over which the conditions the corner cannot +/// hold fade in. +pub const FADE_SHARE: f64 = 0.15; +/// The thin-plate energy's weight per fitted station: a thousand times +/// [`crate::fill::fill_loop`]'s. Its fill only passes through points, which leave the inside to +/// the energy alone; here the slope and curvature rows also act a little way inside the loop, +/// and with the weaker energy they pull the surface up past a plane it is G2 to, a millimetre or +/// two in from the plane's curve (by 0.06 at a box corner's r5 rounds set back 8). This weight +/// keeps it under the plane but where it passes from a round's curvature to the plane's, and +/// costs the loop little (2e-5 mm, 0.025° and 4e-4 per mm there). +const FAIRNESS: f64 = 1e3 * SMOOTHNESS; +/// A face beside the loop whose normal is closer than this cosine to square with the plane's +/// stands along the plane's normal: the height field cannot be tangent to it. +const MIN_NORMAL_COS: f64 = 0.05; + +/// What [`fill_matched`] is asked for. +#[derive(Clone, Copy, Debug)] +pub struct MatchedSpec { + /// How far the surface may be from the loop. + pub tol: f64, + /// How far (radians) its normal may turn from a G1/G2 neighbour's. + pub angle_tol: f64, + /// How far (1/length) its normal curvature across the loop may be from a G2 neighbour's, + /// clear of the corners' fades. + pub curvature_tol: f64, +} + +/// The fill and what it achieved. +#[derive(Clone, Debug, PartialEq)] +pub struct MatchedFill { + /// The height field, its plane and its deviation from the loop (no crown). + pub fill: LoopFill, + /// The largest angle (radians) between the surface's normal and a neighbour's at the G1/G2 + /// stations held to the angle tolerance, and at any G1/G2 station. + pub normal_deviation: f64, + pub normal_deviation_all: f64, + /// The largest difference (1/length) between the surface's normal curvature across the + /// loop and the neighbour's, at the G2 stations clear of the corners' fades, and at any G2 + /// station. + pub curvature_mismatch: f64, + pub curvature_mismatch_all: f64, + /// Spans of the grid each way, and stations sampled. + pub spans: [usize; 2], + pub stations: usize, +} + +/// Fill the closed loop of `sides` sides with one height field meeting each station's face as +/// its `across` asks (see the module documentation). `stations(k, ts)` samples side `k` at the +/// parameters `ts` (0 at its start, which is where side `k − 1` ends, towards 1 at its end); a +/// station at `t = 0` marked `corner` is a corner of the loop. +pub fn fill_matched( + sides: usize, + stations: &mut dyn FnMut(usize, &[f64]) -> Result, String>, + spec: &MatchedSpec, +) -> SplineResult { + if sides == 0 || sides.saturating_mul(16) > MAX_LOOP_POINTS / 2 { + return Err(SplineError::TooLarge(format!("a loop of {sides} sides (at most {})", MAX_LOOP_POINTS / 32))); + } + let positive = |x: f64| x.is_finite() && x > 0.0; + if !(positive(spec.tol) && positive(spec.angle_tol) && positive(spec.curvature_tol)) { + return invalid("the fill tolerances must be positive"); + } + let mut sample = |ts: &[f64]| -> SplineResult>> { + (0..sides) + .map(|k| { + let got = stations(k, ts).map_err(SplineError::Invalid)?; + if got.len() == ts.len() && got.iter().all(finite_station) { + Ok(got) + } else { + Err(SplineError::Invalid(format!("side {k} has no finite stations"))) + } + }) + .collect() + }; + let mut per = 16usize; + let mut fit = sample(&(0..per).map(|i| i as f64 / per as f64).collect::>())?; + let mut best = Shortfall::default(); + let mut level = MIN_SPANS; + loop { + let check = sample(&(0..per).map(|i| (i as f64 + 0.5) / per as f64).collect::>())?; + // The loop in order: fitted and checked stations alternating, side by side. + let mut all: Vec<(Station, bool)> = Vec::with_capacity(2 * per * sides); + for (f, c) in fit.iter().zip(&check) { + for (a, b) in f.iter().zip(c) { + all.push((*a, true)); + all.push((*b, false)); + } + } + match fit_round(&all, spec, level)? { + Round::Done(fill) => return Ok(*fill), + Round::Denser(s, at) => { + best = best.min(s); + level = at; + } + Round::Exhausted(s) => { + best = best.min(s); + break; + } + } + if all.len() * 2 > MAX_LOOP_POINTS { + break; + } + fit = fit.iter().zip(&check).map(|(f, c)| f.iter().zip(c).flat_map(|(a, b)| [*a, *b]).collect()).collect(); + per *= 2; + } + Err(SplineError::Unsupported(format!( + "filling this loop with one surface meeting its neighbours: the closest fit stays {:e} off the loop (tolerance {:e}), turns {:.4}° from \ + its neighbours (tolerance {:.4}°) and misses their curvature by {:e} (tolerance {:e})", + best.deviation, + spec.tol, + best.angle.to_degrees(), + spec.angle_tol.to_degrees(), + best.curvature, + spec.curvature_tol + ))) +} + +fn finite_station(s: &Station) -> bool { + s.p.is_finite() && s.across.is_none_or(|a| a.leave.is_finite() && a.normal.is_finite() && a.bend.is_none_or(|b| b.is_finite())) +} + +/// How close a grid came. +#[derive(Clone, Copy, Debug)] +struct Shortfall { + deviation: f64, + angle: f64, + curvature: f64, +} + +impl Default for Shortfall { + fn default() -> Shortfall { + Shortfall { deviation: f64::INFINITY, angle: f64::INFINITY, curvature: f64::INFINITY } + } +} + +impl Shortfall { + /// The closer of two, by how many times its tolerance each misses. + fn min(self, other: Shortfall) -> Shortfall { + let worst = |s: &Shortfall| s.deviation + s.angle + s.curvature; + if worst(&other) < worst(&self) { other } else { self } + } +} + +enum Round { + Done(Box), + /// Denser sampling is needed for the grid of this level (the coarser ones fell short). + Denser(Shortfall, usize), + Exhausted(Shortfall), +} + +/// A station as the fit takes it: its place over the plane, its height, and what the face beside +/// it asks there, in the plane's terms. +struct Row { + q: [f64; 2], + h: f64, + fitted: bool, + /// G1: the height field's gradient there, and the face's unit normal on `n`'s side. + gradient: Option<([f64; 2], Vec3)>, + /// G2: the leave direction's plane components `(a, b)` and the second derivative of `h` + /// along them, `κ / N·n`, with `κ` itself. + curvature: Option<([f64; 2], f64, f64)>, + /// How near the station is to a corner, and to a clashing corner: its distance along the + /// loop to the nearer of the two around it, as a share of the run between them (0 at a + /// corner, ½ midway; 1 with no corner). + to_corner: f64, + to_clash: f64, + corner: bool, +} + +/// The stations in the plane `f`'s terms, with their distances to the corners along the loop. +fn rows(all: &[(Station, bool)], f: &LoopFrame) -> SplineResult> { + let m = all.len(); + // Arc length along the loop (chords), and where its corners are. + let mut s = vec![0.0f64; m + 1]; + for k in 0..m { + let (Some((a, _)), Some((b, _))) = (all.get(k), all.get((k + 1) % m)) else { return invalid("stations") }; + let next = s.get(k).copied().unwrap_or(0.0) + a.p.dist(b.p); + if let Some(slot) = s.get_mut(k + 1) { + *slot = next; + } + } + let total = s.get(m).copied().unwrap_or(0.0); + if !(total > 0.0 && total.is_finite()) { + return invalid("the loop has no length"); + } + let at = |k: usize| s.get(k).copied().unwrap_or(0.0); + let corners: Vec = (0..m).filter(|&k| all.get(k).is_some_and(|(st, _)| st.corner)).map(at).collect(); + let clashes: Vec = (0..m).filter(|&k| all.get(k).is_some_and(|(st, _)| st.clash)).map(at).collect(); + let share = |list: &[f64], x: f64| { + if list.is_empty() { + return 1.0; + } + let back = list.iter().map(|c| (x - c).rem_euclid(total)).fold(f64::INFINITY, f64::min); + let ahead = list.iter().map(|c| (c - x).rem_euclid(total)).filter(|d| *d > 0.0).fold(total, f64::min); + let run = back + ahead; + if run > 0.0 { back.min(ahead) / run } else { 0.0 } + }; + let mut out = Vec::with_capacity(m); + for (k, (st, fitted)) in all.iter().enumerate() { + let d = st.p - f.o; + let q = [d.dot(f.x), d.dot(f.y)]; + let mut gradient = None; + let mut curvature = None; + if let Some(a) = st.across { + let normal = a.normal.normalized().ok_or_else(|| SplineError::Invalid("a neighbour's normal has no direction".into()))?; + let side = if normal.dot(f.n) >= 0.0 { 1.0 } else { -1.0 }; + let nn = normal * side; + let c = nn.dot(f.n); + if !(c > MIN_NORMAL_COS) { + return Err(SplineError::Unsupported("filling a loop beside a face that stands along the fill's normal".into())); + } + gradient = Some(([-nn.dot(f.x) / c, -nn.dot(f.y) / c], nn)); + if let Some(bend) = a.bend { + let kappa = bend.dot(nn); + let w = a.leave; + curvature = Some(([w.dot(f.x), w.dot(f.y)], kappa / c, kappa)); + } + } + out.push(Row { + q, + h: d.dot(f.n), + fitted: *fitted, + gradient, + curvature, + to_corner: share(&corners, at(k)), + to_clash: share(&clashes, at(k)), + corner: st.corner, + }); + } + Ok(out) +} + +/// The smoothstep `3t² − 2t³` of `share / FADE_SHARE` (1 from [`FADE_SHARE`] on). +fn fade(share: f64) -> f64 { + let t = (share / FADE_SHARE).clamp(0.0, 1.0); + t * t * (3.0 - 2.0 * t) +} + +/// The height field's derivatives at `q`: `[h, h_u, h_v, h_uu, h_uv, h_vv]`. +fn derivatives(grid: &Grid, heights: &[f64], q: [f64; 2]) -> Option<[f64; 6]> { + let cu = *grid.count.first()?; + let mut out = [0.0; 6]; + for (slot, (ru, rv)) in out.iter_mut().zip([(0, 0), (1, 0), (0, 1), (2, 0), (1, 1), (0, 2)]) { + let (i0, bu) = grid.basis_derivative(0, q[0], ru)?; + let (j0, bv) = grid.basis_derivative(1, q[1], rv)?; + let mut s = 0.0; + for (b, wv) in bv.iter().enumerate() { + for (a, wu) in bu.iter().enumerate() { + s += wu * wv * heights.get((j0 + b) * cu + i0 + a)?; + } + } + *slot = s; + } + out.iter().all(|x| x.is_finite()).then_some(out) +} + +/// One grid's fit of the rows: the normal equations of the weighed rows and the energy. +fn solve(grid: &Grid, rows: &[Row], cell: f64) -> SplineResult> { + let [cu, cv] = grid.count; + let n = cu * cv; + let mut band = Band::new(n, 3 * cu + 3); + let mut rhs = vec![0.0; n]; + let add = |terms: &[(usize, f64)], value: f64, weight: f64, band: &mut Band, rhs: &mut Vec| { + let w2 = weight * weight; + for &(r, cr) in terms { + if let Some(slot) = rhs.get_mut(r) { + *slot += w2 * cr * value; + } + for &(s, cs) in terms { + band.add(r, s, w2 * cr * cs); + } + } + }; + let mut fitted = 0usize; + for row in rows.iter().filter(|r| r.fitted) { + let basis = |r: [usize; 2]| -> SplineResult<(usize, [f64; 4], usize, [f64; 4])> { + let (Some((i0, bu)), Some((j0, bv))) = (grid.basis_derivative(0, row.q[0], r[0]), grid.basis_derivative(1, row.q[1], r[1])) else { + return invalid("a loop point is off the grid"); + }; + Ok((i0, bu, j0, bv)) + }; + // The 16 control heights a derivative of order `r` at the station draws on, scaled. + let terms = |r: [usize; 2], scale: f64| -> SplineResult> { + let (i0, bu, j0, bv) = basis(r)?; + let mut t = Vec::with_capacity(16); + for (b, wv) in bv.iter().enumerate() { + for (a, wu) in bu.iter().enumerate() { + t.push(((j0 + b) * cu + i0 + a, wu * wv * scale)); + } + } + Ok(t) + }; + add(&terms([0, 0], 1.0)?, row.h, 1.0, &mut band, &mut rhs); + fitted += 1; + if row.corner { + continue; + } + if let Some((g, _)) = row.gradient { + let w = cell * fade(row.to_clash); + if w > 0.0 { + add(&terms([1, 0], 1.0)?, g[0], w, &mut band, &mut rhs); + add(&terms([0, 1], 1.0)?, g[1], w, &mut band, &mut rhs); + } + } + if let Some(([a, b], second, _)) = row.curvature { + let w = cell * cell * fade(row.to_corner) * fade(row.to_clash); + if w > 0.0 { + // a² h_uu + 2ab h_uv + b² h_vv, the three derivatives' terms summed per control + // height. + let mut sum: Vec<(usize, f64)> = Vec::with_capacity(48); + for (r, s) in [([2, 0], a * a), ([1, 1], 2.0 * a * b), ([0, 2], b * b)] { + for (idx, v) in terms(r, s)? { + match sum.iter_mut().find(|(i, _)| *i == idx) { + Some(slot) => slot.1 += v, + None => sum.push((idx, v)), + } + } + } + add(&sum, second, w, &mut band, &mut rhs); + } + } + } + let lambda = FAIRNESS * fitted as f64 / n as f64 * grid.cell(0) * grid.cell(1); + grid.add_energy(&mut band, lambda); + band.solve(rhs) +} + +/// What a fitted grid achieves at the rows: the deviation from the loop, the normal's angle to +/// the neighbours' (held stations, all), the curvature mismatch (held stations, all). +fn achieved(grid: &Grid, heights: &[f64], rows: &[Row], f: &LoopFrame) -> SplineResult<[f64; 5]> { + let reach = FADE_SHARE; + let (mut dev, mut ang, mut ang_all, mut cur, mut cur_all) = (0.0f64, 0.0f64, 0.0f64, 0.0f64, 0.0f64); + for row in rows { + let Some([h, hu, hv, huu, huv, hvv]) = derivatives(grid, heights, row.q) else { return invalid("the fit is not finite") }; + dev = dev.max((h - row.h).abs()); + if row.corner { + continue; + } + let up = (1.0 + hu * hu + hv * hv).sqrt(); + let normal = (f.n - f.x * hu - f.y * hv) * (1.0 / up); + if let Some((_, nn)) = row.gradient { + let a = normal.dot(nn).clamp(-1.0, 1.0).acos(); + ang_all = ang_all.max(a); + if row.to_clash >= reach { + ang = ang.max(a); + } + } + if let Some(([a, b], _, kappa)) = row.curvature { + // The normal curvature along the unit tangent of the surface over (a, b): the second + // fundamental form over the first. + let t = f.x * a + f.y * b + f.n * (a * hu + b * hv); + let ii = (a * a * huu + 2.0 * a * b * huv + b * b * hvv) / up; + let k = ii / t.len2().max(1e-300); + let miss = (k - kappa).abs(); + cur_all = cur_all.max(miss); + if row.to_corner >= reach && row.to_clash >= reach { + cur = cur.max(miss); + } + } + } + Ok([dev, ang, ang_all, cur, cur_all]) +} + +/// One round: the grids from `level` up, on these stations. +fn fit_round(all: &[(Station, bool)], spec: &MatchedSpec, level: usize) -> SplineResult { + let fitted: Vec = all.iter().filter(|(_, f)| *f).map(|(s, _)| s.p).collect(); + let frame = loop_frame(&fitted)?; + let points: Vec = all.iter().map(|(s, _)| s.p).collect(); + let rows = rows(all, &frame)?; + let uv: Vec<[f64; 2]> = rows.iter().map(|r| r.q).collect(); + check_projectable(&points, &uv)?; + let (mut lo, mut hi) = ([f64::INFINITY; 2], [f64::NEG_INFINITY; 2]); + for q in &uv { + for d in 0..2 { + lo[d] = lo[d].min(q[d]); + hi[d] = hi[d].max(q[d]); + } + } + for d in 0..2 { + let pad = 0.01 * (hi[d] - lo[d]); + lo[d] -= pad; + hi[d] += pad; + } + if !(lo[0] < hi[0] && lo[1] < hi[1] && (hi[0] - lo[0]).is_finite() && (hi[1] - lo[1]).is_finite()) { + return invalid("the loop's extent is not finite"); + } + // The fitted stations' largest step each way: a grid finer than half of it waits for denser + // sampling. + let fq: Vec<[f64; 2]> = rows.iter().filter(|r| r.fitted).map(|r| r.q).collect(); + let mut step = [0.0f64; 2]; + for k in 0..fq.len() { + let (p, q) = (fq.get(k).copied().unwrap_or([0.0; 2]), fq.get((k + 1) % fq.len()).copied().unwrap_or([0.0; 2])); + for d in 0..2 { + step[d] = step[d].max((q[d] - p[d]).abs()); + } + } + let extent = [hi[0] - lo[0], hi[1] - lo[1]]; + let longer = extent[0].max(extent[1]); + let mut best = Shortfall::default(); + let mut level = level; + while level <= MAX_MATCHED_SPANS { + let spans = [0, 1].map(|d| ((extent[d] / longer * level as f64).ceil() as usize).clamp(1, level)); + let holds = (0..2).all(|d| extent[d] / spans[d] as f64 >= 2.0 * step[d]); + if !holds && level > MIN_SPANS { + return Ok(Round::Denser(best, level)); + } + let grid = Grid::new(spans, lo, hi); + let cell = grid.cell(0).max(grid.cell(1)); + let heights = solve(&grid, &rows, cell)?; + let [dev, ang, ang_all, cur, cur_all] = achieved(&grid, &heights, &rows, &frame)?; + let s = Shortfall { deviation: dev, angle: ang, curvature: cur }; + best = best.min(s); + if dev <= spec.tol && ang <= spec.angle_tol && cur <= spec.curvature_tol && holds { + let surface = grid.surface(&heights, frame.o, frame.x, frame.y, frame.n)?; + let fill = LoopFill { + surface, + origin: frame.o, + axes: [frame.x, frame.y], + normal: frame.n, + counter_clockwise: frame.counter_clockwise, + deviation: dev, + evaluations: all.len(), + crown: 0.0, + crowned: 0, + crown_drift: 0.0, + }; + return Ok(Round::Done(Box::new(MatchedFill { + fill, + normal_deviation: ang, + normal_deviation_all: ang_all, + curvature_mismatch: cur, + curvature_mismatch_all: cur_all, + spans, + stations: all.len(), + }))); + } + if !holds { + return Ok(Round::Denser(best, level)); + } + level *= 2; + } + Ok(Round::Exhausted(best)) +} + +#[cfg(test)] +#[path = "fill_matched_tests.rs"] +mod tests; diff --git a/crates/geom/src/fill_matched_tests.rs b/crates/geom/src/fill_matched_tests.rs new file mode 100644 index 0000000000..527631802f --- /dev/null +++ b/crates/geom/src/fill_matched_tests.rs @@ -0,0 +1,181 @@ +use super::*; +use crate::fill_general::Across; +use std::f64::consts::FRAC_PI_2; + +/// The face tolerance of the 60 x 40 x 20 box the corner is cut from (10⁻⁶ of its size). +const TOL: f64 = 7.5e-5; +const ANGLE: f64 = 0.05 * std::f64::consts::PI / 180.0; +/// Half a percent of the 1/5 per mm a rolling ball of radius 5 jumps by across its seams. +const CURVATURE: f64 = 1e-3; + +fn spec() -> MatchedSpec { + MatchedSpec { tol: TOL, angle_tol: ANGLE, curvature_tol: CURVATURE } +} + +/// The hole a setback leaves at the corner of three rounds of radius `r` set back `s` (> r) +/// along each edge of a corner at the origin, the solid below it in every axis: three sections +/// of the rounds (quarter circles on the cylinders about the three edges) and three curves on the +/// faces between their ends, each a quarter circle about the point `s` back along both edges +/// (radius `s − r`), meeting the rounds' contact lines tangentially. Each side is G2 to its face: +/// a round leaves its section along its axis, where it is straight, a face anywhere; both have no +/// curvature across the hole. +fn setback_hole(r: f64, s: f64, g2: bool) -> impl FnMut(usize, &[f64]) -> Result, String> { + let rho = s - r; + move |k, ts| { + ts.iter() + .map(|t| { + let a = t * FRAC_PI_2; + let (sin, cos) = a.sin_cos(); + let (p, normal, leave) = match k { + 0 => (Vec3::new(-s, -r + r * sin, -r + r * cos), Vec3::new(0.0, sin, cos), Vec3::X), + 1 => (Vec3::new(-s + rho * sin, 0.0, -s + rho * cos), Vec3::Y, Vec3::new(sin, 0.0, cos)), + 2 => (Vec3::new(-r + r * sin, -r + r * cos, -s), Vec3::new(sin, cos, 0.0), Vec3::Z), + 3 => (Vec3::new(0.0, -s + rho * cos, -s + rho * sin), Vec3::X, Vec3::new(0.0, cos, sin)), + 4 => (Vec3::new(-r + r * cos, -s, -r + r * sin), Vec3::new(cos, 0.0, sin), Vec3::Y), + 5 => (Vec3::new(-s + rho * cos, -s + rho * sin, 0.0), Vec3::Z, Vec3::new(cos, sin, 0.0)), + _ => return Err(format!("no side {k}")), + }; + let up = Vec3::new(1.0, 1.0, 1.0).normalized().unwrap_or(Vec3::Z); + Ok(Station { p, up, across: Some(Across { leave, normal, bend: g2.then_some(Vec3::ZERO) }), corner: *t == 0.0, clash: false }) + }) + .collect() + } +} + +#[test] +fn a_setback_corner_hole_is_filled_g2_to_its_rounds_and_faces() { + let f = fill_matched(6, &mut setback_hole(5.0, 8.0, true), &spec()).unwrap(); + eprintln!( + "setback hole: spans {:?}, {} stations, deviation {:e}, normal {:.5}° (all {:.5}°), curvature {:e} (all {:e})", + f.spans, + f.stations, + f.fill.deviation, + f.normal_deviation.to_degrees(), + f.normal_deviation_all.to_degrees(), + f.curvature_mismatch, + f.curvature_mismatch_all + ); + assert!(f.fill.deviation <= TOL && f.normal_deviation <= ANGLE && f.curvature_mismatch <= CURVATURE); + // The plane is the corner's diagonal one, by symmetry. + let d = Vec3::new(1.0, 1.0, 1.0).normalized().unwrap(); + assert!(f.fill.normal.dot(d) > 1.0 - 1e-9, "{:?}", f.fill.normal); + // The patch is the corner's cap: inside the corner cube's octant and within the hull of the + // three rounds' tangent planes at 45° (the box's corner cut back by the rounds). + let [du, dv] = f.fill.surface.domain(); + let mut highest = f64::NEG_INFINITY; + for i in 0..=40 { + for j in 0..=40 { + let u = (du[0] + (du[1] - du[0]) * i as f64 / 40.0).min(du[1]); + let v = (dv[0] + (dv[1] - dv[0]) * j as f64 / 40.0).min(dv[1]); + let p = f.fill.surface.eval(u, v).unwrap(); + // Over the loop only: the box's corners outside the hole are not the patch. + if p.x > -8.0 && p.y > -8.0 && p.z > -8.0 && p.x < 0.0 && p.y < 0.0 && p.z < 0.0 { + highest = highest.max(p.dot(d)); + } + } + } + eprintln!("setback hole: highest point along the diagonal {highest:.6}"); + assert!(highest < 0.0); +} + +#[test] +fn a_setback_corner_hole_is_filled_g1_on_a_coarser_grid() { + let f = fill_matched(6, &mut setback_hole(5.0, 8.0, false), &spec()).unwrap(); + eprintln!("setback hole G1: spans {:?}, deviation {:e}, normal {:.5}°", f.spans, f.fill.deviation, f.normal_deviation.to_degrees()); + assert!(f.fill.deviation <= TOL && f.normal_deviation <= ANGLE); + // Flat to the second order along a curved plane's curve is what makes G2 the harder fill. + let g2 = fill_matched(6, &mut setback_hole(5.0, 8.0, true), &spec()).unwrap(); + assert!(f.spans[0] < g2.spans[0], "{:?} {:?}", f.spans, g2.spans); +} + +/// A hole in a cylinder of radius 5 about the x axis (its top line through the origin), the rim +/// over the circle of radius 2 about the z axis, G2 all round: the loop has no corner, and the +/// fill repeats the cylinder there. +#[test] +fn a_hole_in_a_cylinder_is_filled_with_the_cylinder_to_the_second_order() { + let r = 5.0; + let mut rim = move |_: usize, ts: &[f64]| -> Result, String> { + ts.iter() + .map(|t| { + let a = t * std::f64::consts::TAU; + let (x, y) = (2.0 * a.cos(), 2.0 * a.sin()); + let z = (r * r - y * y).sqrt() - r; + let p = Vec3::new(x, y, z); + let normal = Vec3::new(0.0, y, z + r).normalized().ok_or("normal")?; + let tangent = Vec3::new(-a.sin(), a.cos(), -a.cos() * y / (z + r)).normalized().ok_or("tangent")?; + // Into the hole, square to the rim in the cylinder. + let leave = tangent.cross(normal).normalized().ok_or("leave")?; + let leave = if leave.dot(Vec3::new(-x, -y, 0.0)) > 0.0 { leave } else { leave * -1.0 }; + // The cylinder's normal curvature along `leave`: its part across the axis, squared, + // over the radius, bending away from the normal. + let across = leave - Vec3::X * leave.x; + let bend = normal * (-across.len2() / r); + Ok(Station { p, up: normal, across: Some(Across { leave, normal, bend: Some(bend) }), corner: false, clash: false }) + }) + .collect() + }; + let f = fill_matched(1, &mut rim, &spec()).unwrap(); + eprintln!("cylinder hole: spans {:?}, deviation {:e}, curvature {:e}", f.spans, f.fill.deviation, f.curvature_mismatch_all); + assert!(f.fill.deviation <= TOL && f.normal_deviation_all <= ANGLE && f.curvature_mismatch_all <= CURVATURE); +} + +#[test] +fn hostile_matched_fills_are_refused() { + let bad = MatchedSpec { tol: 0.0, ..spec() }; + assert!(fill_matched(6, &mut setback_hole(5.0, 8.0, true), &bad).is_err()); + assert!(fill_matched(0, &mut setback_hole(5.0, 8.0, true), &spec()).is_err()); + // A face beside the loop standing along the fill's normal. + let mut wall = |_: usize, ts: &[f64]| -> Result, String> { + Ok(ts + .iter() + .map(|t| { + let a = t * std::f64::consts::TAU / 3.0; + let p = Vec3::new(a.cos(), a.sin(), 0.0); + Station { p, up: Vec3::Z, across: Some(Across { leave: Vec3::Z, normal: p, bend: None }), corner: false, clash: false } + }) + .collect()) + }; + assert!(matches!(fill_matched(3, &mut wall, &spec()), Err(SplineError::Unsupported(_)))); +} + +/// How far the fill of `setback_hole(r, s)` rises past the planes it is G2 to, over the loop +/// (the corner is convex: all of it should lie under them), sampled. +fn over_the_planes(f: &MatchedFill, r: f64, s: f64) -> f64 { + let mut st = setback_hole(r, s, true); + let ts: Vec = (0..256).map(|i| i as f64 / 256.0).collect(); + let poly: Vec<[f64; 2]> = (0..6) + .flat_map(|k| st(k, &ts).unwrap()) + .map(|q| { + let d = q.p - f.fill.origin; + [d.dot(f.fill.axes[0]), d.dot(f.fill.axes[1])] + }) + .collect(); + let [du, dv] = f.fill.surface.domain(); + let n = 200; + let mut over = f64::NEG_INFINITY; + for i in 0..=n { + for j in 0..=n { + let u = (du[0] + (du[1] - du[0]) * i as f64 / n as f64).min(du[1]); + let v = (dv[0] + (dv[1] - dv[0]) * j as f64 / n as f64).min(dv[1]); + if crate::fill::inside_loop([u, v], &poly) { + let p = f.fill.surface.eval(u, v).unwrap(); + over = over.max(p.x).max(p.y).max(p.z); + } + } + } + over +} + +/// Set back from 1.5 to 3 times the radius the G2 fill stays under the planes it meets, but by +/// under a thousandth where it passes from a round's curvature to the plane's at the hole's +/// corners (SMLib's own setback patch rises past them beyond about twice the radius, by 0.002 at +/// 2.2 r and 0.25 at 3 r). +#[test] +fn a_setback_g2_fill_stays_under_its_planes() { + for s in [7.5, 8.0, 10.0, 12.0, 15.0] { + let f = fill_matched(6, &mut setback_hole(5.0, s, true), &spec()).unwrap(); + let over = over_the_planes(&f, 5.0, s); + eprintln!("set back {s}: spans {:?}, curvature {:.2e}, over the planes {over:.2e}", f.spans, f.curvature_mismatch); + assert!(over < 1e-3, "set back {s}: {over}"); + } +} diff --git a/crates/geom/src/lib.rs b/crates/geom/src/lib.rs index fdd234a4b9..c1b3bc7c3a 100644 --- a/crates/geom/src/lib.rs +++ b/crates/geom/src/lib.rs @@ -22,6 +22,7 @@ pub mod env; pub mod fill; pub mod fill_general; pub mod fill_iso; +pub mod fill_matched; mod grid; mod mesh; pub mod mesh_distance;