ogeom_math/

bspline.rs

//! B-spline algorithms over control points: evaluation, refinement, elevation.
//!
//! Everything here is generic over [`Blend`], the affine structure a control
//! point needs. That is what lets one implementation serve curves and surfaces,
//! 2D and 3D, and (through the homogeneous trick) rational and non-rational
//! alike, instead of four near-copies that drift apart.
//!
//! # Rational curves
//!
//! A rational B-spline is a non-rational one in one higher dimension: weight
//! each control point, carry the weight as an extra coordinate, evaluate as
//! usual, then divide through. Every algorithm here therefore applies unchanged
//! to rational geometry via [`Weighted`], which matters because exact circles,
//! cylinders and spheres are *only* representable rationally.

use smallvec::SmallVec;

/// Derivatives up to a small order in each direction, inline: the kernel
/// asks for jets of order two, and the innermost evaluation loops must not
/// pay heap for their own scratch.
pub type DerivativeGrid<P> = SmallVec<[SmallVec<[P; 4]>; 4]>;
use ogeom_core::{OgeomResult, Tolerances, ogeom_bail};

use crate::{KnotVector, Point, Point2, Vector, Vector2};

/// The affine structure a control point needs: scaling and addition.
///
/// Implemented for vectors, points and scalars. de Boor and the refinement
/// algorithms take only affine combinations (coefficients summing to one), so
/// applying them to positions is meaningful even though positions have no
/// meaningful sum on their own.
pub trait Blend: Copy {
    /// The additive identity.
    fn zero() -> Self;
    /// Scale by a factor.
    fn scale(self, k: f64) -> Self;
    /// Add another value.
    fn add(self, other: Self) -> Self;

    /// `self * (1 - t) + other * t`.
    #[must_use]
    fn lerp(self, other: Self, t: f64) -> Self {
        self.scale(1.0 - t).add(other.scale(t))
    }

    /// Subtract, via scaling by `-1`.
    #[must_use]
    fn sub(self, other: Self) -> Self {
        self.add(other.scale(-1.0))
    }
}

impl Blend for f64 {
    fn zero() -> Self {
        0.0
    }
    fn scale(self, k: f64) -> Self {
        self * k
    }
    fn add(self, other: Self) -> Self {
        self + other
    }
}

impl Blend for Vector {
    fn zero() -> Self {
        Self::ZERO
    }
    fn scale(self, k: f64) -> Self {
        self * k
    }
    fn add(self, other: Self) -> Self {
        self + other
    }
}

impl Blend for Vector2 {
    fn zero() -> Self {
        Self::ZERO
    }
    fn scale(self, k: f64) -> Self {
        self * k
    }
    fn add(self, other: Self) -> Self {
        self + other
    }
}

impl Blend for Point {
    fn zero() -> Self {
        Self::ORIGIN
    }
    fn scale(self, k: f64) -> Self {
        Self::from_vector(self.to_vector() * k)
    }
    fn add(self, other: Self) -> Self {
        Self::from_vector(self.to_vector() + other.to_vector())
    }
}

impl Blend for Point2 {
    fn zero() -> Self {
        Self::ORIGIN
    }
    fn scale(self, k: f64) -> Self {
        Self::from_vector(self.to_vector() * k)
    }
    fn add(self, other: Self) -> Self {
        Self::from_vector(self.to_vector() + other.to_vector())
    }
}

/// A control point carrying a weight, for rational geometry.
///
/// Stored in *homogeneous* form (the point is already multiplied through by
/// the weight) because that is the form every algorithm needs, and converting
/// on each access would be both slower and a source of drift.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Weighted<P> {
    /// The point scaled by the weight.
    pub scaled: P,
    /// The weight.
    pub weight: f64,
}

impl<P: Blend> Weighted<P> {
    /// A weighted control point from a position and a weight.
    ///
    /// # Errors
    ///
    /// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the weight
    /// is not finite and positive. A zero weight makes the projection undefined
    /// and a negative one makes the curve leave its control polygon's convex
    /// hull, so neither is admitted.
    pub fn new(point: P, weight: f64, tol: Tolerances) -> OgeomResult<Self> {
        if !weight.is_finite() || weight <= tol.confusion() {
            ogeom_bail!(
                Construction,
                "control point weight {weight} must be finite and positive"
            );
        }
        Ok(Self {
            scaled: point.scale(weight),
            weight,
        })
    }

    /// The unweighted position.
    #[must_use]
    pub fn point(self) -> P {
        self.scaled.scale(1.0 / self.weight)
    }
}

impl<P: Blend> Blend for Weighted<P> {
    fn zero() -> Self {
        Self {
            scaled: P::zero(),
            weight: 0.0,
        }
    }
    fn scale(self, k: f64) -> Self {
        Self {
            scaled: self.scaled.scale(k),
            weight: self.weight * k,
        }
    }
    fn add(self, other: Self) -> Self {
        Self {
            scaled: self.scaled.add(other.scaled),
            weight: self.weight + other.weight,
        }
    }
}

/// Check that a control point count matches a knot vector.
fn check_shape<P>(knots: &KnotVector, control: &[P]) -> OgeomResult<()> {
    if control.len() != knots.control_point_count() {
        ogeom_bail!(
            Dimension,
            "knot vector describes {} control points, got {}",
            knots.control_point_count(),
            control.len()
        );
    }
    Ok(())
}

/// Evaluate a B-spline at `u` by de Boor's algorithm.
///
/// Numerically the right way to do it: a sequence of convex combinations of
/// control points, so the result stays inside their hull and no intermediate
/// can blow up. Evaluating the basis functions and taking a weighted sum gives
/// the same answer in exact arithmetic but is less stable, and expanding the
/// polynomial in monomials is far worse.
///
/// # Errors
///
/// [`OgeomError::Dimension`](ogeom_core::OgeomError::Dimension) if the control point
/// count disagrees with the knot vector; [`OgeomError::Domain`](ogeom_core::OgeomError::Domain)
/// if `u` is outside the domain.
pub fn evaluate<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    u: f64,
    tol: Tolerances,
) -> OgeomResult<P> {
    check_shape(knots, control)?;
    let span = knots.span(u, tol)?;
    let p = knots.degree();

    // On the stack up to degree seven, which is every spline a model
    // carries in practice: this runs for every point of every curve.
    let mut d: smallvec::SmallVec<[P; 8]> = (0..=p).map(|i| control[span - p + i]).collect();
    let k = knots.knots();
    for r in 1..=p {
        for j in (r..=p).rev() {
            let left = k[span + j - p];
            let right = k[span + j + 1 - r];
            // The span lookup guarantees this width is positive: a zero would
            // mean a knot of multiplicity above the degree, which the knot
            // vector's own validation rejects.
            let alpha = (u - left) / (right - left);
            d[j] = d[j - 1].lerp(d[j], alpha);
        }
    }
    Ok(d[p])
}

/// Evaluate a B-spline and its derivatives up to order `n`.
///
/// `result[0]` is the point; `result[k]` is the `k`th derivative. Orders above
/// the degree are zero.
///
/// # Errors
///
/// As [`evaluate`].
pub fn derivatives<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    u: f64,
    n: usize,
    tol: Tolerances,
) -> OgeomResult<Vec<P>> {
    check_shape(knots, control)?;
    let span = knots.span(u, tol)?;
    let p = knots.degree();
    let basis = knots.basis_derivatives(span, u, n);

    Ok((0..=n)
        .map(|order| {
            let mut sum = P::zero();
            for i in 0..=p {
                sum = sum.add(control[span - p + i].scale(basis[order][i]));
            }
            sum
        })
        .collect())
}

/// Insert `value` into the knot vector `count` times, adjusting control points
/// so the curve is unchanged.
///
/// Boehm's algorithm. The foundation of nearly everything else: splitting a
/// curve, converting to Bézier form, and raising continuity constraints all
/// reduce to knot insertion.
///
/// # Errors
///
/// [`OgeomError::Dimension`](ogeom_core::OgeomError::Dimension) on a shape mismatch,
/// [`OgeomError::Domain`](ogeom_core::OgeomError::Domain) if `value` is outside the
/// domain, and [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the
/// insertion would push a multiplicity above the degree.
pub fn insert_knot<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    value: f64,
    count: usize,
    tol: Tolerances,
) -> OgeomResult<Spline<P>> {
    check_shape(knots, control)?;
    if count == 0 {
        return Ok((knots.clone(), control.to_vec()));
    }
    let span = knots.span(value, tol)?;
    let p = knots.degree();
    let existing = knots.multiplicity_of(value);
    if existing + count > p {
        ogeom_bail!(
            Construction,
            "inserting {count} copies of {value} would reach multiplicity {}, above degree {p}",
            existing + count
        );
    }

    let new_knots = knots.with_knot_inserted(value, count)?;
    let last = control.len() - 1;
    let k = knots.knots();
    let (s, r) = (existing, count);

    let mut points: Vec<P> = vec![P::zero(); control.len() + r];
    // Control points outside the affected window are unchanged. Those before it
    // keep their index, those after it shift right by the number inserted.
    points[..=span - p].copy_from_slice(&control[..=span - p]);
    points[span - s + r..=last + r].copy_from_slice(&control[span - s..=last]);

    // The window that the insertion actually reworks, refined in place. Each
    // pass is a set of convex combinations, so the points stay in the hull.
    let mut window: Vec<P> = (0..=p - s).map(|i| control[span - p + i]).collect();
    let mut window_start = span - p;
    for j in 1..=r {
        window_start = span - p + j;
        for i in 0..=p - j - s {
            let left = k[window_start + i];
            let right = k[i + span + 1];
            let alpha = (value - left) / (right - left);
            window[i] = window[i].lerp(window[i + 1], alpha);
        }
        points[window_start] = window[0];
        points[span + r - j - s] = window[p - j - s];
    }

    // Whatever the passes left in the middle of the window.
    if window_start + 1 < span - s {
        let width = (span - s) - (window_start + 1);
        points[window_start + 1..span - s].copy_from_slice(&window[1..=width]);
    }

    Ok((new_knots, points))
}

