use super::{CubicSpline, SplineValue, bernstein_basis};
use crate::Result;
use crate::common::PCoords;
use crate::common::points::dist;
use crate::common::svd_basis::SvdBasis;
use crate::na::{DMatrix, DVector, Dyn, Matrix, Owned, Point, SVector, U1, Unit, Vector};
use levenberg_marquardt::{LeastSquaresProblem, LevenbergMarquardt};
pub type SplineBuildFn<const D: usize> = Box<dyn Fn(&DVector<f64>) -> Result<CubicSpline<D>>>;
const DELTA: f64 = 1e-6;
const MIN_DIST: f64 = 1e-12;
const ZERO_COLUMN_RATIO: f64 = 1e-12;
#[derive(Clone)]
pub struct SplineFitResult<const D: usize> {
pub spline: CubicSpline<D>,
pub params: DVector<f64>,
pub residuals: DVector<f64>,
}
impl<const D: usize> SplineFitResult<D> {
pub fn new(spline: CubicSpline<D>, params: DVector<f64>, residuals: DVector<f64>) -> Self {
Self {
spline,
params,
residuals,
}
}
}
pub fn fit_spline_to_points<const D: usize>(
points: &[Point<f64, D>],
builder: &SplineBuildFn<D>,
initial: DVector<f64>,
) -> Result<SplineFitResult<D>> {
fit_spline_to_points_weighted(points, None, builder, initial)
}
pub fn fit_spline_to_points_weighted<const D: usize>(
points: &[Point<f64, D>],
weights: Option<&[f64]>,
builder: &SplineBuildFn<D>,
initial: DVector<f64>,
) -> Result<SplineFitResult<D>> {
let weights = match weights {
Some(w) if w.len() != points.len() => {
return Err(format!(
"expected {} weights for {} points, got {}",
points.len(),
points.len(),
w.len()
)
.into());
}
Some(w) => DVector::from_column_slice(w),
None => DVector::from_element(points.len(), 1.0),
};
let problem = SplineFit::try_new(points, weights, builder, initial)?;
let (result, report) = LevenbergMarquardt::new().minimize(problem);
if report.termination.was_successful() {
let state = result.state.unwrap();
let residuals = DVector::from_iterator(points.len(), state.feet.iter().map(|f| f.value));
Ok(SplineFitResult::new(state.spline, result.params, residuals))
} else {
Err(format!("Fitting failed: {:?}", report.termination).into())
}
}
struct FitState<const D: usize> {
spline: CubicSpline<D>,
feet: Vec<SplineValue<f64>>,
}
impl<const D: usize> FitState<D> {
fn new(spline: CubicSpline<D>, points: &[Point<f64, D>]) -> Self {
let queries = spline.into_query();
let feet = points.iter().map(|p| queries.project_point(p)).collect();
Self {
spline: queries.into_spline(),
feet,
}
}
}
struct SplineFit<'a, const D: usize> {
points: &'a [Point<f64, D>],
weights: DVector<f64>,
builder: &'a SplineBuildFn<D>,
params: DVector<f64>,
state: Option<FitState<D>>,
}
impl<'a, const D: usize> SplineFit<'a, D> {
fn try_new(
points: &'a [Point<f64, D>],
weights: DVector<f64>,
builder: &'a SplineBuildFn<D>,
initial: DVector<f64>,
) -> Result<Self> {
let _ = builder(&initial)?;
let mut problem = SplineFit {
points,
weights,
builder,
params: DVector::zeros(initial.len()),
state: None,
};
problem.set_params(&initial);
Ok(problem)
}
fn control_jacobian(&self, spline: &CubicSpline<D>) -> DMatrix<f64> {
let base = control_coords(spline);
let mut dctrl = DMatrix::zeros(4 * D, self.params.len());
for k in 0..self.params.len() {
let mut params = self.params.clone();
params[k] += DELTA;
let Ok(disturbed) = (self.builder)(¶ms) else {
continue;
};
let c = control_coords(&disturbed);
for r in 0..4 * D {
dctrl[(r, k)] = (c[r] - base[r]) / DELTA;
}
}
dctrl
}
}
fn zero_negligible_columns(jac: &mut DMatrix<f64>) {
let norms: Vec<f64> = jac.column_iter().map(|c| c.norm()).collect();
let largest = norms.iter().copied().fold(0.0, f64::max);
for (k, norm) in norms.iter().enumerate() {
if *norm < ZERO_COLUMN_RATIO * largest {
jac.column_mut(k).fill(0.0);
}
}
}
fn control_coords<const D: usize>(spline: &CubicSpline<D>) -> Vec<f64> {
[&spline.p0, &spline.p1, &spline.p2, &spline.p3]
.iter()
.flat_map(|p| p.coords.iter().copied())
.collect()
}
impl<const D: usize> LeastSquaresProblem<f64, Dyn, Dyn> for SplineFit<'_, D> {
type ResidualStorage = Owned<f64, Dyn, U1>;
type JacobianStorage = Owned<f64, Dyn, Dyn>;
type ParameterStorage = Owned<f64, Dyn>;
fn set_params(&mut self, x: &Vector<f64, Dyn, Self::ParameterStorage>) {
self.params = x.clone();
self.state = (self.builder)(&self.params)
.ok()
.map(|spline| FitState::new(spline, self.points));
}
fn params(&self) -> Vector<f64, Dyn, Self::ParameterStorage> {
self.params.clone()
}
fn residuals(&self) -> Option<Vector<f64, Dyn, Self::ResidualStorage>> {
let state = self.state.as_ref()?;
Some(DVector::from_iterator(
self.points.len(),
state
.feet
.iter()
.zip(self.weights.iter())
.map(|(f, w)| f.value * w),
))
}
fn jacobian(&self) -> Option<Matrix<f64, Dyn, Dyn, Self::JacobianStorage>> {
let state = self.state.as_ref()?;
let n_params = self.params.len();
let dctrl = self.control_jacobian(&state.spline);
let mut jac =
Matrix::<f64, Dyn, Dyn, Self::JacobianStorage>::zeros(self.points.len(), n_params);
for (i, (point, foot)) in self.points.iter().zip(state.feet.iter()).enumerate() {
if foot.value < MIN_DIST {
continue;
}
let u = (state.spline.position(foot.t) - point) / foot.value;
let b = bernstein_basis(foot.t);
let w = self.weights[i];
for k in 0..n_params {
let mut sum = 0.0;
for (j, bj) in b.iter().enumerate() {
for (a, ua) in u.iter().enumerate() {
sum += bj * ua * dctrl[(j * D + a, k)];
}
}
jac[(i, k)] = w * sum;
}
}
zero_negligible_columns(&mut jac);
Some(jac)
}
}
impl<const D: usize> CubicSpline<D> {
pub fn from_fit_with_ends(
points: &[impl PCoords<D>],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
weights: Option<&[f64]>,
) -> Result<Self> {
check_fit_inputs(points.len(), weights)?;
let chord = p3 - p0;
if chord.norm() <= f64::EPSILON {
return Err("the endpoints of the spline must be distinct".into());
}
let points: Vec<Point<f64, D>> = points.iter().map(|p| Point::from(p.coords())).collect();
fit_with_ends(&points, p0, p3, weights)
}
pub fn from_fit_principal_axis(
points: &[impl PCoords<D>],
weights: Option<&[f64]>,
) -> Result<Self> {
check_fit_inputs(points.len(), weights)?;
let points: Vec<Point<f64, D>> = points.iter().map(|p| Point::from(p.coords())).collect();
let ones = vec![1.0; points.len()];
let w = weights.unwrap_or(&ones);
let basis = SvdBasis::from_points(&points, weights)
.ok_or("could not compute a principal axis for the points")?;
let local: Vec<Point<f64, D>> = points.iter().map(|p| basis.point_to_basis(p)).collect();
let mut order: Vec<usize> = (0..points.len()).filter(|&i| w[i] > 0.0).collect();
if order.len() < 2 {
return Err("at least two points with positive weight are required".into());
}
order.sort_by(|&a, &b| local[a][0].total_cmp(&local[b][0]));
if let Some(ratio) = jump_ratio(&local, &order)
&& ratio > MAX_JUMP_RATIO
{
return Err(format!(
"the points do not run monotonically along their principal axis (jump ratio {:.2} \
exceeds {}); supply the endpoints with from_fit_with_ends instead",
ratio, MAX_JUMP_RATIO
)
.into());
}
let first = points[order[0]];
let last = points[*order.last().unwrap()];
let initial = seed_with_ends(&points, w, &first, &last);
let perp: Vec<SVector<f64, D>> = basis.basis[1..].to_vec();
let builder: SplineBuildFn<D> = Box::new(move |x: &DVector<f64>| {
let p1 = Point::from(SVector::<f64, D>::from_iterator(x.iter().take(D).copied()));
let p2 = Point::from(SVector::<f64, D>::from_iterator(
x.iter().skip(D).take(D).copied(),
));
let mut p0 = first;
let mut p3 = last;
for (k, v) in perp.iter().enumerate() {
p0 += v * x[2 * D + k];
p3 += v * x[3 * D - 1 + k];
}
Ok(CubicSpline::new(p0, p1, p2, p3))
});
let mut x0 = DVector::zeros(4 * D - 2);
for k in 0..D {
x0[k] = initial.p1[k];
x0[D + k] = initial.p2[k];
}
Ok(fit_spline_to_points_weighted(&points, weights, &builder, x0)?.spline)
}
}
pub const MAX_JUMP_RATIO: f64 = 0.5;
fn jump_ratio<const D: usize>(local: &[Point<f64, D>], order: &[usize]) -> Option<f64> {
let mut lo = [f64::INFINITY; D];
let mut hi = [f64::NEG_INFINITY; D];
for &i in order {
for k in 0..D {
lo[k] = lo[k].min(local[i][k]);
hi[k] = hi[k].max(local[i][k]);
}
}
let extent: f64 = (1..D).map(|k| (hi[k] - lo[k]).powi(2)).sum::<f64>().sqrt();
if extent <= 1e-12 * (hi[0] - lo[0]).max(f64::MIN_POSITIVE) {
return None;
}
let mut jumps: Vec<f64> = order
.windows(2)
.map(|pair| {
(1..D)
.map(|k| (local[pair[1]][k] - local[pair[0]][k]).powi(2))
.sum::<f64>()
.sqrt()
/ extent
})
.collect();
jumps.sort_by(f64::total_cmp);
Some(jumps[3 * jumps.len() / 4])
}
fn fit_with_ends<const D: usize>(
points: &[Point<f64, D>],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
weights: Option<&[f64]>,
) -> Result<CubicSpline<D>> {
let ones = vec![1.0; points.len()];
let w = weights.unwrap_or(&ones);
let initial = seed_with_ends(points, w, p0, p3);
let (p0, p3) = (*p0, *p3);
let builder: SplineBuildFn<D> = Box::new(move |x: &DVector<f64>| {
let p1 = Point::from(SVector::<f64, D>::from_iterator(x.iter().take(D).copied()));
let p2 = Point::from(SVector::<f64, D>::from_iterator(
x.iter().skip(D).take(D).copied(),
));
Ok(CubicSpline::new(p0, p1, p2, p3))
});
let x0 = DVector::from_iterator(
2 * D,
initial
.p1
.coords
.iter()
.chain(initial.p2.coords.iter())
.copied(),
);
Ok(fit_spline_to_points_weighted(points, weights, &builder, x0)?.spline)
}
fn seed_with_ends<const D: usize>(
points: &[Point<f64, D>],
w: &[f64],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
) -> CubicSpline<D> {
let chord = p3 - p0;
let mut seeds = vec![CubicSpline::new(
*p0,
p0 + chord / 3.0,
p3 - chord / 3.0,
*p3,
)];
for order in candidate_orders(points, p0, p3) {
if let Some(params) = chord_length_params(points, &order, w, p0, p3) {
seeds.extend(seed_interior_points(points, ¶ms, w, p0, p3));
}
}
best_seed(seeds, points, w)
}
impl<const D: usize> CubicSpline<D> {
pub fn from_fit_hermite(
points: &[impl PCoords<D>],
p0: &Point<f64, D>,
tangent0: &Unit<SVector<f64, D>>,
p3: &Point<f64, D>,
tangent3: &Unit<SVector<f64, D>>,
weights: Option<&[f64]>,
) -> Result<Self> {
check_fit_inputs(points.len(), weights)?;
let points: Vec<Point<f64, D>> = points.iter().map(|p| Point::from(p.coords())).collect();
let ones = vec![1.0; points.len()];
let w = weights.unwrap_or(&ones);
let t0 = tangent0.into_inner();
let t3 = tangent3.into_inner();
let (q0, q3) = (*p0, *p3);
let hermite = move |a0: f64, a1: f64| CubicSpline::new(q0, q0 + t0 * a0, q3 - t3 * a1, q3);
let reach = points
.iter()
.map(|p| dist(p, p0))
.fold(dist(p0, p3), f64::max);
let mut seeds = vec![hermite(reach / 3.0, reach / 3.0)];
for order in candidate_orders(&points, p0, p3) {
if let Some(params) = chord_length_params(&points, &order, w, p0, p3)
&& let Some((a0, a1)) = seed_arm_lengths(&points, ¶ms, w, p0, &t0, p3, &t3)
{
seeds.push(hermite(a0, a1));
}
}
let initial = best_seed(seeds, &points, w);
let builder: SplineBuildFn<D> = Box::new(move |x: &DVector<f64>| {
if x[0] <= 0.0 || x[1] <= 0.0 {
return Err("tangent arm lengths must be positive".into());
}
Ok(hermite(x[0], x[1]))
});
let x0 = DVector::from(vec![
dist(&initial.p1, &initial.p0),
dist(&initial.p2, &initial.p3),
]);
Ok(fit_spline_to_points_weighted(&points, weights, &builder, x0)?.spline)
}
}
fn check_fit_inputs(n_points: usize, weights: Option<&[f64]>) -> Result<()> {
if n_points < 2 {
return Err("at least two points are required to fit a cubic spline".into());
}
if let Some(w) = weights
&& w.len() != n_points
{
return Err(format!(
"expected {} weights for {} points, got {}",
n_points,
n_points,
w.len()
)
.into());
}
Ok(())
}
fn candidate_orders<const D: usize>(
points: &[Point<f64, D>],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
) -> [Vec<usize>; 2] {
let chord = p3 - p0;
let mut by_projection: Vec<usize> = (0..points.len()).collect();
by_projection.sort_by(|&a, &b| {
let pa = (points[a] - p0).dot(&chord);
let pb = (points[b] - p0).dot(&chord);
pa.total_cmp(&pb)
});
let as_given: Vec<usize> = (0..points.len()).collect();
[by_projection, as_given]
}
fn chord_length_params<const D: usize>(
points: &[Point<f64, D>],
order: &[usize],
weights: &[f64],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
) -> Option<Vec<(usize, f64)>> {
let mut cumulative = Vec::with_capacity(order.len());
let mut prev = *p0;
let mut total = 0.0;
for &i in order.iter().filter(|&&i| weights[i] > 0.0) {
total += dist(&prev, &points[i]);
cumulative.push((i, total));
prev = points[i];
}
total += dist(&prev, p3);
if total <= f64::EPSILON {
return None;
}
Some(
cumulative
.into_iter()
.map(|(i, s)| (i, s / total))
.collect(),
)
}
fn best_seed<const D: usize>(
seeds: Vec<CubicSpline<D>>,
points: &[Point<f64, D>],
weights: &[f64],
) -> CubicSpline<D> {
seeds
.into_iter()
.map(|s| (weighted_rms(&s, points, weights), s))
.min_by(|a, b| a.0.total_cmp(&b.0))
.map(|(_, s)| s)
.expect("at least one seed is always supplied")
}
fn weighted_rms<const D: usize>(
spline: &CubicSpline<D>,
points: &[Point<f64, D>],
weights: &[f64],
) -> f64 {
let queries = spline.clone().into_query();
let (sum, wsum) = points
.iter()
.zip(weights)
.fold((0.0, 0.0), |(sum, wsum), (p, w)| {
let d = queries.project_point(p).value;
(sum + w * d * d, wsum + w)
});
if wsum > 0.0 { (sum / wsum).sqrt() } else { 0.0 }
}
fn seed_interior_points<const D: usize>(
points: &[Point<f64, D>],
params: &[(usize, f64)],
weights: &[f64],
p0: &Point<f64, D>,
p3: &Point<f64, D>,
) -> Option<CubicSpline<D>> {
let (mut a11, mut a12, mut a22) = (0.0, 0.0, 0.0);
let mut r1 = SVector::<f64, D>::zeros();
let mut r2 = SVector::<f64, D>::zeros();
for &(i, t) in params {
let [b0, b1, b2, b3] = bernstein_basis(t);
let w = weights[i];
let rhs = points[i].coords - p0.coords * b0 - p3.coords * b3;
a11 += w * b1 * b1;
a12 += w * b1 * b2;
a22 += w * b2 * b2;
r1 += rhs * (w * b1);
r2 += rhs * (w * b2);
}
let det = a11 * a22 - a12 * a12;
if det <= 1e-12 * a11 * a22 {
return None;
}
let p1 = (r1 * a22 - r2 * a12) / det;
let p2 = (r2 * a11 - r1 * a12) / det;
Some(CubicSpline::new(*p0, Point::from(p1), Point::from(p2), *p3))
}
fn seed_arm_lengths<const D: usize>(
points: &[Point<f64, D>],
params: &[(usize, f64)],
weights: &[f64],
p0: &Point<f64, D>,
t0: &SVector<f64, D>,
p3: &Point<f64, D>,
t3: &SVector<f64, D>,
) -> Option<(f64, f64)> {
let (mut a11, mut a12, mut a22) = (0.0, 0.0, 0.0);
let (mut r1, mut r2) = (0.0, 0.0);
for &(i, t) in params {
let [b0, b1, b2, b3] = bernstein_basis(t);
let w = weights[i];
let va = t0 * b1;
let vb = -t3 * b2;
let rhs = points[i].coords - p0.coords * (b0 + b1) - p3.coords * (b2 + b3);
a11 += w * va.dot(&va);
a12 += w * va.dot(&vb);
a22 += w * vb.dot(&vb);
r1 += w * va.dot(&rhs);
r2 += w * vb.dot(&rhs);
}
let det = a11 * a22 - a12 * a12;
if det <= 1e-12 * a11 * a22 {
return None;
}
let a0 = (r1 * a22 - r2 * a12) / det;
let a1 = (r2 * a11 - r1 * a12) / det;
(a0 > 0.0 && a1 > 0.0).then_some((a0, a1))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::{RandomGeometry2, RandomGeometry3};
use crate::{Point2, Point3, Vector2};
use approx::assert_relative_eq;
fn truth() -> CubicSpline<2> {
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 2.0),
Point2::new(2.0, 2.0),
Point2::new(3.0, 0.0),
)
}
fn inner_builder() -> SplineBuildFn<2> {
Box::new(|p: &DVector<f64>| {
Ok(CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(p[0], p[1]),
Point2::new(p[2], p[3]),
Point2::new(3.0, 0.0),
))
})
}
fn polar_builder() -> SplineBuildFn<2> {
Box::new(|p: &DVector<f64>| {
let p0 = Point2::new(0.0, 0.0);
let p3 = Point2::new(3.0, 0.0);
let p1 = p0 + Vector2::new(p[0].cos(), p[0].sin()) * p[1];
let p2 = p3 + Vector2::new(p[2].cos(), p[2].sin()) * p[3];
Ok(CubicSpline::new(p0, p1, p2, p3))
})
}
fn numeric_jacobian<const D: usize>(fit: &SplineFit<D>) -> DMatrix<f64> {
let base = fit.residuals().unwrap();
let mut jac = DMatrix::zeros(fit.points.len(), fit.params.len());
let h = 1e-7;
for k in 0..fit.params.len() {
let mut params = fit.params.clone();
params[k] += h;
let spline = (fit.builder)(¶ms).unwrap();
let state = FitState::new(spline, fit.points);
for i in 0..fit.points.len() {
jac[(i, k)] = (state.feet[i].value * fit.weights[i] - base[i]) / h;
}
}
jac
}
fn assert_jacobians_agree<const D: usize>(fit: &SplineFit<D>) {
let analytic = fit.jacobian().unwrap();
let numeric = numeric_jacobian(fit);
for i in 0..analytic.nrows() {
for k in 0..analytic.ncols() {
assert_relative_eq!(
analytic[(i, k)],
numeric[(i, k)],
epsilon = 1e-5,
max_relative = 1e-4
);
}
}
}
#[test]
fn recover_inner_control_points() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let initial = DVector::from(vec![1.0, 0.0, 2.0, 0.0]);
let result = fit_spline_to_points(&points, &inner_builder(), initial).unwrap();
let expected = DVector::from(vec![1.0, 2.0, 2.0, 2.0]);
assert_relative_eq!(result.params, expected, epsilon = 1e-6);
assert_relative_eq!(result.spline.p1, curve.p1, epsilon = 1e-6);
assert_relative_eq!(result.spline.p2, curve.p2, epsilon = 1e-6);
}
#[test]
fn residuals_are_small_after_fit() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let initial = DVector::from(vec![1.2, 0.5, 1.8, 0.5]);
let result = fit_spline_to_points(&points, &inner_builder(), initial).unwrap();
for r in result.residuals.iter() {
assert!(r.abs() < 1e-6, "residual {} too large", r);
}
}
#[test]
fn initial_guess_that_fails_is_an_error() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
let builder: SplineBuildFn<2> =
Box::new(|_: &DVector<f64>| Err("always fails".to_string().into()));
let initial = DVector::from(vec![0.0, 0.0, 0.0, 0.0]);
assert!(fit_spline_to_points(&points, &builder, initial).is_err());
}
#[test]
fn wrong_weight_count_is_an_error() {
let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
let initial = DVector::from(vec![0.0, 0.0, 0.0, 0.0]);
let result =
fit_spline_to_points_weighted(&points, Some(&[1.0]), &inner_builder(), initial);
assert!(result.is_err());
}
#[test]
fn analytic_jacobian_matches_finite_differences_2d() {
let mut rg = RandomGeometry2::from_seed(11);
for _ in 0..20 {
let curve = truth();
let points: Vec<Point2> = (0..=15)
.map(|i| curve.position(i as f64 / 15.0) + rg.gaussian_vector::<2>(0.1))
.collect();
let weights: Vec<f64> = (0..points.len()).map(|_| rg.f64(0.2, 2.0)).collect();
let params = DVector::from(vec![
rg.f64(0.3, 1.3),
rg.f64(1.0, 3.0),
rg.f64(1.8, 2.8),
rg.f64(1.0, 3.0),
]);
let builder = polar_builder();
let fit = SplineFit::try_new(
&points,
DVector::from_column_slice(&weights),
&builder,
params,
)
.unwrap();
assert_jacobians_agree(&fit);
}
}
#[test]
fn analytic_jacobian_matches_finite_differences_3d() {
let mut rg = RandomGeometry3::from_seed(7);
let builder: SplineBuildFn<3> = Box::new(|p: &DVector<f64>| {
Ok(CubicSpline::new(
Point3::new(p[0], p[1], p[2]),
Point3::new(p[3], p[4], p[5]),
Point3::new(p[6], p[7], p[8]),
Point3::new(p[9], p[10], p[11]),
))
});
for _ in 0..20 {
let curve =
CubicSpline::new(rg.point(2.0), rg.point(2.0), rg.point(2.0), rg.point(2.0));
let points: Vec<Point3> = (0..=15)
.map(|i| curve.position(i as f64 / 15.0) + rg.gaussian_vector::<3>(0.1))
.collect();
let params = DVector::from_iterator(
12,
control_coords(&curve).iter().map(|c| c + rg.f64_sym(0.2)),
);
let fit = SplineFit::try_new(
&points,
DVector::from_element(points.len(), 1.0),
&builder,
params,
)
.unwrap();
assert_jacobians_agree(&fit);
}
}
#[test]
fn collinear_initial_guess_converges() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let initial = DVector::from(vec![1.0, 0.0, 2.0, 0.0]);
let result = fit_spline_to_points(&points, &inner_builder(), initial).unwrap();
assert_relative_eq!(result.spline.p1, curve.p1, epsilon = 1e-6);
assert_relative_eq!(result.spline.p2, curve.p2, epsilon = 1e-6);
}
fn max_curve_deviation<const D: usize>(truth: &CubicSpline<D>, fitted: &CubicSpline<D>) -> f64 {
let queries = fitted.clone().into_query();
(0..=200)
.map(|i| {
queries
.project_point(&truth.position(i as f64 / 200.0))
.value
})
.fold(0.0, f64::max)
}
fn assert_same_curve<const D: usize>(truth: &CubicSpline<D>, fitted: &CubicSpline<D>) {
let dev = max_curve_deviation(truth, fitted);
assert!(dev < 1e-6, "curve deviation {} too large", dev);
}
fn shuffled<T: Clone>(items: &[T], rg: &mut RandomGeometry2) -> Vec<T> {
let mut out = items.to_vec();
for i in (1..out.len()).rev() {
let j = rg.f64(0.0, (i + 1) as f64).floor() as usize;
out.swap(i, j.min(i));
}
out
}
#[test]
fn with_ends_recovers_exact_curve_2d() {
let curve = truth();
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let fitted = CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn with_ends_recovers_exact_curve_3d() {
let curve = CubicSpline::new(
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 2.0, 1.0),
Point3::new(2.0, 2.0, -1.0),
Point3::new(3.0, 0.0, 0.0),
);
let points: Vec<Point3> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let fitted = CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn with_ends_noisy_unordered_points() {
let mut rg = RandomGeometry2::from_seed(3);
let curve = truth();
let sigma = 0.01;
for _ in 0..10 {
let points: Vec<Point2> = (0..=40)
.map(|i| curve.position(i as f64 / 40.0) + rg.gaussian_vector::<2>(sigma))
.collect();
let points = shuffled(&points, &mut rg);
let fitted =
CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, None).unwrap();
let dev = max_curve_deviation(&curve, &fitted);
assert!(dev < 2.0 * sigma, "deviation {} too large", dev);
}
}
#[test]
fn with_ends_hook_curve_ordered_and_shuffled() {
let curve = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(4.0, -1.0),
Point2::new(4.0, 4.0),
Point2::new(0.0, 1.0),
);
let points: Vec<Point2> = (0..=40).map(|i| curve.position(i as f64 / 40.0)).collect();
let fitted = CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, None).unwrap();
assert_same_curve(&curve, &fitted);
let mut rg = RandomGeometry2::from_seed(5);
let points = shuffled(&points, &mut rg);
let fitted = CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn with_ends_zero_weight_outlier_ignored() {
let curve = truth();
let mut points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let mut weights = vec![1.0; points.len()];
points.push(Point2::new(1.5, 5.0));
weights.push(0.0);
let fitted =
CubicSpline::from_fit_with_ends(&points, &curve.p0, &curve.p3, Some(&weights)).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn with_ends_input_errors() {
let p0 = Point2::new(0.0, 0.0);
let p3 = Point2::new(3.0, 0.0);
let one = vec![Point2::new(1.0, 1.0)];
assert!(CubicSpline::from_fit_with_ends(&one, &p0, &p3, None).is_err());
let two = vec![Point2::new(1.0, 1.0), Point2::new(2.0, 1.0)];
assert!(CubicSpline::from_fit_with_ends(&two, &p0, &p0, None).is_err());
assert!(CubicSpline::from_fit_with_ends(&two, &p0, &p3, Some(&[1.0])).is_err());
}
fn end_tangents<const D: usize>(
c: &CubicSpline<D>,
) -> (Unit<SVector<f64, D>>, Unit<SVector<f64, D>>) {
(
Unit::new_normalize(c.p1 - c.p0),
Unit::new_normalize(c.p3 - c.p2),
)
}
#[test]
fn hermite_recovers_exact_curve_2d() {
let curve = truth();
let (t0, t3) = end_tangents(&curve);
let points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn hermite_recovers_exact_curve_3d() {
let curve = CubicSpline::new(
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 2.0, 1.0),
Point3::new(2.0, 2.0, -1.0),
Point3::new(3.0, 0.0, 0.0),
);
let (t0, t3) = end_tangents(&curve);
let points: Vec<Point3> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn hermite_noisy_unordered_points() {
let mut rg = RandomGeometry2::from_seed(13);
let curve = truth();
let (t0, t3) = end_tangents(&curve);
let sigma = 0.01;
for _ in 0..10 {
let points: Vec<Point2> = (0..=40)
.map(|i| curve.position(i as f64 / 40.0) + rg.gaussian_vector::<2>(sigma))
.collect();
let points = shuffled(&points, &mut rg);
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None)
.unwrap();
let dev = max_curve_deviation(&curve, &fitted);
assert!(dev < 2.0 * sigma, "deviation {} too large", dev);
}
}
#[test]
fn hermite_hook_curve_ordered_and_shuffled() {
let curve = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(4.0, -1.0),
Point2::new(4.0, 4.0),
Point2::new(0.0, 1.0),
);
let (t0, t3) = end_tangents(&curve);
let points: Vec<Point2> = (0..=40).map(|i| curve.position(i as f64 / 40.0)).collect();
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None).unwrap();
assert_same_curve(&curve, &fitted);
let mut rg = RandomGeometry2::from_seed(5);
let points = shuffled(&points, &mut rg);
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn hermite_closed_loop_with_coincident_endpoints() {
let curve = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(3.0, 2.0),
Point2::new(3.0, -2.0),
Point2::new(0.0, 0.0),
);
let (t0, t3) = end_tangents(&curve);
let points: Vec<Point2> = (0..=40).map(|i| curve.position(i as f64 / 40.0)).collect();
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, None).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn hermite_collinear_degenerate_case() {
let p0 = Point2::new(0.0, 0.0);
let p3 = Point2::new(3.0, 0.0);
let t = Unit::new_normalize(Vector2::new(1.0, 0.0));
let points: Vec<Point2> = (0..=20)
.map(|i| Point2::new(3.0 * i as f64 / 20.0, 0.0))
.collect();
let fitted = CubicSpline::from_fit_hermite(&points, &p0, &t, &p3, &t, None).unwrap();
assert!(fitted.p1.y.abs() < 1e-12 && fitted.p2.y.abs() < 1e-12);
assert!(fitted.p1.x > 0.0 && fitted.p2.x < 3.0);
let queries = fitted.into_query();
for p in &points {
assert!(queries.project_point(p).value < 1e-9);
}
}
#[test]
fn hermite_zero_weight_outlier_ignored() {
let curve = truth();
let (t0, t3) = end_tangents(&curve);
let mut points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let mut weights = vec![1.0; points.len()];
points.push(Point2::new(1.5, 5.0));
weights.push(0.0);
let fitted =
CubicSpline::from_fit_hermite(&points, &curve.p0, &t0, &curve.p3, &t3, Some(&weights))
.unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn hermite_input_errors() {
let p0 = Point2::new(0.0, 0.0);
let p3 = Point2::new(3.0, 0.0);
let t = Unit::new_normalize(Vector2::new(1.0, 1.0));
let one = vec![Point2::new(1.0, 1.0)];
assert!(CubicSpline::from_fit_hermite(&one, &p0, &t, &p3, &t, None).is_err());
let two = vec![Point2::new(1.0, 1.0), Point2::new(2.0, 1.0)];
assert!(CubicSpline::from_fit_hermite(&two, &p0, &t, &p3, &t, Some(&[1.0])).is_err());
}
fn samples<const D: usize>(c: &CubicSpline<D>, n: usize) -> Vec<Point<f64, D>> {
(0..n)
.map(|i| c.position(i as f64 / (n - 1) as f64))
.collect()
}
fn gentle_s() -> CubicSpline<2> {
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.5, 0.0),
Point2::new(1.5, 2.0),
Point2::new(3.0, 2.0),
)
}
#[test]
fn principal_axis_exact_shapes() {
let mut rg = RandomGeometry2::from_seed(21);
let shapes = [
truth(),
gentle_s(),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(2.0, 0.0),
Point2::new(1.0, 3.0),
Point2::new(3.0, 3.0),
),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(4.0, 0.0),
Point2::new(4.0, 3.0),
Point2::new(0.0, 3.0),
),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 0.5),
Point2::new(2.0, 1.0),
Point2::new(3.0, 1.5),
),
];
for curve in shapes {
let points = shuffled(&samples(&curve, 41), &mut rg);
let fitted = CubicSpline::from_fit_principal_axis(&points, None).unwrap();
assert_same_curve(&curve, &fitted);
}
}
#[test]
fn principal_axis_noisy_shapes() {
let mut rg = RandomGeometry2::from_seed(22);
let sigma = 0.01;
for curve in [truth(), gentle_s()] {
for _ in 0..5 {
let points: Vec<Point2> = samples(&curve, 60)
.iter()
.map(|p| p + rg.gaussian_vector::<2>(sigma))
.collect();
let points = shuffled(&points, &mut rg);
let fitted = CubicSpline::from_fit_principal_axis(&points, None).unwrap();
let dev = max_curve_deviation(&curve, &fitted);
assert!(dev < 4.0 * sigma, "deviation {} too large", dev);
}
}
}
#[test]
fn principal_axis_noisy_line_accepted() {
let mut rg = RandomGeometry2::from_seed(23);
let points: Vec<Point2> = (0..100)
.map(|i| Point2::new(3.0 * i as f64 / 99.0, 0.0) + rg.gaussian_vector::<2>(0.3))
.collect();
assert!(CubicSpline::from_fit_principal_axis(&points, None).is_ok());
}
#[test]
fn principal_axis_rejects_doubling_back() {
let mut rg = RandomGeometry2::from_seed(24);
let shapes = [
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(4.0, 0.0),
Point2::new(4.0, 1.0),
Point2::new(0.0, 1.0),
),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(4.0, -1.0),
Point2::new(4.0, 4.0),
Point2::new(0.0, 1.0),
),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(3.0, 2.0),
Point2::new(3.0, -2.0),
Point2::new(0.0, 0.0),
),
];
for curve in shapes {
let points = shuffled(&samples(&curve, 60), &mut rg);
assert!(CubicSpline::from_fit_principal_axis(&points, None).is_err());
}
let half = 150.0f64.to_radians();
let arc: Vec<Point2> = (0..60)
.map(|i| {
let a = -half + 2.0 * half * i as f64 / 59.0;
Point2::new(a.cos(), a.sin())
})
.collect();
assert!(CubicSpline::from_fit_principal_axis(&arc, None).is_err());
}
#[test]
fn principal_axis_zero_weight_points_excluded() {
let curve = truth();
let mut points = samples(&curve, 41);
let mut weights = vec![1.0; points.len()];
points.push(Point2::new(20.0, 20.0));
weights.push(0.0);
let fitted = CubicSpline::from_fit_principal_axis(&points, Some(&weights)).unwrap();
assert_same_curve(&curve, &fitted);
}
#[test]
fn principal_axis_3d() {
let mut rg = RandomGeometry2::from_seed(25);
let curve = CubicSpline::new(
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 2.0, 1.0),
Point3::new(2.0, 2.0, -1.0),
Point3::new(3.0, 0.0, 0.0),
);
let points = shuffled(&samples(&curve, 41), &mut rg);
let fitted = CubicSpline::from_fit_principal_axis(&points, None).unwrap();
assert_same_curve(&curve, &fitted);
let hairpin = CubicSpline::new(
Point3::new(0.0, 0.0, 0.0),
Point3::new(4.0, 0.0, 1.0),
Point3::new(4.0, 1.0, 1.0),
Point3::new(0.0, 1.0, 0.0),
);
let points = shuffled(&samples(&hairpin, 60), &mut rg);
assert!(CubicSpline::from_fit_principal_axis(&points, None).is_err());
}
#[test]
fn principal_axis_input_errors() {
let one = vec![Point2::new(1.0, 1.0)];
assert!(CubicSpline::from_fit_principal_axis(&one, None).is_err());
let two = vec![Point2::new(1.0, 1.0), Point2::new(2.0, 1.0)];
assert!(CubicSpline::from_fit_principal_axis(&two, Some(&[1.0])).is_err());
assert!(CubicSpline::from_fit_principal_axis(&two, Some(&[1.0, 0.0])).is_err());
}
#[test]
fn zero_weight_points_are_ignored() {
let curve = truth();
let mut points: Vec<Point2> = (0..=20).map(|i| curve.position(i as f64 / 20.0)).collect();
let mut weights = vec![1.0; points.len()];
points.push(Point2::new(1.5, 5.0));
weights.push(0.0);
let initial = DVector::from(vec![1.2, 0.5, 1.8, 0.5]);
let result =
fit_spline_to_points_weighted(&points, Some(&weights), &inner_builder(), initial)
.unwrap();
let expected = DVector::from(vec![1.0, 2.0, 2.0, 2.0]);
assert_relative_eq!(result.params, expected, epsilon = 1e-6);
}
}