1 files changed, 70 insertions(+), 0 deletions(-)

A src/linear_interpolation.rs

@@ 0,0 1,70 @@
+use alloc::vec::Vec;
+
+pub struct Point<Position, Value> {
+    position: Position,
+    value: Value,
+}
+
+impl<Position, Value> Point<Position, Value> {
+    pub fn new(position: Position, value: Value) -> Self {
+        Self { position, value }
+    }
+}
+
+impl<Position: Copy, Value: Copy> Point<Position, Value> {
+    pub fn deconstruct(&self) -> (Position, Value) {
+        (self.position, self.value)
+    }
+}
+
+pub struct LinearInterpolation<Position, Value> {
+    points: Vec<Point<Position, Value>>,
+}
+
+impl<Position: Ord + PartialOrd + Copy, Value: Copy> LinearInterpolation<Position, Value> {
+    pub fn closest_position_indices(&self, position: Position) -> Option<(usize, usize)> {
+        let mut closest_lower_index = 0;
+        let mut closest_upper_index = self.points.len() - 1;
+        let p = &self.points;
+        for i in 1..self.points.len() {
+            let curr_position = p[i].position;
+            if curr_position > p[closest_lower_index].position && curr_position <= position {
+                closest_lower_index = i;
+            }
+
+            if curr_position < p[closest_upper_index].position && curr_position >= position {
+                closest_upper_index = i;
+            }
+        }
+
+        if p[closest_lower_index].position <= position
+            && p[closest_upper_index].position >= position
+        {
+            Some((closest_lower_index, closest_upper_index))
+        } else {
+            None
+        }
+    }
+}
+
+impl LinearInterpolation<u16, u16> {
+    pub fn new(points: Vec<Point<u16, u16>>) -> Self {
+        Self { points }
+    }
+
+    pub fn interpolate(&self, position: u16) -> Option<u16> {
+        let (lower, upper) = self.closest_position_indices(position)?;
+
+        if lower == upper {
+            return Some(self.points[lower].value);
+        }
+
+        let (lower, upper) = (&self.points[lower], &self.points[upper]);
+
+        let diff = upper.position - lower.position;
+        let value_diff = upper.value as i32 - lower.value as i32;
+        let position_relative = (position - lower.position) as f32 / diff as f32;
+
+        Some((lower.value as f32 + (value_diff as f32 * position_relative)) as u16)
+    }
+}