/// A B-spline: a knot vector paired with its control points.
pub type Spline<P> = (KnotVector, Vec<P>);

/// A Bézier segment: the parameter interval it covers, and its control points.
pub type BezierSegment<P> = ((f64, f64), Vec<P>);

/// Join two clamped B-splines of one degree end to start into one.
///
/// `a`'s last control point and `b`'s first are taken to be the same
/// point (the caller checks, since a control point is whatever blends)
/// and become one control. The join knot is left at multiplicity `degree`,
/// so the curve passes through it and continues with `b`'s parameter
/// shifted to begin where `a`'s ends. The domain is the two domains laid
/// end to end.
///
/// # Errors
///
/// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the
/// degrees differ, either knot vector is not clamped, or a knot vector does
/// not fit its control points.
pub fn join<P: Blend>(a: &Spline<P>, b: &Spline<P>) -> OgeomResult<Spline<P>> {
    let ((ak, ac), (bk, bc)) = (a, b);
    check_shape(ak, ac)?;
    check_shape(bk, bc)?;
    let p = ak.degree();
    if bk.degree() != p {
        ogeom_bail!(
            Construction,
            "cannot join a degree {p} B-spline to a degree {} one",
            bk.degree()
        );
    }
    if !ak.is_clamped() || !bk.is_clamped() {
        ogeom_bail!(Construction, "only clamped B-splines join");
    }
    let shift = ak.domain_end() - bk.domain_start();
    let mut knots: Vec<f64> = ak.knots()[..ak.knots().len() - 1].to_vec();
    knots.extend(bk.knots()[p + 1..].iter().map(|k| k + shift));
    let mut control: Vec<P> = ac[..ac.len() - 1].to_vec();
    control.extend_from_slice(bc);
    Ok((KnotVector::new(knots, p)?, control))
}

/// Continue a clamped B-spline past one end by `span` in parameter: the
/// polynomial continuation of the curve's own end derivatives, joined on.
///
/// The continuation is the Taylor polynomial of order `continuity` at the
/// end (the polynomial whose derivatives up to that order agree with the
/// curve's there), expressed in Bernstein form over the new span, raised to
/// the spline's degree and joined on with the knot at multiplicity
/// `degree`. The curve is continued rather than approximated: a polynomial
/// spline of degree at most `continuity` continues *as itself*, and so does
/// a rational curve's homogeneous polynomial: a rational circle arc
/// continued at order two stays on its circle. Orders above the degree are
/// held to the degree, which is as smooth as the spline itself is.
///
/// Extended at the start, the original run keeps its parameters and the
/// domain grows downward. Extended at the end, it grows upward.
///
/// # Errors
///
/// [`OgeomError::Construction`](ogeom_core::OgeomError::Construction) if the
/// knot vector is not clamped or the span is not positive and finite; as
/// [`derivatives`] on a shape mismatch.
pub fn extend<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    at_end: bool,
    span: f64,
    continuity: usize,
    tol: Tolerances,
) -> OgeomResult<Spline<P>> {
    check_shape(knots, control)?;
    if !knots.is_clamped() {
        ogeom_bail!(Construction, "only clamped B-splines extend");
    }
    if !(span > 0.0 && span.is_finite()) {
        ogeom_bail!(
            Construction,
            "an extension needs a positive, finite span; got {span}"
        );
    }
    if !at_end {
        // The start is the end of the reversed curve. Reversed back, the
        // extension stands before the original, which keeps its parameters
        // once the whole is slid down by the span.
        let (rk, rc) = reverse(knots, control);
        let (ek, ec) = extend(&rk, &rc, true, span, continuity, tol)?;
        let (bk, bc) = reverse(&ek, &ec);
        let (lo, hi) = knots.domain();
        return Ok((bk.reparameterized(lo - span, hi)?, bc));
    }
    let p = knots.degree();
    let k = continuity.min(p);
    let end = knots.domain_end();
    let jet = derivatives(knots, control, end, k, tol)?;
    // Monomial coefficients `D_i / i!` on `s` in `[0, span]`, in Bernstein
    // form: `b_j = sum over i <= j of C(j, i) / C(k, i) * a_i * span^i`.
    let mut bezier: Vec<P> = Vec::with_capacity(k + 1);
    for j in 0..=k {
        let mut b = P::zero();
        let (mut factorial, mut power) = (1.0_f64, 1.0_f64);
        for (i, derivative) in jet.iter().enumerate().take(j + 1) {
            if i > 0 {
                #[allow(clippy::cast_precision_loss)]
                {
                    factorial *= i as f64;
                }
                power *= span;
            }
            #[allow(clippy::cast_precision_loss)]
            let ratio = binomial_coefficient(j, i) as f64 / binomial_coefficient(k, i) as f64;
            b = b.add(derivative.scale(ratio * power / factorial));
        }
        bezier.push(b);
    }
    let mut piece_knots: Vec<f64> = Vec::with_capacity(2 * (k + 1));
    piece_knots.extend(core::iter::repeat_n(end, k + 1));
    piece_knots.extend(core::iter::repeat_n(end + span, k + 1));
    let mut piece: Spline<P> = (KnotVector::new(piece_knots, k)?, bezier);
    for _ in k..p {
        piece = elevate_degree(&piece.0, &piece.1, tol)?;
    }
    join(&(knots.clone(), control.to_vec()), &piece)
}

/// Continue a clamped B-spline past one end to `target`, over `span` in
/// parameter: a Bézier piece whose first `continuity + 1` controls carry
/// the curve's own end derivatives (as [`extend`] does) and whose last is
/// `target`, joined on. The curve is raised a degree first where the piece
/// needs one more than it has.
///
/// # Errors
///
/// As [`extend`].
pub fn extend_to<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    at_end: bool,
    target: P,
    span: f64,
    continuity: usize,
    tol: Tolerances,
) -> OgeomResult<Spline<P>> {
    check_shape(knots, control)?;
    if !knots.is_clamped() {
        ogeom_bail!(Construction, "only clamped B-splines extend");
    }
    if !(span > 0.0 && span.is_finite()) {
        ogeom_bail!(
            Construction,
            "an extension needs a positive, finite span; got {span}"
        );
    }
    if !at_end {
        let (rk, rc) = reverse(knots, control);
        let (ek, ec) = extend_to(&rk, &rc, true, target, span, continuity, tol)?;
        let (bk, bc) = reverse(&ek, &ec);
        let (lo, hi) = knots.domain();
        return Ok((bk.reparameterized(lo - span, hi)?, bc));
    }
    let mut base: Spline<P> = (knots.clone(), control.to_vec());
    let k = continuity.min(base.0.degree());
    let n = k + 1;
    while base.0.degree() < n {
        base = elevate_degree(&base.0, &base.1, tol)?;
    }
    let end = base.0.domain_end();
    let jet = derivatives(&base.0, &base.1, end, k, tol)?;
    // Bernstein controls of degree `n` for the Taylor data through order
    // `k`, then the target in the last place.
    let mut bezier: Vec<P> = Vec::with_capacity(n + 1);
    for j in 0..=k {
        let mut b = P::zero();
        let (mut factorial, mut power) = (1.0_f64, 1.0_f64);
        for (i, derivative) in jet.iter().enumerate().take(j + 1) {
            if i > 0 {
                #[allow(clippy::cast_precision_loss)]
                {
                    factorial *= i as f64;
                }
                power *= span;
            }
            #[allow(clippy::cast_precision_loss)]
            let ratio = binomial_coefficient(j, i) as f64 / binomial_coefficient(n, i) as f64;
            b = b.add(derivative.scale(ratio * power / factorial));
        }
        bezier.push(b);
    }
    bezier.push(target);
    let mut piece_knots: Vec<f64> = Vec::with_capacity(2 * (n + 1));
    piece_knots.extend(core::iter::repeat_n(end, n + 1));
    piece_knots.extend(core::iter::repeat_n(end + span, n + 1));
    let mut piece: Spline<P> = (KnotVector::new(piece_knots, n)?, bezier);
    for _ in n..base.0.degree() {
        piece = elevate_degree(&piece.0, &piece.1, tol)?;
    }
    join(&base, &piece)
}

