rustyqlib/core/interpolation/
thin_plate.rs1use crate::core::optimization::numerics::solve_dense;
11use crate::core::errors::RustyQLibError;
12
13#[derive(Debug, Clone)]
14pub struct ThinPlateSpline {
15 centers: Vec<(f64, f64)>,
16 w: Vec<f64>,
17 affine: [f64; 3],
18}
19
20fn phi_sq(r_sq: f64) -> f64 {
22 if r_sq <= 0.0 { 0.0 } else { 0.5 * r_sq * r_sq.ln() }
23}
24
25impl ThinPlateSpline {
26 pub fn new(points: &[(f64, f64, f64)], smoothing: f64) -> Result<Self, RustyQLibError> {
29 let n = points.len();
30 if n < 3 {
31 return Err(RustyQLibError::invalid_input("thin_plate", "need at least three points"));
32 }
33 let dim = n + 3;
35 let mut a = vec![vec![0.0; dim]; dim];
36 let mut b = vec![0.0; dim];
37 for i in 0..n {
38 let (xi, yi, zi) = points[i];
39 for j in 0..n {
40 let (xj, yj, _) = points[j];
41 let r_sq = (xi - xj) * (xi - xj) + (yi - yj) * (yi - yj);
42 a[i][j] = phi_sq(r_sq);
43 }
44 a[i][i] += smoothing;
45 a[i][n] = 1.0;
46 a[i][n + 1] = xi;
47 a[i][n + 2] = yi;
48 a[n][i] = 1.0;
49 a[n + 1][i] = xi;
50 a[n + 2][i] = yi;
51 b[i] = zi;
52 }
53 let sol = solve_dense(&mut a, &mut b)
54 .ok_or(RustyQLibError::invalid_input("thin_plate", "thin-plate system is singular (collinear or duplicate points?)"))?;
55 Ok(ThinPlateSpline {
56 centers: points.iter().map(|&(x, y, _)| (x, y)).collect(),
57 w: sol[..n].to_vec(),
58 affine: [sol[n], sol[n + 1], sol[n + 2]],
59 })
60 }
61
62 pub fn eval(&self, x: f64, y: f64) -> f64 {
63 let mut v = self.affine[0] + self.affine[1] * x + self.affine[2] * y;
64 for (&(cx, cy), &wi) in self.centers.iter().zip(&self.w) {
65 let r_sq = (x - cx) * (x - cx) + (y - cy) * (y - cy);
66 v += wi * phi_sq(r_sq);
67 }
68 v
69 }
70}
71
72#[cfg(test)]
73mod tests {
74 use super::*;
75
76 fn scattered() -> Vec<(f64, f64, f64)> {
77 let f = |x: f64, y: f64| 0.2 + 0.05 * (x - 1.0) * (x - 1.0) + 0.02 * y;
79 [
80 (0.5, 0.25), (1.0, 0.25), (1.5, 0.25), (0.7, 1.0), (1.3, 1.0),
81 (0.9, 2.0), (1.1, 2.0), (0.6, 1.5), (1.4, 0.6),
82 ]
83 .iter()
84 .map(|&(x, y)| (x, y, f(x, y)))
85 .collect()
86 }
87
88 #[test]
89 fn interpolates_scattered_points_exactly() {
90 let pts = scattered();
91 let tps = ThinPlateSpline::new(&pts, 0.0).unwrap();
92 for &(x, y, z) in &pts {
93 assert!((tps.eval(x, y) - z).abs() < 1e-9, "({x}, {y})");
94 }
95 }
96
97 #[test]
98 fn reproduces_affine_surfaces_everywhere() {
99 let g = |x: f64, y: f64| 1.0 + 2.0 * x - 3.0 * y;
101 let pts: Vec<(f64, f64, f64)> =
102 scattered().iter().map(|&(x, y, _)| (x, y, g(x, y))).collect();
103 let tps = ThinPlateSpline::new(&pts, 0.0).unwrap();
104 for i in 0..=20 {
105 for j in 0..=20 {
106 let (x, y) = (i as f64 * 0.15, j as f64 * 0.15);
107 assert!((tps.eval(x, y) - g(x, y)).abs() < 1e-8, "({x}, {y})");
108 }
109 }
110 }
111
112 #[test]
113 fn smoothing_relaxes_exact_interpolation() {
114 let mut pts = scattered();
115 pts[4].2 += 0.05; let exact = ThinPlateSpline::new(&pts, 0.0).unwrap();
117 let smooth = ThinPlateSpline::new(&pts, 0.1).unwrap();
118 let (x, y, z) = pts[4];
119 assert!((exact.eval(x, y) - z).abs() < 1e-9);
120 assert!((smooth.eval(x, y) - z).abs() > 1e-3);
122 }
123
124 #[test]
125 fn rejects_degenerate_inputs() {
126 assert!(ThinPlateSpline::new(&[(0.0, 0.0, 1.0), (1.0, 1.0, 2.0)], 0.0).is_err());
127 let collinear: Vec<(f64, f64, f64)> =
129 (0..5).map(|i| (i as f64, 2.0 * i as f64, 1.0)).collect();
130 assert!(ThinPlateSpline::new(&collinear, 0.0).is_err());
131 }
132}