geop_core_geometry/nurb_curve/

compatible.rs

//! Making curves *compatible* — one degree, one knot vector — so that they
//! can be the rows of one surface, as skinning a surface through several
//! curves needs: raising a curve's degree and refining its knots, neither of
//! which changes the curve, and bringing a curve onto the domain `[0, 1]`.

use geop_core_math::{
    geop_error::{GeopError, GeopResult},
    scalars::Scalar,
    vector::Vector,
};

use super::NurbCurve;
use crate::{
    aabb::compute_aabb,
    knot_insertion::{insert, is_clamped_at},
    spline::interior_knots,
};

impl<const D: usize> NurbCurve<D> {
    /// The same curve reparametrized from its domain onto `[0, 1]`: its
    /// knots mapped linearly, its control points unchanged. A curve already
    /// on exactly `[0, 1]` is returned as it is: mapping its knots would
    /// only round them outwards.
    pub fn with_unit_domain(&self) -> GeopResult<Self> {
        let (t0, t1) = self.domain();
        let exactly = |t: Scalar, x: Scalar| t.is_subset_of(x) && x.is_subset_of(t);
        if exactly(t0, Scalar::ZERO) && exactly(t1, Scalar::ONE) {
            return Ok(self.clone());
        }
        let span = t1.sub(t0);
        let knots = self
            .knot_vector
            .iter()
            .map(|&k| k.sub(t0).div(span))
            .collect::<GeopResult<Vec<Scalar>>>()?;
        NurbCurve::try_new(self.degree, self.control_points.clone(), knots)
    }

    /// The same curve one degree higher, for a curve clamped at both ends.
    ///
    /// Cut into its Bézier pieces first — every interior knot inserted up to
    /// multiplicity `degree` — each piece is raised on its own:
    /// `Q_i = i/(p+1) P_{i-1} + (1 - i/(p+1)) P_i` on the homogeneous control
    /// points, exact for rational curves too. The pieces stay joined as they
    /// were, every interior knot now of multiplicity `degree + 1`.
    pub fn elevate_degree(&self) -> GeopResult<Self> {
        let p = self.degree;
        let n = self.control_points.len();
        if !(is_clamped_at(&self.knot_vector, p, true)
            && is_clamped_at(&self.knot_vector, p, false))
        {
            return Err(GeopError::new(format!(
                "elevate_degree: the knot vector {:?} is not clamped at both ends",
                self.knot_vector
            )));
        }
        let interior = interior_knots(&self.knot_vector, p, n);
        let mut knots = self.knot_vector.clone();
        let mut rows = [self.control_points.clone()];
        for &(u, m) in &interior {
            if m > p {
                return Err(GeopError::new(format!(
                    "elevate_degree: the knot {u:?} has multiplicity {m}, more than the degree {p}"
                )));
            }
            for _ in m..p {
                insert(&mut knots, &mut rows, p, u);
            }
        }
        let [pts] = rows;
        let pieces = interior.len() + 1;
        debug_assert_eq!(pts.len(), p * pieces + 1);
        let mut out: Vec<Vector<D>> = Vec::with_capacity((p + 1) * pieces + 1);
        out.push(pts[0]);
        for piece in pts.windows(p + 1).step_by(p.max(1)) {
            for i in 1..=p {
                let alpha = Scalar::from_ratio(i as i64, (p + 1) as i64)?;
                out.push(Vector::interpolate(&piece[i], &piece[i - 1], alpha));
            }
            out.push(piece[p]);
        }
        let (start, end) = self.domain();
        let mut new_knots = vec![start; p + 2];
        for &(u, _) in &interior {
            new_knots.extend(std::iter::repeat_n(u, p + 1));
        }
        new_knots.extend(std::iter::repeat_n(end, p + 2));
        NurbCurve::try_new(p + 1, out, new_knots)
    }