/// Split a B-spline at `u` into two, each with its own clamped knot vector.
///
/// Works by raising the multiplicity at `u` to the degree, at which point the
/// control points either side are already independent.
///
/// # Errors
///
/// As [`insert_knot`], plus [`OgeomError::Domain`](ogeom_core::OgeomError::Domain) if
/// `u` is at either end of the domain, where one half would be empty.
pub fn split<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    u: f64,
    tol: Tolerances,
) -> OgeomResult<(Spline<P>, Spline<P>)> {
    check_shape(knots, control)?;
    let (start, end) = knots.domain();
    if u <= start + tol.parametric() || u >= end - tol.parametric() {
        ogeom_bail!(
            Domain,
            "cannot split at {u}, an end of the domain [{start}, {end}]"
        );
    }
    let p = knots.degree();
    let existing = knots.multiplicity_of(u);
    let (refined, points) = insert_knot(knots, control, u, p - existing, tol)?;

    // After refinement the two halves meet at a control point they share.
    let cut = refined.knots().partition_point(|k| *k < u);
    let left_points = points[..cut].to_vec();
    let right_points = points[cut - 1..].to_vec();

    let mut left_knots = refined.knots()[..cut + p].to_vec();
    left_knots.push(u);
    let mut right_knots = vec![u];
    right_knots.extend_from_slice(&refined.knots()[cut..]);

    Ok((
        (KnotVector::new(left_knots, p)?, left_points),
        (KnotVector::new(right_knots, p)?, right_points),
    ))
}

/// Decompose a B-spline into its Bézier segments.
///
/// Returns one control-point array per segment, each of `degree + 1` points,
/// together with the parameter interval it covers. Many algorithms (plotting,
/// intersection, conversion to exchange formats) are far simpler on Bézier
/// pieces than on the whole spline.
///
/// # Errors
///
/// As [`insert_knot`].
pub fn to_bezier_segments<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    tol: Tolerances,
) -> OgeomResult<Vec<BezierSegment<P>>> {
    check_shape(knots, control)?;
    let p = knots.degree();
    let end = knots.domain().1;

    // Raise every knot of the domain to full multiplicity, its ends too: an
    // unclamped vector's curve starts and ends inside its first and last
    // spans' hulls, and only a clamped end makes a segment's first control
    // point its first point. The start is raised in place. The end is the
    // start of the reversed curve.
    let clamp_start = |knots: &KnotVector, control: &[P]| -> OgeomResult<Spline<P>> {
        let (start, _) = knots.domain();
        let needed = p.saturating_sub(knots.multiplicity_of(start));
        insert_knot(knots, control, start, needed, tol)
    };
    let (mut current_knots, mut current_points) = clamp_start(knots, control)?;
    if current_knots.multiplicity_of(end) < p {
        let (reversed_knots, reversed_points) = reverse(&current_knots, &current_points);
        let (reversed_knots, reversed_points) = clamp_start(&reversed_knots, &reversed_points)?;
        (current_knots, current_points) = reverse(&reversed_knots, &reversed_points);
    }
    let (start, end) = current_knots.domain();
    for (value, multiplicity) in current_knots.clone().distinct() {
        if value <= start || value >= end {
            continue;
        }
        let needed = p.saturating_sub(multiplicity);
        if needed > 0 {
            let (k, c) = insert_knot(&current_knots, &current_points, value, needed, tol)?;
            current_knots = k;
            current_points = c;
        }
    }

    let breaks: Vec<f64> = core::iter::once(start)
        .chain(
            current_knots
                .distinct()
                .into_iter()
                .filter(|(v, _)| *v > start && *v < end)
                .map(|(v, _)| v),
        )
        .chain(core::iter::once(end))
        .collect();

    // Each segment's points are the `p + 1` its span reads.
    let mut out = Vec::with_capacity(breaks.len() - 1);
    for w in breaks.windows(2) {
        let span = current_knots.span(f64::midpoint(w[0], w[1]), tol)?;
        out.push(((w[0], w[1]), current_points[span - p..=span].to_vec()));
    }
    Ok(out)
}

/// Raise the degree by one, leaving the curve unchanged.
///
/// Works segment by segment on the Bézier decomposition, where degree elevation
/// is the exact closed form `Q[i] = (i/(p+1)) P[i-1] + (1 - i/(p+1)) P[i]`, and
/// reassembles by removing the knots that were introduced: an interior knot of
/// multiplicity `m` comes back with `m + 1`, so the curve keeps its continuity
/// there.
///
/// # Errors
///
/// As [`to_bezier_segments`].
pub fn elevate_degree<P: Blend>(
    knots: &KnotVector,
    control: &[P],
    tol: Tolerances,
) -> OgeomResult<Spline<P>> {
    check_shape(knots, control)?;
    let p = knots.degree();
    let segments = to_bezier_segments(knots, control, tol)?;

    let mut points: Vec<P> = Vec::with_capacity(segments.len() * (p + 1) + 1);
    let mut new_knots: Vec<f64> = Vec::new();

    for (index, ((a, b), segment)) in segments.iter().enumerate() {
        // The elevated Bezier segment has p + 2 control points.
        let mut elevated: Vec<P> = Vec::with_capacity(p + 2);
        elevated.push(segment[0]);
        #[allow(clippy::cast_precision_loss)]
        for i in 1..=p {
            let t = i as f64 / (p + 1) as f64;
            elevated.push(segment[i - 1].lerp(segment[i], 1.0 - t));
        }
        elevated.push(segment[p]);

        if index == 0 {
            points.extend_from_slice(&elevated);
            new_knots.extend(core::iter::repeat_n(*a, p + 2));
        } else {
            // The shared endpoint is already present.
            points.extend_from_slice(&elevated[1..]);
            new_knots.extend(core::iter::repeat_n(*a, p + 1));
        }
        if index == segments.len() - 1 {
            new_knots.extend(core::iter::repeat_n(*b, p + 2));
        }
    }

    // Each interior knot stands at full multiplicity, `p + 1` in degree
    // `p + 1`. The curve is as smooth there as it was, so all but `m + 1`
    // come out exactly.
    for (value, multiplicity) in knots.distinct() {
        let (start, end) = knots.domain();
        if value <= start || value >= end {
            continue;
        }
        for _ in multiplicity..p {
            (new_knots, points) = remove_knot_once(&new_knots, &points, p + 1, value);
        }
    }

    Ok((KnotVector::new(new_knots, p + 1)?, points))
}

/// One occurrence of the interior knot `u` taken out of a degree-`p` spline
/// the knot is exactly removable from: the inverse of inserting it, solved
/// from both ends of the points it touches and met in the middle.
fn remove_knot_once<P: Blend>(
    knots: &[f64],
    control: &[P],
    p: usize,
    u: f64,
) -> (Vec<f64>, Vec<P>) {
    let Some(r) = knots.iter().rposition(|k| *k == u) else {
        return (knots.to_vec(), control.to_vec());
    };
    let left = knots.iter().filter(|k| **k == u).count() - 1;
    let mut reduced = knots.to_vec();
    reduced.remove(r);
    // Inserting `u` into the reduced spline rewrites its points `lo..=hi`:
    // `P[i] = a[i] Q[i] + (1 - a[i]) Q[i - 1]`, keeping those before and
    // shifting those after by one.
    let k = r - 1;
    let (lo, hi) = (k + 1 - p, k - left);
    let alpha = |i: usize| (u - reduced[i]) / (reduced[i + p] - reduced[i]);
    let mut q: Vec<P> = Vec::with_capacity(control.len() - 1);
    q.extend_from_slice(&control[..lo]);
    let mut forward: Vec<P> = Vec::with_capacity(hi - lo);
    let mut previous = control[lo - 1];
    for (i, point) in control.iter().enumerate().take(hi).skip(lo) {
        let a = alpha(i);
        previous = point.sub(previous.scale(1.0 - a)).scale(1.0 / a);
        forward.push(previous);
    }
    let mut backward: Vec<P> = vec![P::zero(); hi - lo];
    let mut next = control[hi + 1];
    for i in (lo + 1..=hi).rev() {
        let a = alpha(i);
        next = control[i].sub(next.scale(a)).scale(1.0 / (1.0 - a));
        backward[i - 1 - lo] = next;
    }
    let middle = (hi - lo) / 2;
    for j in 0..hi - lo {
        q.push(if j < middle { forward[j] } else { backward[j] });
    }
    q.extend_from_slice(&control[hi + 1..]);
    (reduced, q)
}

/// Reverse the parameter direction, leaving the curve's shape unchanged.
#[must_use]
pub fn reverse<P: Blend>(knots: &KnotVector, control: &[P]) -> Spline<P> {
    let mut points = control.to_vec();
    points.reverse();
    (knots.reversed(), points)
}

