sciforge_lib/maths/interpolation/
spline.rs1#[derive(Clone, Debug)]
2pub struct CubicSpline {
3 xs: Vec<f64>,
4 ys: Vec<f64>,
5 b: Vec<f64>,
6 c: Vec<f64>,
7 d: Vec<f64>,
8}
9
10impl CubicSpline {
11 pub fn natural(xs: &[f64], ys: &[f64]) -> Self {
12 let n = xs.len() - 1;
13 let h: Vec<f64> = (0..n).map(|i| xs[i + 1] - xs[i]).collect();
14 let alpha: Vec<f64> = (1..n)
15 .map(|i| 3.0 * ((ys[i + 1] - ys[i]) / h[i] - (ys[i] - ys[i - 1]) / h[i - 1]))
16 .collect();
17
18 let mut l = vec![1.0; n + 1];
19 let mut mu = vec![0.0; n + 1];
20 let mut z = vec![0.0; n + 1];
21
22 for i in 1..n {
23 l[i] = 2.0 * (xs[i + 1] - xs[i - 1]) - h[i - 1] * mu[i - 1];
24 mu[i] = h[i] / l[i];
25 z[i] = (alpha[i - 1] - h[i - 1] * z[i - 1]) / l[i];
26 }
27
28 let mut c_vec = vec![0.0; n + 1];
29 let mut b_vec = vec![0.0; n];
30 let mut d_vec = vec![0.0; n];
31
32 for j in (0..n).rev() {
33 c_vec[j] = z[j] - mu[j] * c_vec[j + 1];
34 b_vec[j] = (ys[j + 1] - ys[j]) / h[j] - h[j] * (c_vec[j + 1] + 2.0 * c_vec[j]) / 3.0;
35 d_vec[j] = (c_vec[j + 1] - c_vec[j]) / (3.0 * h[j]);
36 }
37
38 Self {
39 b: b_vec,
40 c: c_vec[..n].to_vec(),
41 d: d_vec,
42 xs: xs.to_vec(),
43 ys: ys.to_vec(),
44 }
45 }
46
47 pub fn eval(&self, x: f64) -> f64 {
48 let n = self.xs.len() - 1;
49 let mut i = 0;
50 for k in 0..n {
51 if x >= self.xs[k] {
52 i = k;
53 }
54 }
55 i = i.min(n - 1);
56 let dx = x - self.xs[i];
57 self.ys[i] + self.b[i] * dx + self.c[i] * dx * dx + self.d[i] * dx * dx * dx
58 }
59
60 pub fn integrate(&self, a: f64, b: f64) -> f64 {
61 let n = self.xs.len() - 1;
62 let clamp_a = a.max(self.xs[0]).min(self.xs[n]);
63 let clamp_b = b.max(self.xs[0]).min(self.xs[n]);
64 let mut total = 0.0;
65 let mut x_lo = clamp_a;
66 for k in 0..n {
67 if x_lo >= clamp_b {
68 break;
69 }
70 if x_lo >= self.xs[k + 1] {
71 continue;
72 }
73 let x_hi = clamp_b.min(self.xs[k + 1]);
74 let lo = x_lo - self.xs[k];
75 let hi = x_hi - self.xs[k];
76 let ai = self.ys[k];
77 let bi = self.b[k];
78 let ci = self.c[k];
79 let di = self.d[k];
80 let prim = |t: f64| {
81 ai * t + bi * t * t / 2.0 + ci * t * t * t / 3.0 + di * t * t * t * t / 4.0
82 };
83 total += prim(hi) - prim(lo);
84 x_lo = x_hi;
85 }
86 total
87 }
88
89 pub fn derivative(&self, x: f64) -> f64 {
90 let n = self.xs.len() - 1;
91 let mut i = 0;
92 for k in 0..n {
93 if x >= self.xs[k] {
94 i = k;
95 }
96 }
97 i = i.min(n - 1);
98 let dx = x - self.xs[i];
99 self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx * dx
100 }
101}
102
103pub fn linear_interpolate(xs: &[f64], ys: &[f64], x: f64) -> f64 {
104 let n = xs.len();
105 if x <= xs[0] {
106 return ys[0];
107 }
108 if x >= xs[n - 1] {
109 return ys[n - 1];
110 }
111 let mut i = 0;
112 for k in 0..n - 1 {
113 if x >= xs[k] && x <= xs[k + 1] {
114 i = k;
115 break;
116 }
117 }
118 let t = (x - xs[i]) / (xs[i + 1] - xs[i]);
119 ys[i] * (1.0 - t) + ys[i + 1] * t
120}
121
122pub fn bilinear_interpolate(
123 x: f64,
124 y: f64,
125 x1: f64,
126 x2: f64,
127 y1: f64,
128 y2: f64,
129 q11: f64,
130 q12: f64,
131 q21: f64,
132 q22: f64,
133) -> f64 {
134 let dx = x2 - x1;
135 let dy = y2 - y1;
136 let t = (x - x1) / dx;
137 let u = (y - y1) / dy;
138 q11 * (1.0 - t) * (1.0 - u) + q21 * t * (1.0 - u) + q12 * (1.0 - t) * u + q22 * t * u
139}
140
141pub fn hermite_interpolate(x0: f64, y0: f64, m0: f64, x1: f64, y1: f64, m1: f64, x: f64) -> f64 {
142 let t = (x - x0) / (x1 - x0);
143 let t2 = t * t;
144 let t3 = t2 * t;
145 let h00 = 2.0 * t3 - 3.0 * t2 + 1.0;
146 let h10 = t3 - 2.0 * t2 + t;
147 let h01 = -2.0 * t3 + 3.0 * t2;
148 let h11 = t3 - t2;
149 let dx = x1 - x0;
150 h00 * y0 + h10 * dx * m0 + h01 * y1 + h11 * dx * m1
151}
152
153pub fn akima_interpolate(xs: &[f64], ys: &[f64], x: f64) -> f64 {
154 let n = xs.len();
155 if n < 3 {
156 return linear_interpolate(xs, ys, x);
157 }
158 let mut slopes = Vec::with_capacity(n - 1);
159 for i in 0..n - 1 {
160 slopes.push((ys[i + 1] - ys[i]) / (xs[i + 1] - xs[i]));
161 }
162 let mut tangents = vec![0.0; n];
163 tangents[0] = slopes[0];
164 tangents[n - 1] = slopes[n - 2];
165 for i in 1..n - 1 {
166 if i >= 2 && i < n - 2 {
167 let w1 = (slopes[i] - slopes[i - 1]).abs();
168 let w2 = (slopes[i - 1] - slopes[i - 2]).abs();
169 if (w1 + w2).abs() < 1e-30 {
170 tangents[i] = 0.5 * (slopes[i - 1] + slopes[i]);
171 } else {
172 tangents[i] = (w1 * slopes[i - 1] + w2 * slopes[i]) / (w1 + w2);
173 }
174 } else {
175 tangents[i] = 0.5 * (slopes[i.saturating_sub(1)] + slopes[(i).min(n - 2)]);
176 }
177 }
178 let mut seg = 0;
179 for (k, &xk) in xs[..n - 1].iter().enumerate() {
180 if x >= xk {
181 seg = k;
182 }
183 }
184 seg = seg.min(n - 2);
185 hermite_interpolate(
186 xs[seg],
187 ys[seg],
188 tangents[seg],
189 xs[seg + 1],
190 ys[seg + 1],
191 tangents[seg + 1],
192 x,
193 )
194}
195
196pub fn catmull_rom(p0: f64, p1: f64, p2: f64, p3: f64, t: f64) -> f64 {
197 let t2 = t * t;
198 let t3 = t2 * t;
199 0.5 * ((2.0 * p1)
200 + (-p0 + p2) * t
201 + (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3) * t2
202 + (-p0 + 3.0 * p1 - 3.0 * p2 + p3) * t3)
203}
204
205pub fn monotone_cubic(xs: &[f64], ys: &[f64], x: f64) -> f64 {
206 let n = xs.len();
207 if n < 2 {
208 return ys[0];
209 }
210 let dk: Vec<f64> = (0..n - 1)
211 .map(|i| (ys[i + 1] - ys[i]) / (xs[i + 1] - xs[i]))
212 .collect();
213 let mut mk = vec![0.0; n];
214 mk[0] = dk[0];
215 mk[n - 1] = dk[n - 2];
216 for i in 1..n - 1 {
217 if dk[i - 1] * dk[i] <= 0.0 {
218 mk[i] = 0.0;
219 } else {
220 mk[i] = 0.5 * (dk[i - 1] + dk[i]);
221 }
222 }
223 for i in 0..n - 1 {
224 if dk[i].abs() < 1e-30 {
225 mk[i] = 0.0;
226 mk[i + 1] = 0.0;
227 } else {
228 let a = mk[i] / dk[i];
229 let b = mk[i + 1] / dk[i];
230 let s = a * a + b * b;
231 if s > 9.0 {
232 let tau = 3.0 / s.sqrt();
233 mk[i] = tau * a * dk[i];
234 mk[i + 1] = tau * b * dk[i];
235 }
236 }
237 }
238 let mut seg = 0;
239 for (k, &xk) in xs[..n - 1].iter().enumerate() {
240 if x >= xk {
241 seg = k;
242 }
243 }
244 seg = seg.min(n - 2);
245 hermite_interpolate(
246 xs[seg],
247 ys[seg],
248 mk[seg],
249 xs[seg + 1],
250 ys[seg + 1],
251 mk[seg + 1],
252 x,
253 )
254}
255
256pub fn pchip_interpolate(xs: &[f64], ys: &[f64], x: f64) -> f64 {
257 monotone_cubic(xs, ys, x)
258}
259
260pub fn nearest_neighbor(xs: &[f64], ys: &[f64], x: f64) -> f64 {
261 let mut best = 0;
262 let mut best_dist = (x - xs[0]).abs();
263 for (i, &xi) in xs.iter().enumerate().skip(1) {
264 let d = (x - xi).abs();
265 if d < best_dist {
266 best = i;
267 best_dist = d;
268 }
269 }
270 ys[best]
271}
272
273pub fn trilinear_interpolate(
274 x: f64,
275 y: f64,
276 z: f64,
277 x0: f64,
278 x1: f64,
279 y0: f64,
280 y1: f64,
281 z0: f64,
282 z1: f64,
283 c: &[f64; 8],
284) -> f64 {
285 let xd = (x - x0) / (x1 - x0);
286 let yd = (y - y0) / (y1 - y0);
287 let zd = (z - z0) / (z1 - z0);
288 let c00 = c[0] * (1.0 - xd) + c[4] * xd;
289 let c01 = c[1] * (1.0 - xd) + c[5] * xd;
290 let c10 = c[2] * (1.0 - xd) + c[6] * xd;
291 let c11 = c[3] * (1.0 - xd) + c[7] * xd;
292 let c0 = c00 * (1.0 - yd) + c10 * yd;
293 let c1 = c01 * (1.0 - yd) + c11 * yd;
294 c0 * (1.0 - zd) + c1 * zd
295}
296
297pub fn bezier_cubic(p0: f64, p1: f64, p2: f64, p3: f64, t: f64) -> f64 {
298 let u = 1.0 - t;
299 u * u * u * p0 + 3.0 * u * u * t * p1 + 3.0 * u * t * t * p2 + t * t * t * p3
300}
301
302pub fn bspline_basis(knots: &[f64], i: usize, degree: usize, t: f64) -> f64 {
303 if degree == 0 {
304 if t >= knots[i] && t < knots[i + 1] {
305 return 1.0;
306 }
307 return 0.0;
308 }
309 let mut left = 0.0;
310 let denom1 = knots[i + degree] - knots[i];
311 if denom1.abs() > 1e-30 {
312 left = (t - knots[i]) / denom1 * bspline_basis(knots, i, degree - 1, t);
313 }
314 let mut right = 0.0;
315 let denom2 = knots[i + degree + 1] - knots[i + 1];
316 if denom2.abs() > 1e-30 {
317 right = (knots[i + degree + 1] - t) / denom2 * bspline_basis(knots, i + 1, degree - 1, t);
318 }
319 left + right
320}