    /// The same single-span (Bézier) curve with its end weights one, as a
    /// clamped curve's ends are in a profile — what a split of a rational
    /// curve, an arc, leaves otherwise.
    ///
    /// Scaling the `i`-th weight by `c^i` — every homogeneous control point
    /// with it — reparametrizes a rational Bézier curve without changing it,
    /// for any `c > 0`: `c^p = w_0 / w_p` brings both ends to one weight,
    /// and dividing by `w_0` makes that one. `c` is a free choice; the ends
    /// are divided by their own weights, so they come out at one whatever
    /// `c` rounded to. A curve whose ends already weigh exactly one is
    /// returned as it is.
    pub fn with_unit_end_weights(&self) -> GeopResult<Self> {
        let p = self.degree;
        let n = self.control_points.len();
        let weight = |i: usize| self.control_points[i][D - 1];
        if weight(0).is_exactly(Scalar::ONE) && weight(n - 1).is_exactly(Scalar::ONE) {
            return Ok(self.clone());
        }
        if n != p + 1 {
            return Err(GeopError::new(format!(
                "with_unit_end_weights: a curve of {n} control points and degree {p} is not a single span, and its end weights {:?}, {:?} are not one",
                weight(0),
                weight(n - 1)
            )));
        }
        let c = (weight(0).to_f64() / weight(n - 1).to_f64()).powf(1.0 / p as f64);
        let control_points = (0..n)
            .map(|i| {
                let factor = if i == 0 {
                    Scalar::ONE.div(weight(0))?
                } else if i == n - 1 {
                    Scalar::ONE.div(weight(n - 1))?
                } else {
                    Scalar::from_f64(c.powi(i as i32)).div(weight(0))?
                };
                Ok(self.control_points[i].prod_scalar(factor))
            })
            .collect::<GeopResult<Vec<_>>>()?;
        NurbCurve::try_new(p, control_points, self.knot_vector.clone())
    }

    /// `curves`, on one domain and clamped, made compatible: each raised to
    /// the highest degree among them, and given every knot any of them has,
    /// at the highest multiplicity any of them has it — so that they all end
    /// up on one knot vector. Every curve stays the curve it was.
    ///
    /// Knots of different curves that could be equal are taken as one, the
    /// union of their enclosures, which every curve then carries: an honest
    /// enclosure of each curve's own knot.
    pub fn compatible(curves: &[Self]) -> GeopResult<Vec<Self>> {
        let Some(first) = curves.first() else {
            return Ok(Vec::new());
        };
        let (start, end) = first.domain();
        let degree = curves.iter().map(|c| c.degree).max().unwrap_or(0);
        let mut out = Vec::with_capacity(curves.len());
        for curve in curves {
            let (s, e) = curve.domain();
            if !(s.could_be_equal(start) && e.could_be_equal(end)) {
                return Err(GeopError::new(format!(
                    "compatible: domains ({s:?}, {e:?}) and ({start:?}, {end:?}) differ"
                )));
            }
            let mut curve = curve.clone();
            // Each raise one degree.
            for _ in curve.degree..degree {
                curve = curve.elevate_degree()?;
            }
            out.push(curve);
        }

        let mut merged: Vec<(Scalar, usize)> = Vec::new();
        for curve in &out {
            for (u, m) in interior_knots(&curve.knot_vector, degree, curve.control_points.len()) {
                match merged.iter_mut().find(|(v, _)| v.could_be_equal(u)) {
                    Some((v, k)) => {
                        *v = v.union(u);
                        *k = (*k).max(m);
                    }
                    None => merged.push((u, m)),
                }
            }
        }
        for curve in &mut out {
            let mut knots = curve.knot_vector.clone();
            let mut rows = [curve.control_points.clone()];
            for &(u, m) in &merged {
                let have = interior_knots(&knots, degree, rows[0].len())
                    .iter()
                    .find(|(v, _)| v.could_be_equal(u))
                    .map_or(0, |&(_, k)| k);
                for _ in have..m {
                    insert(&mut knots, &mut rows, degree, u);
                }
            }
            let [pts] = rows;
            curve.control_points = pts;
            curve.knot_vector = knots;
        }

        // One knot vector for all: each knot the union of every curve's.
        let len = out[0].knot_vector.len();
        if out.iter().any(|c| c.knot_vector.len() != len) {
            return Err(GeopError::new(format!(
                "compatible: knot vectors of different lengths after merging: {:?}",
                out.iter()
                    .map(|c| c.knot_vector.clone())
                    .collect::<Vec<_>>()
            )));
        }
        let common: Vec<Scalar> = (0..len)
            .map(|k| {
                out.iter()
                    .map(|c| c.knot_vector[k])
                    .reduce(|a, b| a.union(b))
                    .expect("at least one curve")
            })
            .collect();
        for curve in &mut out {
            curve.knot_vector = common.clone();
            curve.aabb = compute_aabb(&curve.control_points);
        }
        Ok(out)
    }