/// Evaluate a rational B-spline: de Boor in homogeneous coordinates, then
/// divide through by the weight.
///
/// # Errors
///
/// As [`evaluate`], plus [`OgeomError::Numeric`](ogeom_core::OgeomError::Numeric) if the
/// accumulated weight vanishes, which positive input weights make impossible.
pub fn evaluate_rational<P: Blend>(
    knots: &KnotVector,
    control: &[Weighted<P>],
    u: f64,
    tol: Tolerances,
) -> OgeomResult<P> {
    let h = evaluate(knots, control, u, tol)?;
    if h.weight.abs() <= tol.confusion() {
        ogeom_bail!(Numeric, "rational evaluation produced a vanishing weight");
    }
    Ok(h.point())
}

/// Evaluate a rational B-spline and its derivatives up to order `n`.
///
/// The quotient rule applied to the homogeneous form. Differentiating the
/// projected curve directly is not an option: the projection is a quotient, so
/// its derivatives mix all lower orders.
///
/// # Errors
///
/// As [`evaluate_rational`].
pub fn rational_derivatives<P: Blend>(
    knots: &KnotVector,
    control: &[Weighted<P>],
    u: f64,
    n: usize,
    tol: Tolerances,
) -> OgeomResult<Vec<P>> {
    let homogeneous = derivatives(knots, control, u, n, tol)?;
    if homogeneous[0].weight.abs() <= tol.confusion() {
        ogeom_bail!(Numeric, "rational evaluation produced a vanishing weight");
    }

    // C^(k) = ( A^(k) - sum_{i=1..k} C(k,i) w^(i) C^(k-i) ) / w
    let mut out: Vec<P> = Vec::with_capacity(n + 1);
    for (order, term) in homogeneous.iter().enumerate() {
        let mut value = term.scaled;
        for i in 1..=order {
            #[allow(clippy::cast_precision_loss)]
            let binomial = binomial_coefficient(order, i) as f64;
            value = value.sub(out[order - i].scale(binomial * homogeneous[i].weight));
        }
        out.push(value.scale(1.0 / homogeneous[0].weight));
    }
    Ok(out)
}

/// `n choose k`, computed multiplicatively so it stays exact for the small
/// values derivative formulas need.
#[must_use]
pub fn binomial_coefficient(n: usize, k: usize) -> u64 {
    if k > n {
        return 0;
    }
    let k = k.min(n - k);
    let mut result = 1_u64;
    for i in 0..k {
        result = result * (n - i) as u64 / (i as u64 + 1);
    }
    result
}

#[cfg(test)]
#[allow(clippy::unwrap_used)]
mod join_tests {
    use super::*;
    use crate::Point;

    #[test]
    fn a_joined_spline_evaluates_as_its_two_halves_did() {
        let tol = Tolerances::millimetres();
        let control: Vec<Point> = (0..6)
            .map(|i| Point::new(f64::from(i), f64::from(i * i % 5), 0.0))
            .collect();
        let knots = KnotVector::clamped_uniform(3, control.len()).unwrap();
        let ((lk, lc), (rk, rc)) = split(&knots, &control, 0.4, tol).unwrap();
        let (jk, jc) = join(&(lk, lc), &(rk, rc)).unwrap();
        assert_eq!(
            jk.domain(),
            knots.domain(),
            "the domain is the two laid end to end"
        );
        assert_eq!(
            jc.len() + 3 + 1,
            jk.knots().len(),
            "the knots fit the controls"
        );
        for i in 0..=20 {
            let u = f64::from(i) / 20.0;
            let before = evaluate(&knots, &control, u, tol).unwrap();
            let after = evaluate(&jk, &jc, u, tol).unwrap();
            assert!(
                before.is_equal(after, tol),
                "at {u}: {before:?} became {after:?}"
            );
        }
    }
}

