superinstance_ffi/
spline.rs

1/// Linear interpolation along a polyline defined by control points.
2/// `pts` is a flat array of 2*n doubles (x0,y0, x1,y1, ...).
3/// `t` is in [0, 1], mapping to the full arc length.
4/// For simplicity, this does uniform-parameter linear interpolation between segments.
5#[no_mangle]
6pub extern "C" fn si_spline_interpolate(pts: *const f64, t: f64, n: usize) -> f64 {
7    if pts.is_null() || n < 2 {
8        return 0.0;
9    }
10    let s = unsafe { std::slice::from_raw_parts(pts, 2 * n) };
11    if n == 2 {
12        // simple lerp between two points, return x coordinate
13        let x0 = s[0];
14        let x1 = s[2];
15        return x0 + t * (x1 - x0);
16    }
17    // Multi-segment: distribute t across segments
18    let num_segs = n - 1;
19    let seg_t = t * num_segs as f64;
20    let seg_idx = (seg_t.floor() as usize).min(num_segs - 1);
21    let local_t = seg_t - seg_idx as f64;
22    let x0 = s[seg_idx * 2];
23    let x1 = s[(seg_idx + 1) * 2];
24    x0 + local_t * (x1 - x0)
25}
26
27/// Deadband filter: if val is within `band` of `center`, return center.
28/// Otherwise return val (pass through).
29#[no_mangle]
30pub extern "C" fn si_deadband_filter(val: f64, center: f64, band: f64) -> f64 {
31    if (val - center).abs() <= band {
32        center
33    } else {
34        val
35    }
36}
37
38#[cfg(test)]
39mod tests {
40    use super::*;
41
42    #[test]
43    fn test_spline_two_points() {
44        let pts = [0.0, 0.0, 10.0, 10.0];
45        let result = si_spline_interpolate(pts.as_ptr(), 0.5, 2);
46        assert!((result - 5.0).abs() < 1e-10);
47    }
48
49    #[test]
50    fn test_spline_start() {
51        let pts = [0.0, 0.0, 10.0, 0.0];
52        let result = si_spline_interpolate(pts.as_ptr(), 0.0, 2);
53        assert!((result - 0.0).abs() < 1e-10);
54    }
55
56    #[test]
57    fn test_spline_end() {
58        let pts = [0.0, 0.0, 10.0, 0.0];
59        let result = si_spline_interpolate(pts.as_ptr(), 1.0, 2);
60        assert!((result - 10.0).abs() < 1e-10);
61    }
62
63    #[test]
64    fn test_deadband_inside() {
65        assert_eq!(si_deadband_filter(5.1, 5.0, 0.2), 5.0);
66    }
67
68    #[test]
69    fn test_deadband_outside() {
70        assert_eq!(si_deadband_filter(5.5, 5.0, 0.2), 5.5);
71    }
72
73    #[test]
74    fn test_deadband_edge() {
75        // Clearly inside the band
76        assert_eq!(si_deadband_filter(5.1, 5.0, 0.2), 5.0);
77        // Clearly outside
78        assert_eq!(si_deadband_filter(5.3, 5.0, 0.2), 5.3);
79    }
80}
superinstance_ffi/spline.rs

superinstance_ffi/
spline.rs