    /// The chain `curves`, each starting where the one before ends, as one
    /// curve on `[0, 1]`, the `i`-th of `n` curves on `[i / n, (i + 1) / n]`
    /// (see [`Self::join_at`]).
    pub fn join(curves: &[Self]) -> GeopResult<Self> {
        let n = curves.len();
        if n == 1 {
            return curves[0].with_unit_domain()?.with_unit_end_weights();
        }
        let count = Scalar::from_f64(n as f64);
        join_placed(
            curves,
            (Scalar::ZERO, Scalar::ONE),
            |i, k| k.add(Scalar::from_f64(i as f64)).div(count),
            |i| Scalar::from_ratio((i + 1) as i64, n as i64),
        )
    }

    /// The chain `curves`, each starting where the one before ends, as one
    /// curve, the `i`-th on `[breaks[i], breaks[i + 1]]`: each brought onto
    /// its own unit domain with its ends weighted one (see
    /// [`Self::with_unit_end_weights`]) and raised to the highest degree
    /// among them, their knot vectors laid end to end, every joint a knot
    /// of full multiplicity. A joint's control point is the union of the end
    /// of the curve before it and the start of the next — two enclosures of
    /// the one point — and an error if they could not be equal. Every curve
    /// stays the curve it was, on its new piece of the domain.
    pub fn join_at(curves: &[Self], breaks: &[Scalar]) -> GeopResult<Self> {
        let n = curves.len();
        if breaks.len() != n + 1 || !breaks.windows(2).all(|w| w[0].definitely_less(w[1])) {
            return Err(GeopError::new(format!(
                "join_at: {n} curves need {} strictly increasing breaks, not {breaks:?}",
                n + 1
            )));
        }
        join_placed(
            curves,
            (breaks[0], breaks[n]),
            |i, k| Ok(breaks[i].add(k.mul(breaks[i + 1].sub(breaks[i])))),
            |i| Ok(breaks[i + 1]),
        )
    }