#[cfg(test)]
#[allow(clippy::unwrap_used)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    const T: Tolerances = Tolerances::millimetres();

    /// An extension continues the curve: the original run evaluates as it
    /// did, the derivatives agree at the join to the order asked, and the
    /// domain grows by the span at the end asked for.
    #[test]
    fn an_extension_continues_the_curve_to_its_order() {
        let knots = KnotVector::clamped_uniform(3, 6).unwrap();
        let control = vec![
            Point::new(0.0, 0.0, 0.0),
            Point::new(1.0, 2.0, 0.5),
            Point::new(2.5, 1.0, -0.5),
            Point::new(4.0, 3.0, 1.0),
            Point::new(5.0, 0.5, 0.0),
            Point::new(6.0, 2.0, 2.0),
        ];
        let (lo, hi) = knots.domain();
        for at_end in [true, false] {
            let (ek, ec) = extend(&knots, &control, at_end, 0.4, 2, T).unwrap();
            let (elo, ehi) = ek.domain();
            if at_end {
                assert!((elo - lo).abs() < 1e-12 && (ehi - (hi + 0.4)).abs() < 1e-12);
            } else {
                assert!((elo - (lo - 0.4)).abs() < 1e-12 && (ehi - hi).abs() < 1e-12);
            }
            for i in 0..=10 {
                let u = lo + (hi - lo) * f64::from(i) / 10.0;
                let was = evaluate(&knots, &control, u, T).unwrap();
                let now = evaluate(&ek, &ec, u, T).unwrap();
                assert!(
                    was.distance(now) < 1e-9,
                    "the original run at {u}: {was:?} vs {now:?}"
                );
            }
            // A hair either side of the join: the jets agree to the order
            // asked, up to the next derivative's step across the hair.
            let join_at = if at_end { hi } else { lo };
            let step = if at_end { 1e-7 } else { -1e-7 };
            let inside = derivatives(&knots, &control, join_at - step, 2, T).unwrap();
            let outside = derivatives(&ek, &ec, join_at + step, 2, T).unwrap();
            for order in 0..=2 {
                let (a, b) = (inside[order], outside[order]);
                let gap = a.to_vector().sub(b.to_vector()).magnitude();
                let scale = a.to_vector().magnitude().max(1.0);
                assert!(
                    gap < scale * 1e-4,
                    "order {order} across the join: {a:?} vs {b:?}"
                );
            }
        }
    }

    fn cubic_curve() -> (KnotVector, Vec<Point>) {
        let control = vec![
            Point::new(0.0, 0.0, 0.0),
            Point::new(1.0, 2.0, 0.0),
            Point::new(3.0, 3.0, 1.0),
            Point::new(5.0, 1.0, 2.0),
            Point::new(6.0, -1.0, 1.0),
            Point::new(8.0, 0.0, 0.0),
        ];
        (
            KnotVector::clamped_uniform(3, control.len()).unwrap(),
            control,
        )
    }

    fn sample(knots: &KnotVector, control: &[Point], n: usize) -> Vec<Point> {
        let (a, b) = knots.domain();
        (0..=n)
            .map(|i| {
                #[allow(clippy::cast_precision_loss)]
                let u = a + (b - a) * (i as f64 / n as f64);
                evaluate(knots, control, u, T).unwrap()
            })
            .collect()
    }

    #[test]
    fn a_clamped_curve_interpolates_its_end_points() {
        let (k, c) = cubic_curve();
        let (a, b) = k.domain();
        assert!(evaluate(&k, &c, a, T).unwrap().is_equal(c[0], T));
        assert!(evaluate(&k, &c, b, T).unwrap().is_equal(c[c.len() - 1], T));
    }

    #[test]
    fn de_boor_agrees_with_the_basis_function_sum() {
        // Two independent routes to the same value. They must agree.
        let (k, c) = cubic_curve();
        for i in 0..=50 {
            let u = f64::from(i) / 50.0;
            let span = k.span(u, T).unwrap();
            let basis = k.basis(span, u);
            let mut sum = Vector::ZERO;
            for j in 0..=k.degree() {
                sum += c[span - k.degree() + j].to_vector() * basis[j];
            }
            let de_boor = evaluate(&k, &c, u, T).unwrap();
            assert!(de_boor.is_equal(Point::from_vector(sum), T), "at u = {u}");
        }
    }

    #[test]
    fn shape_mismatches_and_out_of_domain_parameters_are_refused() {
        let (k, c) = cubic_curve();
        assert!(
            evaluate(&k, &c[..3], 0.5, T).is_err(),
            "too few control points"
        );
        assert!(evaluate(&k, &c, -0.1, T).is_err());
        assert!(evaluate(&k, &c, 1.1, T).is_err());
    }

    #[test]
    fn derivatives_agree_with_finite_differences() {
        let (k, c) = cubic_curve();
        let h = 1e-6;
        for i in 1..20 {
            let u = f64::from(i) / 20.0;
            let d = derivatives(&k, &c, u, 2, T).unwrap();
            assert!(d[0].is_equal(evaluate(&k, &c, u, T).unwrap(), T));

            let ahead = evaluate(&k, &c, u + h, T).unwrap();
            let behind = evaluate(&k, &c, u - h, T).unwrap();
            let numeric = (ahead - behind) * (1.0 / (2.0 * h));
            assert!(
                (d[1].to_vector() - numeric).magnitude() < 1e-5,
                "first derivative disagrees at {u}"
            );
        }
    }

    #[test]
    fn knot_insertion_does_not_move_the_curve() {
        let (k, c) = cubic_curve();
        let before = sample(&k, &c, 100);
        for (value, count) in [(0.25, 1), (0.5, 2), (0.75, 3), (0.1, 1)] {
            let (k2, c2) = insert_knot(&k, &c, value, count, T).unwrap();
            assert_eq!(c2.len(), c.len() + count);
            assert_eq!(k2.multiplicity_of(value), k.multiplicity_of(value) + count);
            let after = sample(&k2, &c2, 100);
            for (a, b) in before.iter().zip(&after) {
                assert!(
                    a.is_equal(*b, T),
                    "inserting {count} at {value} moved the curve"
                );
            }
        }
    }

    #[test]
    fn repeated_insertion_matches_a_single_multiple_insertion() {
        let (k, c) = cubic_curve();
        let (ka, ca) = insert_knot(&k, &c, 0.4, 3, T).unwrap();

        let (k1, c1) = insert_knot(&k, &c, 0.4, 1, T).unwrap();
        let (k2, c2) = insert_knot(&k1, &c1, 0.4, 1, T).unwrap();
        let (kb, cb) = insert_knot(&k2, &c2, 0.4, 1, T).unwrap();

        assert_eq!(ka.knots(), kb.knots());
        for (a, b) in ca.iter().zip(&cb) {
            assert!(a.is_equal(*b, T));
        }
    }

    #[test]
    fn insertion_beyond_the_degree_is_refused() {
        let (k, c) = cubic_curve();
        assert!(insert_knot(&k, &c, 0.5, 4, T).is_err());
        assert!(insert_knot(&k, &c, 0.5, 3, T).is_ok());
        assert!(
            insert_knot(&k, &c, 2.0, 1, T).is_err(),
            "outside the domain"
        );
    }

    #[test]
    fn splitting_reproduces_both_halves_of_the_original() {
        let (k, c) = cubic_curve();
        let cut = 0.4;
        let ((lk, lc), (rk, rc)) = split(&k, &c, cut, T).unwrap();

        assert_relative_eq!(lk.domain().1, cut, epsilon = 1e-15);
        assert_relative_eq!(rk.domain().0, cut, epsilon = 1e-15);
        assert!(lk.is_clamped() && rk.is_clamped());

        for i in 0..=40 {
            let t = f64::from(i) / 40.0;
            let left_u = lk.domain().0 + (cut - lk.domain().0) * t;
            let right_u = cut + (rk.domain().1 - cut) * t;
            assert!(
                evaluate(&lk, &lc, left_u, T)
                    .unwrap()
                    .is_equal(evaluate(&k, &c, left_u, T).unwrap(), T),
                "left half diverges at {left_u}"
            );
            assert!(
                evaluate(&rk, &rc, right_u, T)
                    .unwrap()
                    .is_equal(evaluate(&k, &c, right_u, T).unwrap(), T),
                "right half diverges at {right_u}"
            );
        }
    }

    #[test]
    fn splitting_at_an_end_of_the_domain_is_refused() {
        let (k, c) = cubic_curve();
        assert!(split(&k, &c, 0.0, T).is_err());
        assert!(split(&k, &c, 1.0, T).is_err());
    }

    #[test]
    fn an_unclamped_curve_decomposes_and_elevates_unchanged() {
        // A uniform cubic whose outer knots lie beyond its domain [3, 6].
        let knots = KnotVector::new((0..10).map(f64::from).collect(), 3).unwrap();
        let control = vec![
            Point::new(0.0, 0.0, 0.0),
            Point::new(5.0, 1.0, 0.0),
            Point::new(-2.0, 3.0, 1.0),
            Point::new(7.0, 2.0, 2.0),
            Point::new(1.0, -1.0, 1.0),
            Point::new(3.0, 0.0, 0.0),
        ];
        let segments = to_bezier_segments(&knots, &control, T).unwrap();
        assert_eq!(segments.len(), 3);
        for ((a, b), points) in &segments {
            let bezier = KnotVector::clamped_uniform(3, points.len()).unwrap();
            for s in [0.0, 0.25, 0.5, 1.0] {
                let on_segment = evaluate(&bezier, points, s, T).unwrap();
                let on_curve = evaluate(&knots, &control, a + (b - a) * s, T).unwrap();
                assert!(on_segment.distance(on_curve) < 1e-12, "[{a}, {b}] at {s}");
            }
        }
        let (raised, points) = elevate_degree(&knots, &control, T).unwrap();
        assert_eq!(raised.domain(), knots.domain());
        for i in 0..=30 {
            let u = 3.0 + f64::from(i) / 10.0;
            let moved = evaluate(&raised, &points, u, T)
                .unwrap()
                .distance(evaluate(&knots, &control, u, T).unwrap());
            assert!(moved < 1e-12, "elevation moved the curve {moved} at {u}");
        }
    }

    #[test]
    fn bezier_decomposition_covers_the_curve_exactly() {
        let (k, c) = cubic_curve();
        let segments = to_bezier_segments(&k, &c, T).unwrap();
        // Two interior knots means three segments.
        assert_eq!(segments.len(), 3);
        for (_, points) in &segments {
            assert_eq!(points.len(), k.degree() + 1);
        }

        // Each segment, evaluated as a Bezier, must match the original curve
        // over its own interval.
        for ((a, b), points) in &segments {
            let bezier = KnotVector::clamped_uniform(k.degree(), points.len())
                .unwrap()
                .reparameterized(*a, *b)
                .unwrap();
            for i in 0..=20 {
                let u = a + (b - a) * (f64::from(i) / 20.0);
                assert!(
                    evaluate(&bezier, points, u, T)
                        .unwrap()
                        .is_equal(evaluate(&k, &c, u, T).unwrap(), T),
                    "segment [{a}, {b}] diverges at {u}"
                );
            }
        }
    }

    #[test]
    fn degree_elevation_does_not_move_the_curve() {
        let (k, c) = cubic_curve();
        let before = sample(&k, &c, 100);
        let (k2, c2) = elevate_degree(&k, &c, T).unwrap();
        assert_eq!(k2.degree(), k.degree() + 1);
        assert_eq!(k2.domain(), k.domain());

        let after = sample(&k2, &c2, 100);
        for (a, b) in before.iter().zip(&after) {
            assert!(a.is_equal(*b, T), "elevation moved the curve");
        }
    }

    #[test]
    fn elevation_keeps_the_continuity_at_every_knot() {
        let (k, c) = cubic_curve();
        let (k2, c2) = elevate_degree(&k, &c, T).unwrap();
        let interior = |v: &KnotVector| -> Vec<(f64, usize)> {
            let (a, b) = v.domain();
            v.distinct()
                .into_iter()
                .filter(|(x, _)| *x > a && *x < b)
                .collect()
        };
        let raised: Vec<(f64, usize)> = interior(&k).into_iter().map(|(x, m)| (x, m + 1)).collect();
        assert_eq!(interior(&k2), raised);
        let before = sample(&k, &c, 100);
        for (a, b) in before.iter().zip(&sample(&k2, &c2, 100)) {
            assert!(a.distance(*b) < 1e-12, "elevation moved the curve");
        }
        // Second derivatives agree from either side of a knot.
        for (x, _) in interior(&k2) {
            let left = derivatives(&k2, &c2, x - 1e-9, 2, T).unwrap()[2];
            let right = derivatives(&k2, &c2, x + 1e-9, 2, T).unwrap()[2];
            assert!(
                left.distance(right) < 1e-5,
                "{left:?} against {right:?} at {x}"
            );
        }
    }

    #[test]
    fn elevation_twice_is_still_the_same_curve() {
        let (k, c) = cubic_curve();
        let before = sample(&k, &c, 60);
        let (k1, c1) = elevate_degree(&k, &c, T).unwrap();
        let (k2, c2) = elevate_degree(&k1, &c1, T).unwrap();
        assert_eq!(k2.degree(), 5);
        for (a, b) in before.iter().zip(&sample(&k2, &c2, 60)) {
            assert!(a.is_equal(*b, T));
        }
    }

    #[test]
    fn reversal_traverses_the_same_points_backwards() {
        let (k, c) = cubic_curve();
        let (rk, rc) = reverse(&k, &c);
        let (a, b) = k.domain();
        for i in 0..=40 {
            let t = f64::from(i) / 40.0;
            let forward = evaluate(&k, &c, a + (b - a) * t, T).unwrap();
            let backward = evaluate(&rk, &rc, a + (b - a) * (1.0 - t), T).unwrap();
            assert!(forward.is_equal(backward, T), "at t = {t}");
        }
    }

    /// A quarter circle, exactly, as a rational quadratic. This is the reason
    /// rational geometry exists: no polynomial curve is a circular arc.
    fn quarter_circle() -> (KnotVector, Vec<Weighted<Point>>) {
        let w = core::f64::consts::FRAC_1_SQRT_2;
        let control = vec![
            Weighted::new(Point::new(1.0, 0.0, 0.0), 1.0, T).unwrap(),
            Weighted::new(Point::new(1.0, 1.0, 0.0), w, T).unwrap(),
            Weighted::new(Point::new(0.0, 1.0, 0.0), 1.0, T).unwrap(),
        ];
        (KnotVector::clamped_uniform(2, 3).unwrap(), control)
    }

    #[test]
    fn a_rational_quadratic_traces_an_exact_circular_arc() {
        let (k, c) = quarter_circle();
        for i in 0..=100 {
            let u = f64::from(i) / 100.0;
            let p = evaluate_rational(&k, &c, u, T).unwrap();
            // Every point is at exactly unit distance from the origin, which
            // no non-rational B-spline can achieve.
            assert_relative_eq!(p.to_vector().magnitude(), 1.0, epsilon = 1e-14);
            assert_relative_eq!(p.z, 0.0, epsilon = 1e-15);
        }
        assert!(
            evaluate_rational(&k, &c, 0.0, T)
                .unwrap()
                .is_equal(Point::new(1.0, 0.0, 0.0), T)
        );
        assert!(
            evaluate_rational(&k, &c, 1.0, T)
                .unwrap()
                .is_equal(Point::new(0.0, 1.0, 0.0), T)
        );
    }

    #[test]
    fn rational_derivatives_agree_with_finite_differences() {
        let (k, c) = quarter_circle();
        let h = 1e-6;
        for i in 1..20 {
            let u = f64::from(i) / 20.0;
            let d = rational_derivatives(&k, &c, u, 2, T).unwrap();
            assert!(d[0].is_equal(evaluate_rational(&k, &c, u, T).unwrap(), T));

            let ahead = evaluate_rational(&k, &c, u + h, T).unwrap();
            let behind = evaluate_rational(&k, &c, u - h, T).unwrap();
            let numeric = (ahead - behind) * (1.0 / (2.0 * h));
            assert!(
                (d[1].to_vector() - numeric).magnitude() < 1e-5,
                "at u = {u}: {:?} vs {numeric:?}",
                d[1]
            );
        }
    }

    #[test]
    fn the_tangent_of_a_circular_arc_is_perpendicular_to_its_radius() {
        let (k, c) = quarter_circle();
        for i in 0..=20 {
            let u = f64::from(i) / 20.0;
            let d = rational_derivatives(&k, &c, u, 1, T).unwrap();
            let radius = d[0].to_vector();
            let tangent = d[1].to_vector();
            assert!(
                radius.dot(tangent).abs() < 1e-12,
                "not perpendicular at {u}: {}",
                radius.dot(tangent)
            );
        }
    }

    #[test]
    fn knot_insertion_preserves_a_rational_curve_too() {
        let (k, c) = quarter_circle();
        let (k2, c2) = insert_knot(&k, &c, 0.5, 1, T).unwrap();
        for i in 0..=50 {
            let u = f64::from(i) / 50.0;
            let a = evaluate_rational(&k, &c, u, T).unwrap();
            let b = evaluate_rational(&k2, &c2, u, T).unwrap();
            assert!(a.is_equal(b, T), "at {u}");
            assert_relative_eq!(b.to_vector().magnitude(), 1.0, epsilon = 1e-14);
        }
    }

    #[test]
    fn degenerate_weights_are_refused() {
        assert!(Weighted::new(Point::ORIGIN, 0.0, T).is_err());
        assert!(Weighted::new(Point::ORIGIN, -1.0, T).is_err());
        assert!(Weighted::new(Point::ORIGIN, f64::NAN, T).is_err());
        assert!(Weighted::new(Point::ORIGIN, f64::INFINITY, T).is_err());
        assert!(Weighted::new(Point::ORIGIN, 2.0, T).is_ok());
    }

    #[test]
    fn weighted_round_trips_through_its_homogeneous_form() {
        let p = Point::new(3.0, -1.0, 2.0);
        let w = Weighted::new(p, 2.5, T).unwrap();
        assert!(w.point().is_equal(p, T));
        assert!(w.scaled.is_equal(Point::new(7.5, -2.5, 5.0), T));
    }

    #[test]
    fn binomial_coefficients() {
        assert_eq!(binomial_coefficient(0, 0), 1);
        assert_eq!(binomial_coefficient(5, 0), 1);
        assert_eq!(binomial_coefficient(5, 5), 1);
        assert_eq!(binomial_coefficient(5, 2), 10);
        assert_eq!(binomial_coefficient(10, 5), 252);
        assert_eq!(binomial_coefficient(3, 4), 0);
    }

    #[test]
    fn scalar_and_planar_control_points_work_too() {
        // The Blend abstraction has to serve every control point type, not just
        // 3D positions.
        let k = KnotVector::clamped_uniform(2, 4).unwrap();
        let scalars = vec![0.0_f64, 1.0, 3.0, 2.0];
        assert_relative_eq!(evaluate(&k, &scalars, 0.0, T).unwrap(), 0.0);
        assert_relative_eq!(evaluate(&k, &scalars, 1.0, T).unwrap(), 2.0);

        let planar = vec![
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 2.0),
            Point2::new(3.0, 1.0),
            Point2::new(4.0, 0.0),
        ];
        assert!(
            evaluate(&k, &planar, 0.0, T)
                .unwrap()
                .is_equal(planar[0], T)
        );
        assert!(
            evaluate(&k, &planar, 1.0, T)
                .unwrap()
                .is_equal(planar[3], T)
        );
    }
}

