diff --git a/crates/geom/src/brep.rs b/crates/geom/src/brep.rs index df6f33e8a..e29dacc66 100644 --- a/crates/geom/src/brep.rs +++ b/crates/geom/src/brep.rs @@ -15,6 +15,7 @@ pub mod eval; pub mod form; pub mod geometry; pub mod history; +pub mod normal_section; pub mod part; pub mod pcurve; pub mod placement; diff --git a/crates/geom/src/brep/eval.rs b/crates/geom/src/brep/eval.rs index 73b6260cc..e4cf59bdd 100644 --- a/crates/geom/src/brep/eval.rs +++ b/crates/geom/src/brep/eval.rs @@ -12,6 +12,7 @@ use super::pcurve::Error; use super::{Curve, Frame, Surface}; use crate::Vec3; use crate::bspline::MAX_SPLINE_DEGREE; +use crate::tess::partials::{Partials, tabulate, trig_derivative}; use crate::tess::uv::{self, Domain}; use crate::tess::{EvalCurve, EvalSurface, MAX_EVAL_KNOTS, MAX_EVAL_POINTS}; use std::f64::consts::{FRAC_PI_2, TAU}; @@ -202,6 +203,53 @@ impl SurfaceEval { }) } + /// The partial derivatives at (u, v) up to the fourth order ([`Partials`]), exactly: in closed + /// form on the analytic surfaces (angles differentiate as `cos(t + kπ/2)`), on the display + /// evaluator otherwise ([`EvalSurface::partials`]). + pub fn partials(&self, u: f64, v: f64) -> Option { + if !(u.is_finite() && v.is_finite()) { + return None; + } + // The i-th derivative of the radial direction at angle u, and the k-th of cos and sin at v. + let radial = |a: &Axes, i: usize| { + let (c, s) = trig_derivative(u, i); + a.x * c + a.y * s + }; + let at_v = |k: usize| trig_derivative(v, k); + let lift = |a: &Axes, i: usize, j: usize, x: Vec3| if i == 0 && j == 0 { a.o + x } else { x }; + match self { + SurfaceEval::Eval(e, _) => e.partials(u, v), + SurfaceEval::Cylinder { axes: a, radius } => tabulate(|i, j| { + let height = match j { + 0 if i == 0 => a.z * v, + 1 if i == 0 => a.z, + _ => Vec3::ZERO, + }; + let ring = if j == 0 { radial(a, i) * *radius } else { Vec3::ZERO }; + Some(lift(a, i, j, ring + height)) + }), + SurfaceEval::Cone { axes: a, radius_at_origin, slope } => tabulate(|i, j| { + let (height, rv) = match j { + 0 => (if i == 0 { a.z * v } else { Vec3::ZERO }, radius_at_origin + v * slope), + 1 => (if i == 0 { a.z } else { Vec3::ZERO }, *slope), + _ => (Vec3::ZERO, 0.0), + }; + Some(lift(a, i, j, radial(a, i) * rv + height)) + }), + SurfaceEval::Sphere { axes: a, radius } => tabulate(|i, j| { + let (c, s) = at_v(j); + let pole = if i == 0 { a.z * s } else { Vec3::ZERO }; + Some(lift(a, i, j, (radial(a, i) * c + pole) * *radius)) + }), + SurfaceEval::Torus { axes: a, major, minor } => tabulate(|i, j| { + let (c, s) = at_v(j); + let ring = if j == 0 { major + minor * c } else { minor * c }; + let tube = if i == 0 { a.z * (minor * s) } else { Vec3::ZERO }; + Some(lift(a, i, j, radial(a, i) * ring + tube)) + }), + } + } + /// The parameters of `p`, a point within `accept` of the surface, from `near` when given /// (not unwrapped to it). pub fn project(&self, p: Vec3, near: Option<[f64; 2]>, accept: f64) -> Option<[f64; 2]> { diff --git a/crates/geom/src/brep/normal_section.rs b/crates/geom/src/brep/normal_section.rs new file mode 100644 index 000000000..c9221bcca --- /dev/null +++ b/crates/geom/src/brep/normal_section.rs @@ -0,0 +1,187 @@ +//! The curvature of a surface's normal section and its first two derivatives by arc length, from +//! the surface's exact partial derivatives ([`Partials`]): what a seam's G2, G3 and G4 compare. +//! +//! At a point `p` with unit normal `n`, the normal section along a unit tangent direction `w` is +//! the curve the plane through `p` spanned by `n` and `w` cuts from the surface. Written as a +//! height over that plane's `w` axis, `h(x) = (r − p) · n` with `x = (r − p) · w`, it starts flat +//! (`h(0) = h′(0) = 0`), and its signed curvature `κ = h″ / (1 + h′²)^(3/2)` has, at `p`, +//! +//! - `κ = h″` (the normal curvature in direction `w`: `n · r″`), +//! - `dκ/ds = h‴` (a curvature that changes along `w`: the G3 quantity), +//! - `d²κ/ds² = h⁗ − 3 h″³` (the G4 quantity). +//! +//! The height's Taylor coefficients come from the surface's: the section is the curve +//! `(u(x), v(x))` on which `(r − p) · w = x` and `(r − p) · t = 0` (`t = w × n`, the cutting +//! plane's normal). Expanding `r` about `p` in its partials and solving those two conditions order +//! by order gives each coefficient of `u` and `v` from the 2 × 2 system of the first partials +//! (the lower orders' terms on the right), and the height's coefficient is the normal part of the +//! same expansion. Nothing is sampled: the answer is exact up to rounding. + +use crate::Vec3; +use crate::tess::partials::{MAX_PARTIAL_ORDER, Partials, orders}; + +/// Taylor coefficients in `x` up to the fourth power (index = power). +type Series = [T; MAX_PARTIAL_ORDER + 1]; + +/// The normal section's curvature and its first two arc-length derivatives at a point. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct SectionJet { + /// `κ = n · r″`, 1/mm: negative where the surface bends away from `n`. + pub curvature: f64, + /// `dκ/ds` along the direction, 1/mm². + pub curvature_change: f64, + /// `d²κ/ds²` along the direction, 1/mm³. + pub curvature_change_rate: f64, +} + +impl SectionJet { + /// The three values in order: curvature, its change, the change's rate. + pub fn values(&self) -> [f64; 3] { + [self.curvature, self.curvature_change, self.curvature_change_rate] + } +} + +/// The normal section at the point of `d` (`d[0][0]`) with unit normal `normal`, along the unit +/// tangent `direction`. `None` where the first partials do not span the tangent plane across the +/// section (a pole, a degenerate point) or a value is not finite. +pub fn normal_section(d: &Partials, normal: Vec3, direction: Vec3) -> Option { + let cut = direction.cross(normal); + let (ru, rv) = (d[1][0], d[0][1]); + // [ru·w rv·w; ru·t rv·t] [a; b] = rhs, solved by Cramer's rule. + let (m11, m12, m21, m22) = (ru.dot(direction), rv.dot(direction), ru.dot(cut), rv.dot(cut)); + let det = m11 * m22 - m12 * m21; + let scale = ru.len() * rv.len(); + if !(det.abs() > 1e-12 * scale && scale.is_finite()) { + return None; + } + let mut du: Series = [0.0; MAX_PARTIAL_ORDER + 1]; + let mut dv: Series = [0.0; MAX_PARTIAL_ORDER + 1]; + let mut height: Series = [0.0; MAX_PARTIAL_ORDER + 1]; + for k in 1..=MAX_PARTIAL_ORDER { + // The terms of order two and up, which only reach x^k through the coefficients below k. + let rest = *nonlinear(d, &du, &dv).get(k)?; + let target = if k == 1 { 1.0 } else { 0.0 }; + let (r1, r2) = (target - rest.dot(direction), -rest.dot(cut)); + *du.get_mut(k)? = (r1 * m22 - m12 * r2) / det; + *dv.get_mut(k)? = (m11 * r2 - r1 * m21) / det; + *height.get_mut(k)? = rest.dot(normal); + } + let [_, _, h2, h3, h4] = height; + let (curvature, change) = (2.0 * h2, 6.0 * h3); + let rate = 24.0 * h4 - 3.0 * curvature.powi(3); + let jet = SectionJet { curvature, curvature_change: change, curvature_change_rate: rate }; + jet.values().iter().all(|x| x.is_finite()).then_some(jet) +} + +/// `Σ_{2 ≤ i+j ≤ 4} d[i][j] du^i dv^j / (i! j!)`, as a series in `x`. +fn nonlinear(d: &Partials, du: &Series, dv: &Series) -> Series { + let pu = powers(du); + let pv = powers(dv); + let mut out: Series = [Vec3::ZERO; MAX_PARTIAL_ORDER + 1]; + for (i, j) in orders().filter(|(i, j)| i + j >= 2) { + let term = times(&pu[i], &pv[j]); + let weight = 1.0 / (factorial(i) * factorial(j)); + for (o, t) in out.iter_mut().zip(term) { + *o = *o + d[i][j] * (t * weight); + } + } + out +} + +/// `s^0 … s^4`, truncated to the fourth power of `x`. +fn powers(s: &Series) -> [Series; MAX_PARTIAL_ORDER + 1] { + let mut out = [[0.0; MAX_PARTIAL_ORDER + 1]; MAX_PARTIAL_ORDER + 1]; + out[0][0] = 1.0; + for k in 1..=MAX_PARTIAL_ORDER { + out[k] = times(&out[k - 1], s); + } + out +} + +/// The product of two series, truncated to the fourth power. +fn times(a: &Series, b: &Series) -> Series { + let mut out = [0.0; MAX_PARTIAL_ORDER + 1]; + for (i, x) in a.iter().enumerate() { + for (j, y) in b.iter().enumerate().take(MAX_PARTIAL_ORDER + 1 - i) { + out[i + j] += x * y; + } + } + out +} + +fn factorial(n: usize) -> f64 { + (1..=n).map(|k| k as f64).product() +} + +#[cfg(test)] +#[allow(clippy::unwrap_used)] +mod tests { + use super::*; + use crate::brep::eval::SurfaceEval; + use crate::brep::{Frame, Surface}; + + fn frame() -> Frame { + Frame { origin: [1., 2., 3.], x_axis: [0., 1., 0.], y_axis: [0., 0., 1.] } + } + + fn jet(s: &Surface, uv: [f64; 2], along: impl Fn(Vec3, Vec3) -> Vec3) -> SectionJet { + let e = SurfaceEval::new(s).unwrap(); + let d = e.partials(uv[0], uv[1]).unwrap(); + let n = d[1][0].cross(d[0][1]).normalized().unwrap(); + let w = along(d[1][0].normalized().unwrap(), d[0][1].normalized().unwrap()); + normal_section(&d, n, w).unwrap() + } + + fn near(a: f64, b: f64) -> bool { + (a - b).abs() < 1e-9 * (1.0 + b.abs()) + } + + /// A sphere's normal sections are great circles: curvature 1/R (towards the centre, against + /// the outward normal), constant, in every direction. + #[test] + fn a_sphere_is_a_constant_curvature_every_way() { + let s = Surface::Sphere { frame: frame(), radius: 4.0 }; + for along in [|u: Vec3, _: Vec3| u, |_: Vec3, v: Vec3| v, |u: Vec3, v: Vec3| (u + v).normalized().unwrap_or(u)] { + let j = jet(&s, [0.7, 0.3], along); + assert!(near(j.curvature, -0.25) && near(j.curvature_change, 0.0) && near(j.curvature_change_rate, 0.0), "{j:?}"); + } + } + + /// A cylinder across its axis: a circle (-1/R, constant); along it: a straight line. + #[test] + fn a_cylinder_is_a_circle_across_and_a_line_along() { + let s = Surface::Cylinder { frame: frame(), radius: 5.0 }; + let across = jet(&s, [1.1, 2.0], |u, _| u); + assert!(near(across.curvature, -0.2) && near(across.curvature_change, 0.0) && near(across.curvature_change_rate, 0.0), "{across:?}"); + let along = jet(&s, [1.1, 2.0], |_, v| v); + assert!(along.values().iter().all(|x| x.abs() < 1e-12), "{along:?}"); + } + + /// The graph `z = x³/6 + x⁴/12`, an exact quartic Bézier in x and straight in y, has h″ = 0, + /// h‴ = 1 and h⁗ = 2 at the origin: curvature 0, its change 1 and the change's rate 2 along x. + #[test] + fn a_polynomial_graph_reads_its_own_derivatives() { + let z = |x: f64| x.powi(3) / 6.0 + x.powi(4) / 12.0; + // Monomial coefficients a0..a4 = [0, 0, 0, 1/6, 1/12]; Bézier b_i = Σ_k C(i,k)/C(4,k) a_k. + let a = [0.0, 0.0, 0.0, 1.0 / 6.0, 1.0 / 12.0]; + let c = |n: usize, k: usize| crate::bspline::binomial(n, k); + let b: Vec = (0..=4).map(|i| (0..=i).map(|k| c(i, k) / c(4, k) * a[k]).sum()).collect(); + let mut points = Vec::new(); + for (i, bi) in b.iter().enumerate() { + points.push(vec![[i as f64 / 4.0, 0.0, *bi], [i as f64 / 4.0, 1.0, *bi]]); + } + let s = Surface::Spline { u_knots: vec![0., 0., 0., 0., 0., 1., 1., 1., 1., 1.], v_knots: vec![0., 0., 1., 1.], points }; + let e = SurfaceEval::new(&s).unwrap(); + assert!(near(e.point([0.5, 0.5]).unwrap().z, z(0.5))); + let d = e.partials(0.0, 0.5).unwrap(); + let j = normal_section(&d, Vec3::Z, Vec3::X).unwrap(); + assert!(near(j.curvature, 0.0) && near(j.curvature_change, 1.0) && near(j.curvature_change_rate, 2.0), "{j:?}"); + } + + #[test] + fn a_degenerate_point_has_no_section() { + let mut d: Partials = [[Vec3::ZERO; 5]; 5]; + d[1][0] = Vec3::X; + assert!(normal_section(&d, Vec3::Z, Vec3::X).is_none(), "r_v is zero"); + } +} diff --git a/crates/geom/src/brep/tests.rs b/crates/geom/src/brep/tests.rs index bd233a282..716961579 100644 --- a/crates/geom/src/brep/tests.rs +++ b/crates/geom/src/brep/tests.rs @@ -872,3 +872,36 @@ fn a_point_beside_a_pole_is_one_within_its_reach() { assert_eq!(sphere.pole_within(near, 1e-7), [false, false]); assert_eq!(sphere.pole_within([0.3, 0.2], 1e-3), [false, false]); } + +/// The analytic surfaces' partials up to the fourth order agree with their first and second +/// derivatives and are each the derivative of the order below (central differences). +#[test] +fn analytic_partials_are_the_derivatives_of_the_order_below() { + use crate::tess::partials::MAX_PARTIAL_ORDER; + let f = Frame { origin: [1., 2., 3.], x_axis: [0.6, 0.8, 0.], y_axis: [0., 0., 1.] }; + let surfaces = [ + Surface::Cylinder { frame: f.clone(), radius: 3. }, + Surface::Cone { frame: f.clone(), radius_at_origin: 2., half_angle: 0.3 }, + Surface::Sphere { frame: f.clone(), radius: 4. }, + Surface::Torus { frame: f.clone(), major_radius: 5., minor_radius: 1.5 }, + ]; + let (u, v, h) = (0.8, 0.4, 1e-4); + let close = |a: crate::Vec3, b: crate::Vec3, tol: f64| a.dist(b) <= tol * (1.0 + a.len().max(b.len())); + for s in &surfaces { + let e = eval::SurfaceEval::new(s).unwrap(); + let d = e.partials(u, v).unwrap(); + let (p, su, sv) = e.derivs(u, v).unwrap(); + let (suu, suv, svv) = e.second(u, v).unwrap(); + for (a, b) in [(d[0][0], p), (d[1][0], su), (d[0][1], sv), (d[2][0], suu), (d[1][1], suv), (d[0][2], svv)] { + assert!(close(a, b, 1e-12), "{s:?}: {a:?} vs {b:?}"); + } + let (up, um, vp, vm) = + (e.partials(u + h, v).unwrap(), e.partials(u - h, v).unwrap(), e.partials(u, v + h).unwrap(), e.partials(u, v - h).unwrap()); + for i in 0..MAX_PARTIAL_ORDER { + for j in 0..MAX_PARTIAL_ORDER - i { + assert!(close((up[i][j] - um[i][j]) * (0.5 / h), d[i + 1][j], 1e-6), "{s:?} d/du ({i},{j})"); + assert!(close((vp[i][j] - vm[i][j]) * (0.5 / h), d[i][j + 1], 1e-6), "{s:?} d/dv ({i},{j})"); + } + } + } +} diff --git a/crates/geom/src/tess.rs b/crates/geom/src/tess.rs index 35544e711..6276fdbbf 100644 --- a/crates/geom/src/tess.rs +++ b/crates/geom/src/tess.rs @@ -15,6 +15,7 @@ use crate::bspline::MAX_SPLINE_DEGREE; use crate::{Mesh, Vec3}; +pub mod partials; pub mod uv; /// Most knots in one evaluable knot vector (bounds the shader's span search). @@ -367,17 +368,22 @@ impl EvalSurface { /// evaluation, which [`nurbs_derivs`] takes as it is) is that end: the basis functions are zero /// outside their supports, so the derivatives there would not exist. fn basis_second(knots: &[f64], p: usize, n: usize, t: f64) -> Option<(usize, Vec<[f64; 3]>)> { + let (first, ders) = crate::bspline::basis_derivatives(knots, p, n, onto_knots(knots, p, n, t)?, 2)?; + Some((first + p, ders.iter().map(|d| Some([*d.first()?, *d.get(1)?, *d.get(2)?])).collect::>()?)) +} + +/// `t`, or the end of the knot range of a spline of degree `p` over `n` points when `t` is past +/// it by no more than rounding ([`basis_second`]). +pub(crate) fn onto_knots(knots: &[f64], p: usize, n: usize, t: f64) -> Option { let (&lo, &hi) = (knots.get(p)?, knots.get(n)?); let slack = BOUNDARY_ROUNDING * (hi - lo).abs().max(lo.abs()).max(hi.abs()).max(1.0); - let t = if t < lo && t >= lo - slack { + Some(if t < lo && t >= lo - slack { lo } else if t > hi && t <= hi + slack { hi } else { t - }; - let (first, ders) = crate::bspline::basis_derivatives(knots, p, n, t, 2)?; - Some((first + p, ders.iter().map(|d| Some([*d.first()?, *d.get(1)?, *d.get(2)?])).collect::>()?)) + }) } /// How far past its knot range, relative to the range's magnitude, a parameter is still the range's diff --git a/crates/geom/src/tess/partials.rs b/crates/geom/src/tess/partials.rs new file mode 100644 index 000000000..e87fd278a --- /dev/null +++ b/crates/geom/src/tess/partials.rs @@ -0,0 +1,268 @@ +//! Exact partial derivatives of the display evaluator's surfaces up to the fourth order, and the +//! derivatives of a rational spline curve: what a seam's change of curvature (G3) and the change +//! of that change (G4) are read from. Splines differentiate their basis functions +//! ([`crate::bspline::basis_derivatives`]) and divide out the weight by Leibniz's rule (NURBS +//! Book eq. 4.8 and 4.20); a revolution turns its profile's derivatives by the rotation's own. + +use super::{EvalCurve, EvalSurface, affine, linear, onto_knots}; +use crate::Vec3; +use crate::bspline::{MAX_SPLINE_DEGREE, basis_derivatives, binomial}; + +/// The highest total order of [`Partials`]. +pub const MAX_PARTIAL_ORDER: usize = 4; + +/// A surface's partial derivatives at a point: `d[i][j]` is `∂^(i+j) S / ∂u^i ∂v^j`, filled for +/// `i + j ≤` [`MAX_PARTIAL_ORDER`] (`d[0][0]` the point itself) and zero past it. +pub type Partials = [[Vec3; MAX_PARTIAL_ORDER + 1]; MAX_PARTIAL_ORDER + 1]; + +/// The pairs `(i, j)` with `i + j ≤ MAX_PARTIAL_ORDER`. +pub(crate) fn orders() -> impl Iterator { + (0..=MAX_PARTIAL_ORDER).flat_map(|i| (0..=MAX_PARTIAL_ORDER - i).map(move |j| (i, j))) +} + +/// A table of partials from `f(i, j)`; `None` when one is missing or not finite. +pub(crate) fn tabulate(mut f: impl FnMut(usize, usize) -> Option) -> Option { + let mut d = [[Vec3::ZERO; MAX_PARTIAL_ORDER + 1]; MAX_PARTIAL_ORDER + 1]; + for (i, j) in orders() { + let x = f(i, j).filter(|x| x.is_finite())?; + *d.get_mut(i)?.get_mut(j)? = x; + } + Some(d) +} + +impl EvalSurface { + /// The partial derivatives at (u, v) up to the fourth order ([`Partials`]), exactly: a closed + /// spline's parameters wrapped first as in [`EvalSurface::derivs`]. `None` where the surface + /// cannot be evaluated (a malformed spline, a vanishing weight). + pub fn partials(&self, u: f64, v: f64) -> Option { + if !(u.is_finite() && v.is_finite()) { + return None; + } + match self { + EvalSurface::Plane { origin, du, dv } => tabulate(|i, j| { + Some(match (i, j) { + (0, 0) => *origin + *du * u + *dv * v, + (1, 0) => *du, + (0, 1) => *dv, + _ => Vec3::ZERO, + }) + }), + EvalSurface::Nurbs { degree, knots, count, ctrl, .. } => { + let [u, v] = self.wrap([u, v]); + nurbs_partials(*degree, knots, *count, ctrl, u, v) + } + EvalSurface::Revolved { profile, origin, axis, transform, swap } => { + let [u, v] = self.wrap([u, v]); + let (s, a) = if *swap { (v, u) } else { (u, v) }; + let c = profile.derivatives(s, MAX_PARTIAL_ORDER)?; + let turned = |i: usize, j: usize| -> Option { + // The profile's i-th derivative (its point taken about the axis), turned by + // the j-th derivative of the rotation. + let x = if i == 0 { *c.first()? - *origin } else { *c.get(i)? }; + let r = rotation_derivative(*axis, x, a, j); + Some(if i == 0 && j == 0 { affine(transform, *origin + r) } else { linear(transform, r) }) + }; + tabulate(|i, j| if *swap { turned(j, i) } else { turned(i, j) }) + } + } + } +} + +/// The `j`-th derivative by the angle `a` of `x` turned by `a` about the unit `axis` (Rodrigues: +/// `x cos a + (axis × x) sin a + axis (axis · x)(1 − cos a)`). +fn rotation_derivative(axis: Vec3, x: Vec3, a: f64, j: usize) -> Vec3 { + let (cos, sin) = trig_derivative(a, j); + let along = axis * axis.dot(x); + let constant = if j == 0 { along } else { Vec3::ZERO }; + x * cos + axis.cross(x) * sin - along * cos + constant +} + +/// The `k`-th derivatives of cos and sin at `t`: `cos(t + kπ/2)` and `sin(t + kπ/2)`. +pub(crate) fn trig_derivative(t: f64, k: usize) -> (f64, f64) { + let (sin, cos) = t.sin_cos(); + match k % 4 { + 0 => (cos, sin), + 1 => (-sin, cos), + 2 => (-cos, -sin), + _ => (sin, -cos), + } +} + +/// The basis functions alive at `t` and their derivatives up to `order`: the index of the first +/// and, for each, element `k` its `k`-th derivative (a `t` past an end by rounding is that end). +fn basis(knots: &[f64], p: usize, n: usize, t: f64, order: usize) -> Option<(usize, Vec>)> { + basis_derivatives(knots, p, n, onto_knots(knots, p, n, t)?, order) +} + +fn valid(p: usize, n: usize, knots: &[f64]) -> bool { + (1..=MAX_SPLINE_DEGREE).contains(&p) && n > p && knots.len() == n + p + 1 +} + +fn xyz(h: [f64; 4]) -> Vec3 { + Vec3::new(h[0], h[1], h[2]) +} + +fn nurbs_partials(degree: [usize; 2], knots: &[Vec; 2], count: [usize; 2], ctrl: &[[f64; 4]], u: f64, v: f64) -> Option { + let [pu, pv] = degree; + let [nu, nv] = count; + if !valid(pu, nu, &knots[0]) || !valid(pv, nv, &knots[1]) || nu.checked_mul(nv)? != ctrl.len() { + return None; + } + let ((fu, bu), (fv, bv)) = (basis(&knots[0], pu, nu, u, MAX_PARTIAL_ORDER)?, basis(&knots[1], pv, nv, v, MAX_PARTIAL_ORDER)?); + // The homogeneous partials A_ij = Σ N_a^(i) N_b^(j) P_ab (weights in the fourth place). + let mut a = [[[0.0f64; 4]; MAX_PARTIAL_ORDER + 1]; MAX_PARTIAL_ORDER + 1]; + for (jb, nb) in bv.iter().enumerate() { + for (ia, na) in bu.iter().enumerate() { + let c = ctrl.get((fv + jb) * nu + fu + ia)?; + for (i, j) in orders() { + let w = na.get(i)? * nb.get(j)?; + let sum = a.get_mut(i)?.get_mut(j)?; + for (s, c) in sum.iter_mut().zip(c) { + *s += w * c; + } + } + } + } + let w = a[0][0][3]; + if !(w.abs() > 1e-300) { + return None; + } + // S_kl = (A_kl − Σ_{(i,j) ≠ (0,0)} C(k,i) C(l,j) w_ij S_(k−i)(l−j)) / w, lower orders first. + let mut d = [[Vec3::ZERO; MAX_PARTIAL_ORDER + 1]; MAX_PARTIAL_ORDER + 1]; + for total in 0..=MAX_PARTIAL_ORDER { + for k in 0..=total { + let l = total - k; + let mut x = xyz(a[k][l]); + for (i, j) in (0..=k).flat_map(|i| (0..=l).map(move |j| (i, j))).filter(|ij| *ij != (0, 0)) { + x = x - d[k - i][l - j] * (binomial(k, i) * binomial(l, j) * a[i][j][3]); + } + d[k][l] = x * (1.0 / w); + } + } + tabulate(|i, j| d.get(i)?.get(j).copied()) +} + +impl EvalCurve { + /// The point and derivatives up to `order` at `t` (element `k` the `k`-th), exactly: the + /// weight divided out by Leibniz's rule. `None` if the curve is malformed or its weight + /// vanishes there. + pub fn derivatives(&self, t: f64, order: usize) -> Option> { + let (p, n) = (self.degree, self.ctrl.len()); + if !valid(p, n, &self.knots) || !t.is_finite() || order > MAX_PARTIAL_ORDER { + return None; + } + let (first, b) = basis(&self.knots, p, n, t, order)?; + let mut a = vec![[0.0f64; 4]; order + 1]; + for (k, nk) in b.iter().enumerate() { + let c = self.ctrl.get(first + k)?; + for (ak, w) in a.iter_mut().zip(nk) { + for (s, c) in ak.iter_mut().zip(c) { + *s += w * c; + } + } + } + let w = a.first()?[3]; + if !(w.abs() > 1e-300) { + return None; + } + let mut d: Vec = Vec::with_capacity(order + 1); + for k in 0..=order { + let mut x = xyz(*a.get(k)?); + for i in 1..=k { + x = x - *d.get(k - i)? * (binomial(k, i) * a.get(i)?[3]); + } + d.push(x * (1.0 / w)); + } + d.iter().all(|x| x.is_finite()).then_some(d) + } +} + +#[cfg(test)] +#[allow(clippy::unwrap_used)] +mod tests { + use super::*; + + /// Within `tol`, relative to the larger length (at least 1). + fn close(a: Vec3, b: Vec3, tol: f64) -> bool { + a.dist(b) <= tol * (1.0 + a.len().max(b.len())) + } + + fn quarter_cylinder() -> EvalSurface { + // A rational quadratic quarter circle of radius 2 in u, a line 3 long in v: exact. + let w = std::f64::consts::FRAC_1_SQRT_2; + EvalSurface::nurbs( + [2, 1], + [vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0], vec![0.0, 0.0, 1.0, 1.0]], + [3, 2], + vec![ + [2.0, 0.0, 0.0, 1.0], + [2.0 * w, 2.0 * w, 0.0, w], + [0.0, 2.0, 0.0, 1.0], + [2.0, 0.0, 3.0, 1.0], + [2.0 * w, 2.0 * w, 3.0 * w, w], + [0.0, 2.0, 3.0, 1.0], + ], + ) + } + + #[test] + fn every_partial_is_the_derivative_of_the_order_below() { + let line = EvalCurve { degree: 1, knots: vec![0.0, 0.0, 1.0, 1.0], ctrl: vec![[2.0, 0.0, 0.0, 1.0], [3.0, 0.0, 1.0, 1.0]] }; + let cubic = EvalSurface::nurbs( + [3, 2], + [vec![0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0], vec![0.0, 0.0, 0.0, 1.0, 1.0, 1.0]], + [4, 3], + (0..12).map(|k| [k as f64 * 0.7, (k * k) as f64 * 0.1, (k % 5) as f64, 1.0 + 0.1 * (k % 3) as f64]).collect(), + ); + let surfaces = [ + quarter_cylinder(), + cubic, + EvalSurface::Revolved { + profile: line, + origin: Vec3::ZERO, + axis: Vec3::Z, + transform: [[1.0, 0.0, 0.0, 1.0], [0.0, 0.0, -1.0, 2.0], [0.0, 1.0, 0.0, 3.0]], + swap: true, + }, + ]; + let h = 1e-4; + for s in &surfaces { + let (u, v) = (0.37, 0.61); + let d = s.partials(u, v).unwrap(); + let (p, su, sv) = s.derivs(u, v).unwrap(); + assert!(close(d[0][0], p, 1e-12) && close(d[1][0], su, 1e-12) && close(d[0][1], sv, 1e-12), "{s:?}"); + let (up, um) = (s.partials(u + h, v).unwrap(), s.partials(u - h, v).unwrap()); + let (vp, vm) = (s.partials(u, v + h).unwrap(), s.partials(u, v - h).unwrap()); + for (i, j) in orders().filter(|(i, j)| i + j < MAX_PARTIAL_ORDER) { + let du = (up[i][j] - um[i][j]) * (0.5 / h); + let dv = (vp[i][j] - vm[i][j]) * (0.5 / h); + assert!(close(du, d[i + 1][j], 1e-5), "d/du of ({i},{j}): {du:?} vs {:?}", d[i + 1][j]); + assert!(close(dv, d[i][j + 1], 1e-5), "d/dv of ({i},{j}): {dv:?} vs {:?}", d[i][j + 1]); + } + } + } + + #[test] + fn a_rational_quarter_circle_has_the_circles_derivatives() { + // On the circle of radius 2, the speed varies, but |r'| r'' and the curvature 1/2 do not: + // check the curvature (|r' × r''| / |r'|³) and its derivative (zero) from the table. + let s = quarter_cylinder(); + for u in [0.0, 0.2, 0.5, 0.9, 1.0] { + let d = s.partials(u, 0.4).unwrap(); + assert!((d[0][0].x.hypot(d[0][0].y) - 2.0).abs() < 1e-12); + let (r1, r2) = (d[1][0], d[2][0]); + let k = r1.cross(r2).len() / r1.len().powi(3); + assert!((k - 0.5).abs() < 1e-12, "{k}"); + assert!(d[0][2].len() < 1e-12 && d[1][1].z.abs() < 1e-12); + } + } + + #[test] + fn hostile_parameters_give_none() { + let s = quarter_cylinder(); + assert!(s.partials(f64::NAN, 0.0).is_none()); + assert!(s.partials(0.0, f64::INFINITY).is_none()); + let c = EvalCurve { degree: 2, knots: vec![0.0; 3], ctrl: vec![[0.0; 4]; 3] }; + assert!(c.derivatives(0.5, 2).is_none()); + } +}