    /// The same curve between the parameters `from[0]` and `from[n - 1]`,
    /// reparametrized piece by piece: the stretch between `from[i]` and
    /// `from[i + 1]` onto `[to[i], to[i + 1]]`, linearly, so that the curve
    /// is at `to[i]` where it was at `from[i]`. What lies outside `from` is
    /// cut off. Both are strictly increasing; a `from` that could be an end
    /// of the domain is that end.
    ///
    /// Every `from[i]` becomes a joint of full multiplicity (see
    /// [`Self::join_at`]). A rational curve is cut at its knots as well, so
    /// that each piece can be weighted one at its ends: its weight is then
    /// one at every joint, and so at every `to[i]`, as a polynomial curve's
    /// is everywhere. Where a piece's own knots land between `to[i]` and
    /// `to[i + 1]` is a free choice, so they are sharpened.
    ///
    /// The `from` are used as given: like [`Self::split`]'s, they must be
    /// narrow, refined rather than sharpened.
    pub fn reparametrized(&self, from: &[Scalar], to: &[Scalar]) -> GeopResult<Self> {
        let n = from.len();
        let ctx = |e: GeopError| {
            e.with_context(format!(
                "NurbCurve::reparametrized(from={from:?}, to={to:?}): domain={:?}",
                self.domain()
            ))
        };
        if n < 2 || to.len() != n {
            return Err(ctx(GeopError::new(format!(
                "{n} parameters to map onto {} others",
                to.len()
            ))));
        }
        let (t0, t1) = self.domain();
        let mut from = from.to_vec();
        if from[0].could_be_equal(t0) {
            from[0] = t0;
        }
        if from[n - 1].could_be_equal(t1) {
            from[n - 1] = t1;
        }
        let increasing = |x: &[Scalar]| x.windows(2).all(|w| w[0].definitely_less(w[1]));
        if !increasing(&from) || !increasing(to) {
            return Err(ctx(GeopError::new(
                "the parameters must be strictly increasing",
            )));
        }
        let mut rest = self.sub_curve(from[0], from[n - 1]).map_err(ctx)?;
        let mut pieces = Vec::with_capacity(n - 1);
        for &t in &from[1..n - 1] {
            let (left, right) = rest.split(t).map_err(ctx)?;
            pieces.push(left);
            rest = right;
        }
        pieces.push(rest);
        let rational = self
            .control_points
            .iter()
            .any(|p| !p[D - 1].is_exactly(Scalar::ONE));
        let mut curves = Vec::new();
        let mut breaks = vec![to[0]];
        for (i, mut piece) in pieces.into_iter().enumerate() {
            let parts = if rational {
                piece.bezier_pieces().map_err(ctx)?
            } else {
                // Every weight is one, and so is every blend of them: the
                // knot insertion cutting the pieces would only round it.
                for cp in &mut piece.control_points {
                    cp[D - 1] = Scalar::ONE;
                }
                vec![piece]
            };
            let scale = to[i + 1]
                .sub(to[i])
                .div(from[i + 1].sub(from[i]))
                .map_err(ctx)?;
            let last = parts.len() - 1;
            for (k, part) in parts.into_iter().enumerate() {
                breaks.push(if k == last {
                    to[i + 1]
                } else {
                    to[i].add(part.domain().1.sub(from[i]).mul(scale)).sharpen()
                });
                curves.push(part);
            }
        }
        NurbCurve::join_at(&curves, &breaks).map_err(ctx)
    }
}

/// The chain `curves` as one curve on `ends` (see [`NurbCurve::join_at`]):
/// the `i`-th curve's interior knot `k`, on its unit domain, at
/// `place(i, k)`, its end at `joint(i)`.
fn join_placed<const D: usize>(
    curves: &[NurbCurve<D>],
    (start, end): (Scalar, Scalar),
    place: impl Fn(usize, Scalar) -> GeopResult<Scalar>,
    joint: impl Fn(usize) -> GeopResult<Scalar>,
) -> GeopResult<NurbCurve<D>> {
    let n = curves.len();
    if n == 0 {
        return Err(GeopError::new("join: no curves to join"));
    }
    let mut pieces = curves
        .iter()
        .map(|c| c.with_unit_domain()?.with_unit_end_weights())
        .collect::<GeopResult<Vec<_>>>()?;
    let degree = pieces.iter().map(|c| c.degree).max().unwrap_or(0);
    for piece in &mut pieces {
        // Each raise one degree.
        for _ in piece.degree..degree {
            *piece = piece.elevate_degree()?;
        }
    }
    let mut points: Vec<Vector<D>> = Vec::new();
    let mut knots = vec![start; degree + 1];
    for (i, piece) in pieces.iter().enumerate() {
        let len = piece.control_points.len();
        if let Some(end) = points.last_mut() {
            let start = &piece.control_points[0];
            if !end.could_be_equal(start) {
                return Err(GeopError::new(format!(
                    "join: curve {i} starts at {start:?}, not where the one before ends, {end:?}"
                )));
            }
            *end = end.union(start);
            points.extend_from_slice(&piece.control_points[1..]);
        } else {
            points.extend_from_slice(&piece.control_points);
        }
        for &k in &piece.knot_vector[degree + 1..len] {
            knots.push(place(i, k)?);
        }
        if i + 1 < n {
            knots.extend(std::iter::repeat_n(joint(i)?, degree));
        }
    }
    knots.extend(std::iter::repeat_n(end, degree + 1));
    NurbCurve::try_new(degree, points, knots)
}