/// A rectangular grid of control points for a tensor-product surface.
///
/// Stored row-major: `points[i * v_count + j]` is the point at `u` index `i` and
/// `v` index `j`. Carrying the shape with the data means the surface functions
/// cannot be handed a grid with the wrong stride, which is the mistake that
/// otherwise produces a plausible but transposed surface.
#[derive(Debug, Clone, PartialEq)]
pub struct ControlGrid<P> {
    points: Vec<P>,
    u_count: usize,
    v_count: usize,
}

impl<P: Blend> ControlGrid<P> {
    /// A grid from row-major points.
    ///
    /// # Errors
    ///
    /// [`OgeomError::Dimension`](ogeom_core::OgeomError::Dimension) if the point count
    /// is not `u_count * v_count`, or either count is zero.
    pub fn new(points: Vec<P>, u_count: usize, v_count: usize) -> OgeomResult<Self> {
        if u_count == 0 || v_count == 0 {
            ogeom_bail!(Dimension, "control grid must be at least 1x1");
        }
        if points.len() != u_count * v_count {
            ogeom_bail!(
                Dimension,
                "a {u_count}x{v_count} grid needs {} points, got {}",
                u_count * v_count,
                points.len()
            );
        }
        Ok(Self {
            points,
            u_count,
            v_count,
        })
    }

    /// Number of control points along `u`.
    #[must_use]
    pub const fn u_count(&self) -> usize {
        self.u_count
    }

    /// Number of control points along `v`.
    #[must_use]
    pub const fn v_count(&self) -> usize {
        self.v_count
    }

    /// The point at `(i, j)`, or `None` if either index is out of range.
    #[must_use]
    pub fn get(&self, i: usize, j: usize) -> Option<P> {
        if i >= self.u_count || j >= self.v_count {
            return None;
        }
        self.points.get(i * self.v_count + j).copied()
    }

    /// All points, row-major.
    #[must_use]
    pub fn points(&self) -> &[P] {
        &self.points
    }

    /// This grid with `u` and `v` exchanged.
    #[must_use]
    pub fn transposed(&self) -> Self {
        let mut points = Vec::with_capacity(self.points.len());
        for j in 0..self.v_count {
            for i in 0..self.u_count {
                points.push(self.points[i * self.v_count + j]);
            }
        }
        Self {
            points,
            u_count: self.v_count,
            v_count: self.u_count,
        }
    }

    /// Apply `f` to every point.
    #[must_use]
    pub fn map<Q: Blend>(&self, f: impl Fn(P) -> Q) -> ControlGrid<Q> {
        ControlGrid {
            points: self.points.iter().map(|p| f(*p)).collect(),
            u_count: self.u_count,
            v_count: self.v_count,
        }
    }
}

/// Check that a grid's shape matches its two knot vectors.
fn check_grid_shape<P>(ku: &KnotVector, kv: &KnotVector, grid: &ControlGrid<P>) -> OgeomResult<()> {
    if grid.u_count != ku.control_point_count() || grid.v_count != kv.control_point_count() {
        ogeom_bail!(
            Dimension,
            "knot vectors describe a {}x{} grid, got {}x{}",
            ku.control_point_count(),
            kv.control_point_count(),
            grid.u_count,
            grid.v_count
        );
    }
    Ok(())
}

