embedded_dsp/
interpolation.rs

1//! Interpolation functions (Linear, Bilinear, Spline).
2
3use crate::types::*;
4
5// --- Linear Interpolation ---
6
7/// Linear interpolation for f32: y = y0 + (x - x0) * (y1 - y0) / (x1 - x0).
8pub fn linear_interp_f32(table: &[f32], x: f32, x_step: f32) -> f32 {
9    if table.is_empty() || x_step <= 0.0 {
10        return 0.0;
11    }
12    let idx_f = x / x_step;
13    let idx = idx_f as usize;
14    if idx >= table.len() - 1 {
15        return table[table.len() - 1];
16    }
17    let frac = idx_f - (idx as f32);
18    table[idx] + frac * (table[idx + 1] - table[idx])
19}
20
21pub fn linear_interp_q31(table: &[q31], x: q31) -> q31 {
22    if table.len() < 2 {
23        return 0;
24    }
25    let idx_f = ((x as i64 + 2147483648) as u64 * (table.len() - 1) as u64) >> 31;
26    let idx = (idx_f as usize).min(table.len() - 2);
27    let y0 = table[idx] as i64;
28    let y1 = table[idx + 1] as i64;
29    ((y0 + y1) / 2) as q31
30}
31
32pub fn linear_interp_q15(table: &[q15], x: q15) -> q15 {
33    if table.len() < 2 {
34        return 0;
35    }
36    let idx_f = ((x as i32 + 32768) as u32 * (table.len() - 1) as u32) >> 15;
37    let idx = (idx_f as usize).min(table.len() - 2);
38    let y0 = table[idx] as i32;
39    let y1 = table[idx + 1] as i32;
40    ((y0 + y1) / 2) as q15
41}
42
43// --- Bilinear Interpolation ---
44
45/// Bilinear interpolation for 2D grid of size `num_rows x num_cols`.
46pub fn bilinear_interp_f32(table: &[f32], num_rows: usize, num_cols: usize, x: f32, y: f32) -> f32 {
47    let r = x.clamp(0.0, (num_rows - 1) as f32);
48    let c = y.clamp(0.0, (num_cols - 1) as f32);
49
50    let r0 = r as usize;
51    let r1 = (r0 + 1).min(num_rows - 1);
52    let c0 = c as usize;
53    let c1 = (c0 + 1).min(num_cols - 1);
54
55    let fr = r - r0 as f32;
56    let fc = c - c0 as f32;
57
58    let f00 = table[r0 * num_cols + c0];
59    let f01 = table[r0 * num_cols + c1];
60    let f10 = table[r1 * num_cols + c0];
61    let f11 = table[r1 * num_cols + c1];
62
63    (1.0 - fr) * ((1.0 - fc) * f00 + fc * f01) + fr * ((1.0 - fc) * f10 + fc * f11)
64}
65
66// --- Cubic Spline Interpolation ---
67
68pub struct SplineInstanceF32<'a> {
69    pub x: &'a [f32],
70    pub y: &'a [f32],
71    pub coeffs: &'a [f32], // 4 * (n - 1) coefficients: [a, b, c, d] per segment
72}
73
74impl<'a> SplineInstanceF32<'a> {
75    pub fn interpolate(&self, x_val: f32) -> f32 {
76        let n = self.x.len();
77        if n == 0 {
78            return 0.0;
79        }
80        if x_val <= self.x[0] {
81            return self.y[0];
82        }
83        if x_val >= self.x[n - 1] {
84            return self.y[n - 1];
85        }
86
87        let mut idx = 0;
88        for i in 0..(n - 1) {
89            if x_val >= self.x[i] && x_val <= self.x[i + 1] {
90                idx = i;
91                break;
92            }
93        }
94        let dx = x_val - self.x[idx];
95        let a = self.coeffs[idx * 4];
96        let b = self.coeffs[idx * 4 + 1];
97        let c = self.coeffs[idx * 4 + 2];
98        let d = self.coeffs[idx * 4 + 3];
99
100        a + b * dx + c * dx * dx + d * dx * dx * dx
101    }
102}
embedded_dsp/interpolation.rs

embedded_dsp/
interpolation.rs