#[cfg(test)]
mod tests {
    use crate::nurb_curve::{NurbCurve, NurbCurve3D};
    use crate::test_fixtures::{line, weighted};
    use geop_core_math::{scalars::Scalar, vector::Vector3};

    fn quarter_arc() -> NurbCurve3D {
        let f = Scalar::from_f64;
        NurbCurve::try_new(
            2,
            vec![
                weighted(1., 0., 0., 1.),
                weighted(1., 1., 0., std::f64::consts::SQRT_2 / 2.0),
                weighted(0., 1., 0., 1.),
            ],
            vec![f(0.), f(0.), f(0.), f(1.), f(1.), f(1.)],
        )
        .unwrap()
    }

    fn cubic_spline() -> NurbCurve3D {
        let f = Scalar::from_f64;
        NurbCurve::try_new(
            3,
            vec![
                weighted(0., 0., 0., 1.),
                weighted(1., 2., 0., 1.),
                weighted(2., -1., 1., 1.),
                weighted(3., 1., 0., 1.),
                weighted(4., 0., 2., 1.),
            ],
            vec![
                f(0.),
                f(0.),
                f(0.),
                f(0.),
                f(0.5),
                f(1.),
                f(1.),
                f(1.),
                f(1.),
            ],
        )
        .unwrap()
    }

    fn assert_same_curve(a: &NurbCurve3D, b: &NurbCurve3D) {
        for i in 0..=16 {
            let t = Scalar::from_ratio(i, 16).unwrap();
            let (pa, pb) = (a.evaluate(t).unwrap(), b.evaluate(t).unwrap());
            assert!(pa.could_be_equal(&pb), "at {t:?}: {pa:?} vs {pb:?}");
        }
    }

    /// Raising the degree keeps the curve: a rational arc, and a cubic
    /// spline with an interior knot.
    #[test]
    fn elevate_degree_keeps_the_curve() {
        for curve in [quarter_arc(), cubic_spline(), line([0.; 3], [2., 1., 3.])] {
            let raised = curve.elevate_degree().unwrap();
            assert_eq!(raised.degree, curve.degree + 1);
            assert_same_curve(&curve, &raised);
            let twice = raised.elevate_degree().unwrap();
            assert_same_curve(&curve, &twice);
        }
    }

    /// A line, an arc and a spline brought onto one degree and one knot
    /// vector, each still the curve it was.
    #[test]
    fn compatible_curves_share_degree_and_knots() {
        let curves = [line([0.; 3], [2., 1., 3.]), quarter_arc(), cubic_spline()];
        let out = NurbCurve::compatible(&curves).unwrap();
        for (before, after) in curves.iter().zip(&out) {
            assert_eq!(after.degree, 3);
            assert_eq!(after.control_points.len(), out[0].control_points.len());
            for (a, b) in after.knot_vector.iter().zip(&out[0].knot_vector) {
                assert!(a.is_subset_of(*b) && b.is_subset_of(*a));
            }
            assert_same_curve(before, after);
        }
    }

    /// The second half of a split curve, brought onto `[0, 1]`.
    #[test]
    fn with_unit_domain_rescales_the_knots() {
        let curve = cubic_spline();
        let (_, right) = curve.split(Scalar::from_f64(0.25)).unwrap();
        let unit = right.with_unit_domain().unwrap();
        let (t0, t1) = unit.domain();
        assert!(t0.could_be_equal(Scalar::ZERO) && t1.could_be_equal(Scalar::ONE));
        let mid = unit.evaluate(Scalar::from_f64(0.5)).unwrap();
        assert!(mid.could_be_equal(&curve.evaluate(Scalar::from_f64(0.625)).unwrap()));
    }