/// Evaluate a tensor-product B-spline surface at `(u, v)`.
///
/// Sums the `(p+1) x (q+1)` non-zero basis products over the control window.
/// Only that window contributes (the basis has local support), so cost depends
/// on the degrees, not on the size of the surface.
///
/// # Errors
///
/// [`OgeomError::Dimension`](ogeom_core::OgeomError::Dimension) on a shape mismatch, and
/// [`OgeomError::Domain`](ogeom_core::OgeomError::Domain) if a parameter is outside its
/// knot vector's domain.
pub fn evaluate_surface<P: Blend>(
    ku: &KnotVector,
    kv: &KnotVector,
    grid: &ControlGrid<P>,
    u: f64,
    v: f64,
    tol: Tolerances,
) -> OgeomResult<P> {
    check_grid_shape(ku, kv, grid)?;
    let (p, q) = (ku.degree(), kv.degree());
    let (su, sv) = (ku.span(u, tol)?, kv.span(v, tol)?);
    let (nu, nv) = (ku.basis(su, u), kv.basis(sv, v));

    let mut total = P::zero();
    for (i, &weight_u) in nu.iter().enumerate() {
        // Accumulate along v first, then weight the row: one multiply per row
        // instead of one per point.
        let mut row = P::zero();
        for (j, &weight_v) in nv.iter().enumerate() {
            let Some(point) = grid.get(su - p + i, sv - q + j) else {
                ogeom_bail!(Dimension, "control grid index out of range");
            };
            row = row.add(point.scale(weight_v));
        }
        total = total.add(row.scale(weight_u));
    }
    Ok(total)
}

/// Evaluate a surface and its partial derivatives up to total order `order`.
///
/// `result[k][l]` is the derivative taken `k` times in `u` and `l` times in `v`,
/// so `result[0][0]` is the point itself, for `k + l <= order`; entries of
/// higher total order are left zero rather than computed.
///
/// # Errors
///
/// As [`evaluate_surface`].
pub fn surface_derivatives<P: Blend>(
    ku: &KnotVector,
    kv: &KnotVector,
    grid: &ControlGrid<P>,
    u: f64,
    v: f64,
    order: usize,
    tol: Tolerances,
) -> OgeomResult<DerivativeGrid<P>> {
    check_grid_shape(ku, kv, grid)?;
    let (p, q) = (ku.degree(), kv.degree());
    let (su, sv) = (ku.span(u, tol)?, kv.span(v, tol)?);
    let du = ku.basis_derivatives(su, u, order);
    let dv = kv.basis_derivatives(sv, v, order);

    let mut out: DerivativeGrid<P> =
        core::iter::repeat_with(|| core::iter::repeat_with(P::zero).take(order + 1).collect())
            .take(order + 1)
            .collect();
    // Each control row's sum across v, per v order: the same for every u
    // order, so summed once. Only the orders a caller can use are filled,
    // those of total order at most `order`; the rest stay zero.
    let mut across: SmallVec<[SmallVec<[P; 8]>; 4]> = SmallVec::new();
    for weights_v in dv.iter().take(order + 1) {
        let mut row: SmallVec<[P; 8]> = SmallVec::new();
        for i in 0..=p {
            let mut inner = P::zero();
            for (j, &weight_v) in weights_v.iter().enumerate() {
                let Some(point) = grid.get(su - p + i, sv - q + j) else {
                    ogeom_bail!(Dimension, "control grid index out of range");
                };
                inner = inner.add(point.scale(weight_v));
            }
            row.push(inner);
        }
        across.push(row);
    }
    for (k, row) in out.iter_mut().enumerate() {
        for (l, cell) in row.iter_mut().enumerate().take(order + 1 - k) {
            // Derivatives past the degree in either direction vanish, and the
            // basis returns them as exact zeros, so this sums to zero without
            // needing a special case.
            let mut total = P::zero();
            for (i, &weight_u) in du[k].iter().enumerate() {
                total = total.add(across[l][i].scale(weight_u));
            }
            *cell = total;
        }
    }
    Ok(out)
}

/// Evaluate a rational tensor-product surface: homogeneous evaluation, then
/// divide through.
///
/// # Errors
///
/// As [`evaluate_surface`], plus
/// [`OgeomError::Numeric`](ogeom_core::OgeomError::Numeric) if the accumulated weight
/// vanishes, which positive input weights make impossible.
pub fn evaluate_rational_surface<P: Blend>(
    ku: &KnotVector,
    kv: &KnotVector,
    grid: &ControlGrid<Weighted<P>>,
    u: f64,
    v: f64,
    tol: Tolerances,
) -> OgeomResult<P> {
    let h = evaluate_surface(ku, kv, grid, u, v, tol)?;
    if h.weight.abs() <= tol.confusion() {
        ogeom_bail!(
            Numeric,
            "rational surface evaluation produced a vanishing weight"
        );
    }
    Ok(h.point())
}

/// Evaluate a rational surface and its partial derivatives up to total order
/// `order`.
///
/// The two-parameter quotient rule. Each mixed partial subtracts the weight's
/// influence in `u`, in `v`, and in both together; dropping the last of those
/// three sums is the classic error, and it only shows up on genuinely rational
/// surfaces with mixed derivatives, which is to say, on exactly the spheres and
/// tori where the answer matters.
///
/// # Errors
///
/// As [`evaluate_rational_surface`].
pub fn rational_surface_derivatives<P: Blend>(
    ku: &KnotVector,
    kv: &KnotVector,
    grid: &ControlGrid<Weighted<P>>,
    u: f64,
    v: f64,
    order: usize,
    tol: Tolerances,
) -> OgeomResult<DerivativeGrid<P>> {
    let h = surface_derivatives(ku, kv, grid, u, v, order, tol)?;
    let w0 = h[0][0].weight;
    if w0.abs() <= tol.confusion() {
        ogeom_bail!(
            Numeric,
            "rational surface evaluation produced a vanishing weight"
        );
    }

    let mut s: DerivativeGrid<P> =
        core::iter::repeat_with(|| core::iter::repeat_with(P::zero).take(order + 1).collect())
            .take(order + 1)
            .collect();
    for k in 0..=order {
        // Each value reads only lower orders in both directions, so the
        // total order's bound holds the recurrence closed.
        for l in 0..=order - k {
            let mut value = h[k][l].scaled;
            #[allow(clippy::cast_precision_loss)]
            for i in 1..=k {
                let c = binomial_coefficient(k, i) as f64;
                value = value.sub(s[k - i][l].scale(c * h[i][0].weight));
            }
            #[allow(clippy::cast_precision_loss)]
            for j in 1..=l {
                let c = binomial_coefficient(l, j) as f64;
                value = value.sub(s[k][l - j].scale(c * h[0][j].weight));
            }
            #[allow(clippy::cast_precision_loss)]
            for i in 1..=k {
                let ci = binomial_coefficient(k, i) as f64;
                for j in 1..=l {
                    let cj = binomial_coefficient(l, j) as f64;
                    value = value.sub(s[k - i][l - j].scale(ci * cj * h[i][j].weight));
                }
            }
            s[k][l] = value.scale(1.0 / w0);
        }
    }
    Ok(s)
}

#[cfg(test)]
#[allow(clippy::unwrap_used)]
mod surface_tests {
    use super::*;
    use approx::assert_relative_eq;

    const T: Tolerances = Tolerances::millimetres();

    /// A bicubic patch with some genuine curvature.
    fn patch() -> (KnotVector, KnotVector, ControlGrid<Point>) {
        let (nu, nv) = (5, 4);
        let mut points = Vec::with_capacity(nu * nv);
        for i in 0..nu {
            for j in 0..nv {
                #[allow(clippy::cast_precision_loss)]
                let (x, y) = (i as f64, j as f64);
                points.push(Point::new(x, y, (x * 0.7).sin() * (y * 0.5).cos()));
            }
        }
        (
            KnotVector::clamped_uniform(3, nu).unwrap(),
            KnotVector::clamped_uniform(2, nv).unwrap(),
            ControlGrid::new(points, nu, nv).unwrap(),
        )
    }

    #[test]
    fn grid_shape_is_checked_on_construction() {
        assert!(ControlGrid::new(vec![Point::ORIGIN; 6], 2, 3).is_ok());
        assert!(ControlGrid::new(vec![Point::ORIGIN; 6], 3, 3).is_err());
        assert!(ControlGrid::new(Vec::<Point>::new(), 0, 3).is_err());
    }

    #[test]
    fn grid_indexing_is_row_major_and_bounds_checked() {
        let g = ControlGrid::new(
            vec![
                Point::new(0.0, 0.0, 0.0),
                Point::new(0.0, 1.0, 0.0),
                Point::new(0.0, 2.0, 0.0),
                Point::new(1.0, 0.0, 0.0),
                Point::new(1.0, 1.0, 0.0),
                Point::new(1.0, 2.0, 0.0),
            ],
            2,
            3,
        )
        .unwrap();
        assert_eq!(g.get(1, 2), Some(Point::new(1.0, 2.0, 0.0)));
        assert_eq!(g.get(0, 1), Some(Point::new(0.0, 1.0, 0.0)));
        assert_eq!(g.get(2, 0), None);
        assert_eq!(g.get(0, 3), None);
    }

