qualia_core_db/solvers/interpolation/
spline.rs

1//! Natural cubic spline interpolation (the tridiagonal system for the second
2//! derivatives is solved with the Thomas algorithm) and piecewise-linear interpolation.
3
4use super::InterpolationError;
5
6/// Piecewise-linear interpolation of `(xs, ys)` (xs strictly increasing) at `x`.
7/// Clamps to the endpoints outside the range.
8pub fn linear_interp(xs: &[f64], ys: &[f64], x: f64) -> Result<f64, InterpolationError> {
9    if xs.len() < 2 || xs.len() != ys.len() {
10        return Err(InterpolationError::InsufficientData);
11    }
12    if x <= xs[0] {
13        return Ok(ys[0]);
14    }
15    if x >= xs[xs.len() - 1] {
16        return Ok(ys[ys.len() - 1]);
17    }
18    let i = xs.partition_point(|&xi| xi <= x) - 1;
19    let t = (x - xs[i]) / (xs[i + 1] - xs[i]);
20    Ok(ys[i] * (1.0 - t) + ys[i + 1] * t)
21}
22
23/// A natural cubic spline: second derivatives zero at both ends.
24#[derive(Debug, Clone)]
25pub struct CubicSpline {
26    xs: Vec<f64>,
27    ys: Vec<f64>,
28    m: Vec<f64>, // second derivatives at the nodes
29}
30
31impl CubicSpline {
32    /// Fit a natural cubic spline through `(xs, ys)`. `xs` must be strictly increasing
33    /// (duplicate/unsorted nodes fail closed). Needs ≥ 2 points.
34    pub fn natural(xs: &[f64], ys: &[f64]) -> Result<Self, InterpolationError> {
35        let n = xs.len();
36        if n < 2 || n != ys.len() {
37            return Err(InterpolationError::InsufficientData);
38        }
39        for i in 1..n {
40            if xs[i] <= xs[i - 1] {
41                return Err(InterpolationError::DuplicateNodes);
42            }
43        }
44        let mut m = vec![0.0; n];
45        if n >= 3 {
46            // Interior tridiagonal system for M_1..M_{n-2} (M_0 = M_{n-1} = 0).
47            let h: Vec<f64> = (0..n - 1).map(|i| xs[i + 1] - xs[i]).collect();
48            let sz = n - 2;
49            let mut sub = vec![0.0; sz]; // sub-diagonal
50            let mut diag = vec![0.0; sz];
51            let mut sup = vec![0.0; sz]; // super-diagonal
52            let mut rhs = vec![0.0; sz];
53            for k in 0..sz {
54                let i = k + 1; // interior node index
55                sub[k] = h[i - 1];
56                diag[k] = 2.0 * (h[i - 1] + h[i]);
57                sup[k] = h[i];
58                rhs[k] = 6.0 * ((ys[i + 1] - ys[i]) / h[i] - (ys[i] - ys[i - 1]) / h[i - 1]);
59            }
60            let sol = thomas(&sub, &diag, &sup, &rhs).ok_or(InterpolationError::Singular)?;
61            for k in 0..sz {
62                m[k + 1] = sol[k];
63            }
64        }
65        Ok(Self {
66            xs: xs.to_vec(),
67            ys: ys.to_vec(),
68            m,
69        })
70    }
71
72    /// Evaluate the spline at `x` (clamped to the node range).
73    pub fn eval(&self, x: f64) -> f64 {
74        let n = self.xs.len();
75        if x <= self.xs[0] {
76            return self.ys[0];
77        }
78        if x >= self.xs[n - 1] {
79            return self.ys[n - 1];
80        }
81        let i = self.xs.partition_point(|&xi| xi <= x) - 1;
82        let h = self.xs[i + 1] - self.xs[i];
83        let a = self.xs[i + 1] - x;
84        let b = x - self.xs[i];
85        self.m[i] * a.powi(3) / (6.0 * h)
86            + self.m[i + 1] * b.powi(3) / (6.0 * h)
87            + (self.ys[i] - self.m[i] * h * h / 6.0) * a / h
88            + (self.ys[i + 1] - self.m[i + 1] * h * h / 6.0) * b / h
89    }
90}
91
92/// Thomas algorithm: solve a tridiagonal system with sub/diag/super-diagonals. `None`
93/// on a zero pivot (singular).
94fn thomas(sub: &[f64], diag: &[f64], sup: &[f64], rhs: &[f64]) -> Option<Vec<f64>> {
95    let n = diag.len();
96    let mut c = vec![0.0; n];
97    let mut d = vec![0.0; n];
98    if diag[0] == 0.0 {
99        return None;
100    }
101    c[0] = sup[0] / diag[0];
102    d[0] = rhs[0] / diag[0];
103    for i in 1..n {
104        let denom = diag[i] - sub[i] * c[i - 1];
105        if denom == 0.0 {
106            return None;
107        }
108        c[i] = sup[i] / denom;
109        d[i] = (rhs[i] - sub[i] * d[i - 1]) / denom;
110    }
111    let mut x = vec![0.0; n];
112    x[n - 1] = d[n - 1];
113    for i in (0..n - 1).rev() {
114        x[i] = d[i] - c[i] * x[i + 1];
115    }
116    Some(x)
117}
118
119#[cfg(test)]
120mod tests {
121    use super::*;
122    const EPS: f64 = 1e-9;
123
124    #[test]
125    fn spline_passes_through_nodes() {
126        let xs = [0.0, 1.0, 2.0, 3.0, 4.0];
127        let ys = [0.0, 1.0, 0.0, 1.0, 0.0];
128        let s = CubicSpline::natural(&xs, &ys).unwrap();
129        for i in 0..xs.len() {
130            assert!((s.eval(xs[i]) - ys[i]).abs() < EPS);
131        }
132    }
133
134    #[test]
135    fn natural_spline_reproduces_a_line() {
136        // A natural spline of a linear function is that line (zero curvature).
137        let f = |x: f64| 2.0 * x - 1.0;
138        let xs = [0.0, 1.0, 2.5, 4.0];
139        let ys = xs.map(f);
140        let s = CubicSpline::natural(&xs, &ys).unwrap();
141        for &q in &[0.5, 1.7, 3.2] {
142            assert!((s.eval(q) - f(q)).abs() < 1e-9);
143        }
144    }
145
146    #[test]
147    fn linear_interpolation_midpoints() {
148        let xs = [0.0, 2.0, 4.0];
149        let ys = [0.0, 10.0, 0.0];
150        assert!((linear_interp(&xs, &ys, 1.0).unwrap() - 5.0).abs() < EPS);
151        assert!((linear_interp(&xs, &ys, 3.0).unwrap() - 5.0).abs() < EPS);
152        assert!((linear_interp(&xs, &ys, -1.0).unwrap() - 0.0).abs() < EPS); // clamp
153    }
154
155    #[test]
156    fn fails_closed() {
157        assert_eq!(
158            CubicSpline::natural(&[1.0], &[2.0]).unwrap_err(),
159            InterpolationError::InsufficientData
160        );
161        assert_eq!(
162            CubicSpline::natural(&[0.0, 0.0, 1.0], &[1.0, 2.0, 3.0]).unwrap_err(),
163            InterpolationError::DuplicateNodes
164        );
165    }
166}
qualia_core_db/solvers/interpolation/spline.rs

qualia_core_db/solvers/interpolation/
spline.rs