    /// Half of a quarter arc, its end weights brought back to one: still
    /// the same arc.
    #[test]
    fn unit_end_weights_keep_the_curve() {
        let (left, _) = quarter_arc().split(Scalar::from_f64(0.5)).unwrap();
        let left = left.with_unit_domain().unwrap();
        let unit = left.with_unit_end_weights().unwrap();
        for cp in [unit.control_points[0], unit.control_points[2]] {
            assert!(cp[3].could_be_equal(Scalar::ONE));
        }
        // A different parametrization of the same points: every point of it
        // lies on the unit circle, and the ends are where they were.
        for i in 0..=8 {
            let p = unit.evaluate(Scalar::from_ratio(i, 8).unwrap()).unwrap();
            assert!(p.norm_sq().could_be_equal(Scalar::ONE), "{p:?}");
        }
        assert!(
            unit.evaluate(Scalar::ONE)
                .unwrap()
                .could_be_equal(&left.evaluate(Scalar::ONE).unwrap())
        );
    }

    /// An arc and a line on from its end, joined: one quadratic on
    /// `[0, 1]`, the arc on its first half, the line on its second. A
    /// curve starting elsewhere does not join.
    #[test]
    fn join_lays_curves_end_to_end() {
        let f = Scalar::from_f64;
        let on = NurbCurve::try_new(
            1,
            vec![weighted(0., 1., 0., 1.), weighted(-1., 1., 0., 1.)],
            vec![f(0.), f(0.), f(1.), f(1.)],
        )
        .unwrap();
        let arc = quarter_arc();
        let joined = NurbCurve::join(&[arc.clone(), on.clone()]).unwrap();
        assert_eq!(joined.degree, 2);
        for i in 0..=8 {
            let t = Scalar::from_ratio(i, 8).unwrap();
            let half = |k: f64| t.add(f(k)).div(Scalar::TWO).unwrap();
            let (a, b) = (
                joined.evaluate(half(0.0)).unwrap(),
                arc.evaluate(t).unwrap(),
            );
            assert!(a.could_be_equal(&b), "{a:?} vs {b:?}");
            let (a, b) = (joined.evaluate(half(1.0)).unwrap(), on.evaluate(t).unwrap());
            assert!(a.could_be_equal(&b), "{a:?} vs {b:?}");
        }
        assert!(NurbCurve::join(&[arc, line([0.; 3], [2., 1., 3.])]).is_err());
    }

    /// A quarter arc and a spline reparametrized piece by piece, cut short
    /// at the start: where they were at `from[i]` they are at `to[i]`,
    /// still on the curve they were, the arc weighing one there.
    #[test]
    fn reparametrized_maps_pieces() {
        let f = Scalar::from_f64;
        let from = [f(0.1), f(0.3), f(1.0)];
        let to = [f(0.0), f(0.6), f(1.0)];
        for curve in [quarter_arc(), cubic_spline()] {
            let mapped = curve.reparametrized(&from, &to).unwrap();
            let (t0, t1) = mapped.domain();
            assert!(t0.could_be_equal(Scalar::ZERO) && t1.could_be_equal(Scalar::ONE));
            for (a, b) in from.iter().zip(&to) {
                let (p, q) = (curve.evaluate(*a).unwrap(), mapped.evaluate(*b).unwrap());
                assert!(p.could_be_equal(&q), "{p:?} vs {q:?}");
            }
            let joint = mapped
                .control_points
                .iter()
                .find(|cp| {
                    Vector3::from_array([cp[0], cp[1], cp[2]])
                        .prod_scalar(Scalar::ONE.div(cp[3]).unwrap())
                        .could_be_equal(&curve.evaluate(from[1]).unwrap())
                })
                .expect("a control point at the joint");
            assert!(joint[3].could_be_equal(Scalar::ONE), "{joint:?}");
            if curve.degree == 2 {
                for i in 0..=8 {
                    let p = mapped.evaluate(Scalar::from_ratio(i, 8).unwrap()).unwrap();
                    assert!(p.norm_sq().could_be_equal(Scalar::ONE), "{p:?}");
                }
            }
        }
    }
}