    #[test]
    fn transposing_twice_is_the_identity() {
        let (_, _, g) = patch();
        let t = g.transposed();
        assert_eq!(t.u_count(), g.v_count());
        assert_eq!(t.v_count(), g.u_count());
        for i in 0..g.u_count() {
            for j in 0..g.v_count() {
                assert_eq!(t.get(j, i), g.get(i, j));
            }
        }
        assert_eq!(t.transposed(), g);
    }

    #[test]
    fn a_clamped_patch_interpolates_its_corner_control_points() {
        let (ku, kv, g) = patch();
        let ((u0, u1), (v0, v1)) = (ku.domain(), kv.domain());
        let corners = [
            (u0, v0, g.get(0, 0).unwrap()),
            (u0, v1, g.get(0, g.v_count() - 1).unwrap()),
            (u1, v0, g.get(g.u_count() - 1, 0).unwrap()),
            (u1, v1, g.get(g.u_count() - 1, g.v_count() - 1).unwrap()),
        ];
        for (u, v, expected) in corners {
            assert!(
                evaluate_surface(&ku, &kv, &g, u, v, T)
                    .unwrap()
                    .is_equal(expected, T),
                "corner ({u}, {v})"
            );
        }
    }

    #[test]
    fn surface_shape_mismatches_are_refused() {
        let (ku, kv, g) = patch();
        let wrong = ControlGrid::new(g.points().to_vec(), 4, 5).unwrap();
        assert!(evaluate_surface(&ku, &kv, &wrong, 0.5, 0.5, T).is_err());
        assert!(evaluate_surface(&ku, &kv, &g, 1.5, 0.5, T).is_err());
        assert!(evaluate_surface(&ku, &kv, &g, 0.5, -0.5, T).is_err());
    }

    #[test]
    fn surface_partials_agree_with_finite_differences() {
        let (ku, kv, g) = patch();
        let h = 1e-6;
        for iu in 1..6 {
            for iv in 1..6 {
                let (u, v) = (f64::from(iu) / 6.0, f64::from(iv) / 6.0);
                let d = surface_derivatives(&ku, &kv, &g, u, v, 2, T).unwrap();
                assert!(d[0][0].is_equal(evaluate_surface(&ku, &kv, &g, u, v, T).unwrap(), T));

                let du = (evaluate_surface(&ku, &kv, &g, u + h, v, T).unwrap()
                    - evaluate_surface(&ku, &kv, &g, u - h, v, T).unwrap())
                    * (1.0 / (2.0 * h));
                let dv = (evaluate_surface(&ku, &kv, &g, u, v + h, T).unwrap()
                    - evaluate_surface(&ku, &kv, &g, u, v - h, T).unwrap())
                    * (1.0 / (2.0 * h));
                assert!((d[1][0].to_vector() - du).magnitude() < 1e-5 * du.magnitude().max(1.0));
                assert!((d[0][1].to_vector() - dv).magnitude() < 1e-5 * dv.magnitude().max(1.0));

                // The mixed partial, which the naive quotient rule drops.
                let mixed = (evaluate_surface(&ku, &kv, &g, u + h, v + h, T).unwrap()
                    - evaluate_surface(&ku, &kv, &g, u + h, v - h, T).unwrap()
                    - (evaluate_surface(&ku, &kv, &g, u - h, v + h, T).unwrap()
                        - evaluate_surface(&ku, &kv, &g, u - h, v - h, T).unwrap()))
                    * (1.0 / (4.0 * h * h));
                assert!(
                    (d[1][1].to_vector() - mixed).magnitude() < 1e-3 * mixed.magnitude().max(1.0),
                    "mixed partial wrong at ({u}, {v})"
                );
            }
        }
    }

    /// A hemisphere, exactly, as a rational biquadratic. Only a rational
    /// surface can be one.
    fn rational_hemisphere() -> (KnotVector, KnotVector, ControlGrid<Weighted<Point>>) {
        let w = core::f64::consts::FRAC_1_SQRT_2;
        // A quarter arc in u, swept through a quarter turn in v.
        let rows: [[(Point, f64); 3]; 3] = [
            [
                (Point::new(1.0, 0.0, 0.0), 1.0),
                (Point::new(1.0, 1.0, 0.0), w),
                (Point::new(0.0, 1.0, 0.0), 1.0),
            ],
            [
                (Point::new(1.0, 0.0, 1.0), w),
                (Point::new(1.0, 1.0, 1.0), w * w),
                (Point::new(0.0, 1.0, 1.0), w),
            ],
            [
                (Point::new(0.0, 0.0, 1.0), 1.0),
                (Point::new(0.0, 0.0, 1.0), w),
                (Point::new(0.0, 0.0, 1.0), 1.0),
            ],
        ];
        let points: Vec<_> = rows
            .iter()
            .flatten()
            .map(|(p, w)| Weighted::new(*p, *w, T).unwrap())
            .collect();
        (
            KnotVector::clamped_uniform(2, 3).unwrap(),
            KnotVector::clamped_uniform(2, 3).unwrap(),
            ControlGrid::new(points, 3, 3).unwrap(),
        )
    }

    #[test]
    fn a_rational_biquadratic_traces_an_exact_sphere() {
        let (ku, kv, g) = rational_hemisphere();
        for iu in 0..=10 {
            for iv in 0..=10 {
                let (u, v) = (f64::from(iu) / 10.0, f64::from(iv) / 10.0);
                let p = evaluate_rational_surface(&ku, &kv, &g, u, v, T).unwrap();
                assert_relative_eq!(
                    p.to_vector().magnitude(),
                    1.0,
                    epsilon = 1e-13,
                    max_relative = 1e-13
                );
            }
        }
    }

    #[test]
    fn rational_surface_partials_agree_with_finite_differences() {
        let (ku, kv, g) = rational_hemisphere();
        let h = 1e-6;
        let at = |u: f64, v: f64| evaluate_rational_surface(&ku, &kv, &g, u, v, T).unwrap();
        for iu in 1..6 {
            for iv in 1..6 {
                let (u, v) = (f64::from(iu) / 6.0, f64::from(iv) / 6.0);
                let d = rational_surface_derivatives(&ku, &kv, &g, u, v, 2, T).unwrap();
                assert!(d[0][0].is_equal(at(u, v), T));

                let du = (at(u + h, v) - at(u - h, v)) * (1.0 / (2.0 * h));
                let dv = (at(u, v + h) - at(u, v - h)) * (1.0 / (2.0 * h));
                assert!(
                    (d[1][0].to_vector() - du).magnitude() < 1e-5 * du.magnitude().max(1.0),
                    "du wrong at ({u}, {v})"
                );
                assert!(
                    (d[0][1].to_vector() - dv).magnitude() < 1e-5 * dv.magnitude().max(1.0),
                    "dv wrong at ({u}, {v})"
                );

                // The mixed partial is where the cross term in the two-parameter
                // quotient rule matters; without it this is visibly wrong.
                let mixed =
                    (at(u + h, v + h) - at(u + h, v - h) - (at(u - h, v + h) - at(u - h, v - h)))
                        * (1.0 / (4.0 * h * h));
                assert!(
                    (d[1][1].to_vector() - mixed).magnitude() < 1e-2 * mixed.magnitude().max(1.0),
                    "mixed partial wrong at ({u}, {v}): {:?} vs {mixed:?}",
                    d[1][1]
                );
            }
        }
    }

    #[test]
    fn a_spheres_normal_is_radial() {
        // Independent of the derivative formulas: on a unit sphere centred at
        // the origin, du x dv must be parallel to the position vector.
        let (ku, kv, g) = rational_hemisphere();
        for iu in 1..8 {
            for iv in 1..8 {
                let (u, v) = (f64::from(iu) / 8.0, f64::from(iv) / 8.0);
                let d = rational_surface_derivatives(&ku, &kv, &g, u, v, 1, T).unwrap();
                let radius = d[0][0].to_vector();
                let normal = d[1][0].to_vector().cross(d[0][1].to_vector());
                assert!(
                    normal.magnitude() > 1e-6,
                    "degenerate tangents at ({u}, {v})"
                );
                let sine =
                    radius.cross(normal).magnitude() / (radius.magnitude() * normal.magnitude());
                assert!(sine < 1e-9, "normal not radial at ({u}, {v}): sine {sine}");
            }
        }
    }

    #[test]
    fn uniform_weights_reduce_to_the_polynomial_surface() {
        let (ku, kv, g) = patch();
        let weighted = g.map(|p| Weighted {
            scaled: p.scale(2.0),
            weight: 2.0,
        });
        for iu in 0..=6 {
            for iv in 0..=6 {
                let (u, v) = (f64::from(iu) / 6.0, f64::from(iv) / 6.0);
                let plain = evaluate_surface(&ku, &kv, &g, u, v, T).unwrap();
                let rational = evaluate_rational_surface(&ku, &kv, &weighted, u, v, T).unwrap();
                assert!(plain.is_equal(rational, T));
            }
        }
    }
}