use super::*;
use crate::common::{Line, PCoords, Segment, dist, linear_space};
const N_INTR: usize = 6;
#[derive(Debug, Clone)]
struct IntervalData<const D: usize> {
t0: f64,
line0: Line<D>,
e_max: f64,
}
fn dist_min_max<const D: usize>(
test: &impl PCoords<D>,
data: &IntervalData<D>,
end: &Point<f64, D>,
) -> (f64, f64) {
let dir = end - data.line0.origin;
let t =
((test.coords() - data.line0.origin.coords).dot(&dir) / dir.norm_squared()).clamp(0.0, 1.0);
let shortest = (test.coords() - (data.line0.origin + dir * t).coords).norm();
let da = test.coords() - data.line0.origin.coords;
let db = test.coords() - end.coords;
let longest = da.norm_squared().max(db.norm_squared()).sqrt();
((shortest - data.e_max).max(0.0), longest + data.e_max)
}
fn closest_to_point<const D: usize>(
test: &impl PCoords<D>,
spline: &CubicSpline<D>,
t0: f64,
t1: f64,
l0: &Line<D>,
l1: &Line<D>,
) -> SplineValue<f64> {
let before_front = l0.scalar_project(test) < 0.0;
let after_back = l1.scalar_project(test) > 0.0;
match (before_front, after_back) {
(true, false) => SplineValue::new(t0, dist(test, &l0.origin)),
(false, true) => SplineValue::new(t1, dist(test, &l1.origin)),
(true, true) => {
let q0 = SplineValue::new(t0, dist(test, &l0.origin));
let q1 = SplineValue::new(t1, dist(test, &l1.origin));
if q0.value < q1.value { q0 } else { q1 }
}
(false, false) => {
let q0 = iterate_to_closest(test, spline, t0, t0, t1);
let q1 = iterate_to_closest(test, spline, t1, t0, t1);
if q0.value < q1.value { q0 } else { q1 }
}
}
}
fn iterate_to_closest<const D: usize>(
test: &impl PCoords<D>,
spline: &CubicSpline<D>,
t_start: f64,
lo: f64,
hi: f64,
) -> SplineValue<f64> {
const MAX_ITER: usize = 32;
let tc = test.coords();
let g = |t: f64| -> f64 { (spline.position(t).coords - tc).dot(&spline.derivative(t)) };
let mut a = lo;
let mut b = hi;
let have_bracket = g(lo) <= 0.0 && g(hi) >= 0.0;
let mut t = t_start.clamp(lo, hi);
for _ in 0..MAX_ITER {
let r = spline.position(t).coords - tc;
let d1 = spline.derivative(t);
let gt = r.dot(&d1);
if gt.abs() < 1e-12 {
break;
}
if have_bracket {
if gt < 0.0 {
a = t;
} else {
b = t;
}
}
let gpt = d1.norm_squared() + r.dot(&spline.second_derivative(t));
let newton = t - gt / gpt;
let next = if gpt.abs() > 1e-14 && newton > lo && newton < hi {
newton
} else if have_bracket {
0.5 * (a + b)
} else {
break;
};
let converged = (next - t).abs() < 1e-12;
t = next;
if converged {
break;
}
}
SplineValue::new(t, dist(test, &spline.position(t)))
}
#[derive(Debug, Clone)]
pub struct CubicSplineQueries<const D: usize> {
spline: CubicSpline<D>,
line1: Line<D>,
intervals: [IntervalData<D>; N_INTR],
}
impl<const D: usize> CubicSplineQueries<D> {
pub fn project_point(&self, point: &impl PCoords<D>) -> SplineValue<f64> {
let mut prune = [(f64::INFINITY, f64::INFINITY); N_INTR];
for (i, slot) in prune.iter_mut().enumerate() {
let end = if i < N_INTR - 1 {
self.intervals[i + 1].line0.origin
} else {
self.line1.origin
};
*slot = dist_min_max(point, &self.intervals[i], &end);
}
let min_farthest = prune
.iter()
.map(|(_, far)| *far)
.min_by(|a, b| a.partial_cmp(b).unwrap())
.unwrap();
let mut closest = [SplineValue::new(f64::NAN, f64::INFINITY); N_INTR];
for i in 0..N_INTR {
if prune[i].0 <= min_farthest {
let (t1, line1) = if i < N_INTR - 1 {
(self.intervals[i + 1].t0, self.intervals[i + 1].line0)
} else {
(1.0, self.line1)
};
closest[i] = closest_to_point(
point,
&self.spline,
self.intervals[i].t0,
t1,
&self.intervals[i].line0,
&line1,
);
}
}
*closest
.iter()
.min_by(|a, b| a.value.partial_cmp(&b.value).unwrap())
.unwrap()
}
pub(crate) fn new(spline: CubicSpline<D>) -> Self {
let mut working = vec![WorkingInterval::new(&spline, 0.0, 1.0)];
let line1 = spline.line_at(1.0);
if let Some(tc) = spline.find_cusp() {
Self::try_split_at(&spline, &mut working, tc);
}
for &t in spline.find_curvature_zeros().iter() {
if t.is_finite() {
Self::try_split_at(&spline, &mut working, t);
}
}
while working.len() < N_INTR {
let worst = working
.iter()
.enumerate()
.max_by(|(_, a), (_, b)| a.e_max.partial_cmp(&b.e_max).unwrap())
.map(|(i, _)| i)
.expect("working always has at least one interval");
let t_split = working[worst].t_max;
Self::split_at_index(&spline, &mut working, worst, t_split);
}
let intervals: [IntervalData<D>; N_INTR] = working
.into_iter()
.map(|w| IntervalData {
t0: w.t0,
line0: spline.line_at(w.t0),
e_max: interval_error_bound(&spline, w.t0, w.t1),
})
.collect::<Vec<_>>()
.try_into()
.unwrap_or_else(|_| panic!("working did not have exactly N_INTR intervals"));
Self {
spline,
line1,
intervals,
}
}
fn try_split_at(spline: &CubicSpline<D>, working: &mut Vec<WorkingInterval>, t: f64) {
if working.len() >= N_INTR {
return;
}
let index = working
.iter()
.position(|w| t > w.t0 + 1e-12 && t < w.t1 - 1e-12);
if let Some(index) = index {
Self::split_at_index(spline, working, index, t);
}
}
fn split_at_index(
spline: &CubicSpline<D>,
working: &mut Vec<WorkingInterval>,
index: usize,
t: f64,
) {
let old = working.remove(index);
let left = WorkingInterval::new(spline, old.t0, t);
let right = WorkingInterval::new(spline, t, old.t1);
working.insert(index, right);
working.insert(index, left);
}
pub fn into_spline(self) -> CubicSpline<D> {
self.spline
}
}
impl<const D: usize> From<&CubicSpline<D>> for CubicSplineQueries<D> {
fn from(value: &CubicSpline<D>) -> Self {
CubicSplineQueries::new(value.clone())
}
}
impl<const D: usize> From<CubicSpline<D>> for CubicSplineQueries<D> {
fn from(value: CubicSpline<D>) -> Self {
CubicSplineQueries::new(value)
}
}
#[derive(Debug, Clone, Copy)]
struct WorkingInterval {
t0: f64,
t1: f64,
t_max: f64,
e_max: f64,
}
impl WorkingInterval {
fn new<const D: usize>(spline: &CubicSpline<D>, t0: f64, t1: f64) -> Self {
let (t_max, e_max) = interval_max_error(spline, t0, t1);
Self {
t0,
t1,
t_max,
e_max,
}
}
}
fn sub_curve<const D: usize>(spline: &CubicSpline<D>, t0: f64, t1: f64) -> CubicSpline<D> {
let right = if t0 <= 0.0 {
spline.clone()
} else {
spline.split(t0).1
};
if t1 >= 1.0 {
right
} else {
let local_t1 = (t1 - t0) / (1.0 - t0);
right.split(local_t1).0
}
}
fn interval_max_error<const D: usize>(spline: &CubicSpline<D>, t0: f64, t1: f64) -> (f64, f64) {
if t1 - t0 < 1e-12 {
return (t0, 0.0);
}
let sub = sub_curve(spline, t0, t1);
let helper = CubicSplineBaseDist::new(&sub);
let local_t = helper.find_max_t(0.0, 1.0);
let e_max = helper.e(local_t);
(t0 + local_t * (t1 - t0), e_max)
}
fn interval_error_bound<const D: usize>(spline: &CubicSpline<D>, t0: f64, t1: f64) -> f64 {
if t1 - t0 < 1e-12 {
return 0.0;
}
let sub = sub_curve(spline, t0, t1);
if dist(&sub.p0, &sub.p3) < 1e-12 {
return dist(&sub.p1, &sub.p0).max(dist(&sub.p2, &sub.p0));
}
let seg = Segment::new_unchecked(sub.p0, sub.p3);
let d1 = dist(&seg.closest_point(&sub.p1), &sub.p1);
let d2 = dist(&seg.closest_point(&sub.p2), &sub.p2);
d1.max(d2)
}
pub(crate) struct CubicSplineBaseDist<'a, const D: usize> {
spline: &'a CubicSpline<D>,
seg: Option<Segment<D>>,
}
impl<'a, const D: usize> CubicSplineBaseDist<'a, D> {
pub fn new(spline: &'a CubicSpline<D>) -> Self {
Self {
spline,
seg: if dist(&spline.p0, &spline.p3) < 1e-12 {
None
} else {
Some(Segment::new_unchecked(spline.p0, spline.p3))
},
}
}
pub fn e(&self, t: f64) -> f64 {
let p = self.spline.position(t);
let cp = if let Some(seg) = self.seg {
seg.closest_point(&p)
} else {
self.spline.p0
};
dist(&cp, &p)
}
pub fn dedt(&self, t: f64) -> f64 {
let p = self.spline.position(t);
let cp = if let Some(seg) = self.seg {
seg.closest_point(&p)
} else {
self.spline.p0
};
let n = (p - cp).normalize();
n.dot(&self.spline.derivative(t))
}
pub(crate) fn find_max_t(&self, t0: f64, t1: f64) -> f64 {
const N_SAMPLES: usize = 5;
const MAX_ITER: usize = 20;
let samples = linear_space(t0, t1, N_SAMPLES);
let mut best_t = samples[N_SAMPLES / 2];
let mut best_e = self.e(best_t);
for &t in samples.values() {
let e = self.e(t);
if e > best_e {
best_e = e;
best_t = t;
}
}
let h = ((t1 - t0).abs() * 1e-4).max(1e-9);
let mut t = best_t;
for _ in 0..MAX_ITER {
let d1 = self.dedt(t);
if d1.abs() < 1e-12 {
break;
}
let t_minus = (t - h).max(t0);
let t_plus = (t + h).min(t1);
if t_plus - t_minus < 1e-15 {
break;
}
let d2 = (self.dedt(t_plus) - self.dedt(t_minus)) / (t_plus - t_minus);
if d2.abs() < 1e-12 {
break;
}
let next_t = (t - d1 / d2).clamp(t0, t1);
let converged = (next_t - t).abs() < 1e-12;
t = next_t;
if converged {
break;
}
}
let e_t = self.e(t);
if e_t > best_e { t } else { best_t }
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::Point2;
use approx::assert_relative_eq;
fn sample_2d() -> CubicSpline<2> {
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 1.0),
Point2::new(2.0, 1.0),
Point2::new(3.0, 0.0),
)
}
#[test]
fn find_max_t_locates_symmetric_peak() {
let c = sample_2d();
let helper = CubicSplineBaseDist::new(&c);
let t = helper.find_max_t(0.0, 1.0);
assert_relative_eq!(t, 0.5, epsilon = 1e-6);
}
#[test]
fn find_max_t_matches_dense_sampling() {
let c = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(0.5, 2.0),
Point2::new(2.0, -3.0),
Point2::new(3.0, 0.0),
);
let helper = CubicSplineBaseDist::new(&c);
let t = helper.find_max_t(0.0, 1.0);
let mut best_t = 0.0;
let mut best_e = f64::NEG_INFINITY;
for i in 0..=10_000 {
let s = i as f64 / 10_000.0;
let e = helper.e(s);
if e > best_e {
best_e = e;
best_t = s;
}
}
assert_relative_eq!(t, best_t, epsilon = 1e-3);
assert_relative_eq!(helper.e(t), best_e, epsilon = 1e-6);
}
#[test]
fn find_max_t_restricted_to_sub_interval() {
let c = sample_2d();
let helper = CubicSplineBaseDist::new(&c);
let t = helper.find_max_t(0.6, 1.0);
assert!((0.6 - 1e-9..=1.0 + 1e-9).contains(&t));
assert_relative_eq!(t, 0.6, epsilon = 1e-6);
}
#[test]
fn find_max_t_degenerate_interval_returns_endpoint() {
let c = sample_2d();
let helper = CubicSplineBaseDist::new(&c);
let t = helper.find_max_t(0.3, 0.3);
assert_relative_eq!(t, 0.3, epsilon = 1e-12);
}
fn assert_well_formed<const D: usize>(q: &CubicSplineQueries<D>) {
assert_eq!(q.intervals.len(), N_INTR);
assert_relative_eq!(q.intervals[0].t0, 0.0, epsilon = 1e-12);
for w in q.intervals.windows(2) {
assert!(
w[1].t0 > w[0].t0,
"t0 values must be strictly ascending: {} then {}",
w[0].t0,
w[1].t0
);
}
assert!(q.intervals[N_INTR - 1].t0 < 1.0);
}
#[test]
fn new_builds_n_intr_well_formed_intervals() {
let q = CubicSplineQueries::new(sample_2d());
assert_well_formed(&q);
}
#[test]
fn new_splits_at_a_cusp() {
let c = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 1.0),
Point2::new(1.0, 1.0),
Point2::new(0.0, 0.0),
);
let cusp = c.find_cusp().expect("expected a cusp");
let q = CubicSplineQueries::new(c);
assert_well_formed(&q);
assert!(
q.intervals.iter().any(|iv| (iv.t0 - cusp).abs() < 1e-9),
"expected an interval boundary at the cusp t = {}",
cusp
);
}
#[test]
fn new_splits_at_an_inflection() {
let c = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(1.0, 1.0),
Point2::new(2.0, -1.0),
Point2::new(3.0, 0.0),
);
let infl = c.find_inflections()[0];
assert!(infl.is_finite());
let q = CubicSplineQueries::new(c);
assert_well_formed(&q);
assert!(
q.intervals.iter().any(|iv| (iv.t0 - infl).abs() < 1e-9),
"expected an interval boundary at the inflection t = {}",
infl
);
}
#[test]
fn new_intervals_have_bounded_error() {
let c = CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(0.5, 3.0),
Point2::new(2.5, -3.0),
Point2::new(3.0, 0.0),
);
let whole_error = {
let helper = CubicSplineBaseDist::new(&c);
let t = helper.find_max_t(0.0, 1.0);
helper.e(t)
};
let q = CubicSplineQueries::new(c);
assert_well_formed(&q);
for iv in &q.intervals {
assert!(
iv.e_max <= whole_error + 1e-9,
"interval error {} exceeded whole-curve error {}",
iv.e_max,
whole_error
);
}
let worst = q.intervals.iter().map(|iv| iv.e_max).fold(0.0, f64::max);
assert!(
worst < whole_error * 0.5,
"worst interval error {} did not shrink enough from whole-curve error {}",
worst,
whole_error
);
}
#[test]
fn new_works_in_3d() {
use crate::Point3;
let c = CubicSpline::new(
Point3::new(0.0, 0.0, 0.0),
Point3::new(1.0, 1.0, 0.5),
Point3::new(2.0, -1.0, -0.5),
Point3::new(3.0, 0.0, 0.0),
);
let q = CubicSplineQueries::new(c);
assert_well_formed(&q);
}
fn brute_closest<const D: usize>(spline: &CubicSpline<D>, p: &Point<f64, D>) -> f64 {
let mut best_t = 0.0;
let mut best_d = f64::INFINITY;
for i in 0..=20_000 {
let t = i as f64 / 20_000.0;
let d = dist(p, &spline.position(t));
if d < best_d {
best_d = d;
best_t = t;
}
}
best_t
}
fn assert_projects_like_brute(spline: &CubicSpline<2>, p: Point<f64, 2>) {
let q = CubicSplineQueries::new(spline.clone());
let proj = q.project_point(&p);
let t_ref = brute_closest(spline, &p);
let d = dist(&p, &spline.position(proj.t));
let d_ref = dist(&p, &spline.position(t_ref));
assert_relative_eq!(proj.value, d, epsilon = 1e-9);
assert!(
d <= d_ref + 1e-6,
"project_point distance {} worse than brute-force {} (t={}, t_ref={})",
d,
d_ref,
proj.t,
t_ref
);
}
#[test]
fn interval_e_max_is_a_true_upper_bound() {
let curves = [
sample_2d(),
CubicSpline::new(
Point2::new(0.0, 0.0),
Point2::new(0.5, 3.0),
Point2::new(2.5, -3.0),
Point2::new(3.0, 0.0),
),
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),
),
];
for c in &curves {
let q = CubicSplineQueries::new(c.clone());
for (i, iv) in q.intervals.iter().enumerate() {
let t0 = iv.t0;
let t1 = if i < N_INTR - 1 {
q.intervals[i + 1].t0
} else {
1.0
};
let p0 = c.position(t0);
let p1 = c.position(t1);
let leg = Segment::new_unchecked(p0, p1);
let mut worst = 0.0_f64;
for k in 0..=2000 {
let t = t0 + (t1 - t0) * (k as f64 / 2000.0);
let p = c.position(t);
worst = worst.max(dist(&leg.closest_point(&p), &p));
}
assert!(
iv.e_max >= worst - 1e-9,
"interval {} radius {} under-bounds observed deviation {}",
i,
iv.e_max,
worst
);
}
}
}
#[test]
fn project_point_matches_brute_force_arch() {
let c = sample_2d();
for &p in &[
Point2::new(1.5, 5.0),
Point2::new(1.5, -3.0),
Point2::new(-2.0, 0.0),
Point2::new(5.0, 0.0),
Point2::new(1.5, 0.75),
Point2::new(0.5, 0.5),
] {
assert_projects_like_brute(&c, p);
}
}
#[test]
fn project_point_matches_brute_force_s_curve() {
let c = 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),
);
for &p in &[
Point2::new(0.5, 2.0),
Point2::new(2.5, -2.0),
Point2::new(1.5, 0.0),
Point2::new(1.5, 3.0),
Point2::new(1.5, -3.0),
Point2::new(-1.0, 1.0),
Point2::new(4.0, -1.0),
] {
assert_projects_like_brute(&c, p);
}
}
}