use alloc::vec;
use alloc::vec::Vec;
use crate::geom::point::Point2D;
use crate::sqrt;
const BAND: usize = 2;
pub fn solve_tangents(seeds: &[Point2D], lambda: f64) -> Vec<Point2D> {
let n = seeds.len();
if n < 2 {
return seeds.to_vec();
}
let lambda = if lambda > 0.0 { lambda } else { 0.0 };
let last = n as i64 - 2;
let valid = |i: i64| (1..=last).contains(&i);
let rows: Vec<[f64; 2 * BAND + 1]> = (0..n)
.map(|j| {
let j = j as i64;
let mut row = [0.0f64; 2 * BAND + 1];
let mut add = |offset: i64, coeff: f64| {
let idx = j + offset;
if idx >= 0 && (idx as usize) < n {
row[(offset + BAND as i64) as usize] += coeff;
}
};
if valid(j - 1) {
add(-2, 1.0);
add(-1, -2.0);
add(0, 1.0);
}
if valid(j) {
add(-1, -2.0);
add(0, 4.0);
add(1, -2.0);
}
if valid(j + 1) {
add(0, 1.0);
add(1, -2.0);
add(2, 1.0);
}
row[BAND] += lambda;
row
})
.collect();
let mut lower: Vec<[f64; 2 * BAND + 1]> = vec![[0.0; 2 * BAND + 1]; n];
for j in 0..n {
for k in j.saturating_sub(BAND)..j {
let mut v = rows[j][(k + BAND) - j];
for m in j.saturating_sub(BAND)..k {
v -= lower[j][(m + BAND) - j] * lower[k][(m + BAND) - k];
}
lower[j][(k + BAND) - j] = v / lower[k][BAND];
}
let mut diag = rows[j][BAND];
for k in j.saturating_sub(BAND)..j {
let v = lower[j][(k + BAND) - j];
diag -= v * v;
}
if !diag.is_finite() || diag <= 0.0 {
return seeds.to_vec();
}
lower[j][BAND] = sqrt(diag);
}
let solve = |b: &[f64]| -> Option<Vec<f64>> {
let mut y = vec![0.0f64; n];
for j in 0..n {
let mut v = b[j];
for k in j.saturating_sub(BAND)..j {
v -= lower[j][(k + BAND) - j] * y[k];
}
y[j] = v / lower[j][BAND];
}
let mut x = vec![0.0f64; n];
for j in (0..n).rev() {
let mut v = y[j];
for k in (j + 1)..(j + BAND + 1).min(n) {
v -= lower[k][(j + BAND) - k] * x[k];
}
x[j] = v / lower[j][BAND];
}
if x.iter().all(|v| v.is_finite()) { Some(x) } else { None }
};
let rhs_x: Vec<f64> = seeds.iter().map(|d| lambda * d.x).collect();
let rhs_y: Vec<f64> = seeds.iter().map(|d| lambda * d.y).collect();
match (solve(&rhs_x), solve(&rhs_y)) {
(Some(xs), Some(ys)) => (0..n).map(|j| Point2D::new(xs[j], ys[j])).collect(),
_ => seeds.to_vec(),
}
}
#[cfg(test)]
mod tests {
use super::*;
use alloc::vec;
fn on_line(n: usize) -> Vec<Point2D> {
(0..n).map(|i| Point2D::new(i as f64, 0.0)).collect()
}
#[test]
fn zero_bias_is_singular_and_returns_the_seeds() {
let seeds: Vec<Point2D> = (0..5).map(|i| Point2D::new(i as f64, (i % 2) as f64)).collect();
assert_eq!(solve_tangents(&seeds, 0.0), seeds);
}
#[test]
fn a_huge_bias_returns_the_seeds() {
let seeds: Vec<Point2D> = (0..5).map(|i| Point2D::new(i as f64, i as f64 * 0.5)).collect();
let out = solve_tangents(&seeds, 1e12);
for (a, b) in out.iter().zip(&seeds) {
assert!((a.x - b.x).abs() < 1e-6 && (a.y - b.y).abs() < 1e-6, "{a:?} vs {b:?}");
}
}
#[test]
fn zero_bias_gives_a_straight_line_for_a_straight_input() {
let knots = on_line(6);
let seeds: Vec<Point2D> = knots.windows(2).map(|w| w[1] - w[0]).collect();
let out = solve_tangents(&seeds, 0.0);
for t in &out {
assert!(t.y.abs() < 1e-9, "tangent {t:?} left the line");
}
assert!(out.iter().all(|t| t.x > 0.0), "tangents reversed: {out:?}");
}
#[test]
fn the_solution_stationarises_the_objective() {
let knots: Vec<Point2D> = vec![
Point2D::new(0.0, 0.0),
Point2D::new(3.0, 2.0),
Point2D::new(7.0, 1.0),
Point2D::new(10.0, 4.0),
];
let seeds: Vec<Point2D> = knots.windows(2).map(|w| (w[1] - w[0]) / 3.0).collect();
let lambda = 0.5;
let out = solve_tangents(&seeds, lambda);
let objective = |t: &[Point2D]| {
let mut acc = 0.0;
for i in 1..t.len() - 1 {
let s = t[i + 1] - t[i] * 2.0 + t[i - 1];
acc += s.norm_squared();
}
for i in 0..t.len() {
acc += lambda * t[i].distance_squared(seeds[i]);
}
acc
};
let base = objective(&out);
for i in 0..out.len() {
for delta in [1e-3, 1e-2] {
let mut bumped = out.clone();
bumped[i] += Point2D::new(delta, 0.0);
assert!(objective(&bumped) >= base - 1e-9, "objective fell at knot {i}");
let mut bumped = out.clone();
bumped[i] += Point2D::new(0.0, delta);
assert!(objective(&bumped) >= base - 1e-9, "objective fell at knot {i} in y");
}
}
}
#[test]
fn trivial_inputs_are_returned_unchanged() {
assert_eq!(solve_tangents(&[], 1.0), Vec::<Point2D>::new());
let one = vec![Point2D::new(1.0, 2.0)];
assert_eq!(solve_tangents(&one, 1.0), one);
let two = vec![Point2D::new(0.0, 0.0), Point2D::new(1.0, 0.0)];
assert_eq!(solve_tangents(&two, 1.0).len(), 2);
}
#[test]
fn the_system_is_pentadiagonal() {
let seeds: Vec<Point2D> = (0..5).map(|i| Point2D::new(1.0, if i % 2 == 0 { 0.0 } else { 1.0 })).collect();
let a = solve_tangents(&seeds, 0.1);
let mut b_seeds = seeds.clone();
b_seeds[2] += Point2D::new(0.0, 5.0);
let b = solve_tangents(&b_seeds, 0.1);
assert!(a[1].distance(b[1]) > 1e-9, "knot 1 unaffected by knot 2's seed");
}
}