a770fa5a5e5f
curves.rs
//! Curve math for the residuals: ellipse points and derivatives, B-spline end derivatives and
//! curve end frames (point, tangent, curvature) for tangent and smooth (G2) joins.
//!
//! Textbook formulas only: the derivative of a clamped B-spline at its ends follows from the
//! derivative control points (Piegl & Tiller, *The NURBS Book*, ch. 3–4), rational curves through
//! the quotient rule on homogeneous coordinates.
use cadcraft_geom::{Spline as GSpline, Vec2};
/// `v <= lim`, or NaN.
pub(crate) fn not_above(v: f64, lim: f64) -> bool {
v.is_nan() || v <= lim
}
/// A curve end: point, unit tangent in the curve's own direction, signed curvature (positive
/// when the curve turns left).
#[derive(Clone, Copy, Debug, PartialEq)]
pub(crate) struct Frame {
pub p: Vec2,
pub t: Vec2,
pub k: f64,
}
impl Frame {
/// From a point and the first and second derivatives with respect to any parameter.
pub fn from_derivs(p: Vec2, d1: Vec2, d2: Vec2) -> Option<Frame> {
let l = d1.len();
if !l.is_finite() || not_above(l, 1e-300) {
return None;
}
let k = d1.cross(d2) / (l * l * l);
let f = Frame { p, t: d1 * (1.0 / l), k };
(f.p.is_finite() && f.t.is_finite() && f.k.is_finite()).then_some(f)
}
/// The same end traversed the other way.
pub fn reversed(self) -> Frame {
Frame { p: self.p, t: -self.t, k: -self.k }
}
}
/// An ellipse or elliptical arc in solver parameters.
#[derive(Clone, Copy, Debug, PartialEq)]
pub(crate) struct Ell {
pub c: Vec2,
/// Major axis vector (centre to the end of the major axis).
pub mj: Vec2,
pub ratio: f64,
pub t0: f64,
pub t1: f64,
pub arc: bool,
}
impl Ell {
pub fn minor(&self) -> Vec2 {
self.mj.perp() * self.ratio
}
pub fn at(&self, t: f64) -> Vec2 {
self.c + self.mj * t.cos() + self.minor() * t.sin()
}
pub fn d1(&self, t: f64) -> Vec2 {
self.mj * -t.sin() + self.minor() * t.cos()
}
pub fn d2(&self, t: f64) -> Vec2 {
self.mj * -t.cos() - self.minor() * t.sin()
}
pub fn frame(&self, t: f64) -> Option<Frame> {
Frame::from_derivs(self.at(t), self.d1(t), self.d2(t))
}
/// Semi-axes `(a, b)`.
fn axes(&self) -> (f64, f64) {
let a = self.mj.len();
(a, a * self.ratio.abs())
}
/// Local coordinates of `p` along the major and minor axes.
fn local(&self, p: Vec2) -> Option<(f64, f64, Vec2, Vec2)> {
let (a, _) = self.axes();
if not_above(a, 1e-300) {
return None;
}
let u_dir = self.mj * (1.0 / a);
let v_dir = u_dir.perp();
let q = p - self.c;
Some((q.dot(u_dir), q.dot(v_dir), u_dir, v_dir))
}
/// A distance-like residual of `p` from the ellipse: zero exactly on it, about the distance
/// near it.
pub fn on(&self, p: Vec2) -> f64 {
let (a, b) = self.axes();
match self.local(p) {
Some((u, v, _, _)) if b > 1e-300 => {
let s = a / b;
(u * u + v * v * s * s).sqrt() - a
}
_ => p.dist(self.c) - a,
}
}
/// Unit outward normal of the ellipse's implicit form at (or near) `p`.
pub fn normal(&self, p: Vec2) -> Vec2 {
let (a, b) = self.axes();
match self.local(p) {
Some((u, v, ud, vd)) if b > 1e-300 => (ud * (u / (a * a)) + vd * (v / (b * b))).normalized(),
_ => (p - self.c).normalized(),
}
}
/// Largest `n·(P − c)` over the ellipse for a unit direction `n` (its support distance).
pub fn support(&self, n: Vec2) -> f64 {
let (x, y) = (n.dot(self.mj), n.dot(self.minor()));
(x * x + y * y).sqrt()
}
}
/// A circle (or the circle of an arc) as a parametric curve.
pub(crate) fn circle_at(c: Vec2, r: f64, t: f64) -> (Vec2, Vec2) {
let (s, co) = t.sin_cos();
(c + Vec2::new(co, s) * r, Vec2::new(-s, co) * r)
}
/// Homogeneous control point.
#[derive(Clone, Copy, Debug)]
struct H {
x: f64,
y: f64,
w: f64,
}
impl H {
fn sub(self, o: H) -> H {
H { x: self.x - o.x, y: self.y - o.y, w: self.w - o.w }
}
fn scale(self, f: f64) -> H {
H { x: self.x * f, y: self.y * f, w: self.w * f }
}
}
/// Is the knot vector clamped (end knot repeated `degree + 1` times) at the start or end?
pub(crate) fn clamped(s: &GSpline, at_end: bool) -> bool {
let p = s.degree;
let k = &s.knots;
if !s.is_valid() || k.len() < 2 * (p + 1) {
return false;
}
let scale = k.iter().fold(1.0f64, |m, v| m.max(v.abs()));
let tol = 1e-12 * scale;
let window = if at_end { k.get(k.len() - p - 1..) } else { k.get(..=p) };
match window {
Some(w) => w.iter().all(|v| w.first().is_some_and(|f| (v - f).abs() <= tol)),
None => false,
}
}
/// End frame of a clamped spline at its start (`at_end` false) or end, in the curve's own
/// direction. `None` for an unclamped end or degenerate data.
pub(crate) fn spline_end(s: &GSpline, at_end: bool) -> Option<Frame> {
if !clamped(s, at_end) {
return None;
}
let p = s.degree;
let n = s.control.len();
let wt = |i: usize| if s.weights.is_empty() { Some(1.0) } else { s.weights.get(i).copied() };
let hp = |i: usize| -> Option<H> {
let c = s.control.get(i)?;
let w = wt(i)?;
Some(H { x: c.x * w, y: c.y * w, w })
};
// Work on the start of the curve; for the end, reverse the control points and knots.
let (q, u): (Vec<H>, Vec<f64>) = if at_end {
let q = (0..n.min(3)).map(|i| hp(n - 1 - i)).collect::<Option<Vec<_>>>()?;
let last = s.knots.len() - 1;
let u = (0..s.knots.len().min(p + 3)).map(|i| s.knots.get(last - i).map(|v| -v)).collect::<Option<Vec<_>>>()?;
(q, u)
} else {
let q = (0..n.min(3)).map(hp).collect::<Option<Vec<_>>>()?;
let u = s.knots.get(..(p + 3).min(s.knots.len()))?.to_vec();
(q, u)
};
let pf = p as f64;
let (q0, q1) = (*q.first()?, *q.get(1)?);
let den1 = u.get(p + 1)? - u.get(1)?;
if not_above(den1.abs(), 1e-300) {
return None;
}
let a1 = q1.sub(q0).scale(pf / den1);
let a2 = if p >= 2 {
let q2 = *q.get(2)?;
let den2 = u.get(p + 2)? - u.get(2)?;
let den0 = u.get(p + 1)? - u.get(2)?;
if !(den2.abs() > 1e-300 && den0.abs() > 1e-300) {
return None;
}
q2.sub(q1).scale(1.0 / den2).sub(q1.sub(q0).scale(1.0 / den1)).scale(pf * (pf - 1.0) / den0)
} else {
H { x: 0.0, y: 0.0, w: 0.0 }
};
// Quotient rule: C = A / w.
if not_above(q0.w.abs(), 1e-300) {
return None;
}
let c = Vec2::new(q0.x / q0.w, q0.y / q0.w);
let c1 = (Vec2::new(a1.x, a1.y) - c * a1.w) / q0.w;
let c2 = (Vec2::new(a2.x, a2.y) - c1 * (2.0 * a1.w) - c * a2.w) / q0.w;
let f = Frame::from_derivs(c, c1, c2)?;
// The reversed curve's start is the end traversed backwards.
Some(if at_end { f.reversed() } else { f })
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn ellipse_residuals() {
let e = Ell { c: Vec2::new(1.0, 2.0), mj: Vec2::new(4.0, 0.0), ratio: 0.5, t0: 0.0, t1: 1.0, arc: false };
assert!(e.on(Vec2::new(5.0, 2.0)).abs() < 1e-12);
assert!(e.on(Vec2::new(1.0, 4.0)).abs() < 1e-12);
assert!(e.on(Vec2::new(6.0, 2.0)) > 0.0);
assert!((e.support(Vec2::new(0.0, 1.0)) - 2.0).abs() < 1e-12);
assert!((e.support(Vec2::new(1.0, 0.0)) - 4.0).abs() < 1e-12);
assert!(e.normal(Vec2::new(1.0, 4.0)).near(Vec2::new(0.0, 1.0), 1e-12));
// Curvature at the end of the major axis: a / b².
let f = e.frame(0.0).unwrap();
assert!((f.k - 4.0 / 4.0).abs() < 1e-12);
}
#[test]
fn spline_end_derivatives_match_sampling() {
let s = GSpline::from_control(vec![Vec2::new(0.0, 0.0), Vec2::new(1.0, 2.0), Vec2::new(3.0, 3.0), Vec2::new(6.0, 1.0)], 3);
for at_end in [false, true] {
let f = spline_end(&s, at_end).unwrap();
let (lo, hi) = s.domain();
let (t, h) = if at_end { (hi, -1e-4) } else { (lo, 1e-4) };
let (p0, p1, p2) = (s.eval(t), s.eval(t + h), s.eval(t + 2.0 * h));
assert!(f.p.near(p0, 1e-12));
// One-sided differences (a negative step at the end still gives d/dt).
let d1 = (p1 - p0) * (1.0 / h);
let d2 = (p2 - p1 * 2.0 + p0) * (1.0 / (h * h));
let g = Frame::from_derivs(p0, d1, d2).unwrap();
assert!(f.t.near(g.t, 1e-3), "{f:?} {g:?}");
assert!((f.k - g.k).abs() < 1e-2, "{f:?} {g:?}");
}
// An unclamped (periodic) spline has no ends.
let closed = GSpline::from_fit_points_closed(&[Vec2::new(0.0, 0.0), Vec2::new(4.0, 0.0), Vec2::new(2.0, 3.0)]);
assert!(spline_end(&closed, false).is_none());
}
}
No file is open. Choose one in the tree, or press Ctrl+O (⌘O on a Mac) to find one by name.
Syntax highlighting could not load. The unmodified read-only text remains available above.