Add simple interpolation library
I found myself wanting this for use with some imu calibration code I was
prototyping.
Change-Id: I1b51cdecbbd8377fc63e24f7abcb8d784ed440c3
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/math/interpolate.h b/frc971/math/interpolate.h
new file mode 100644
index 0000000..a00b123
--- /dev/null
+++ b/frc971/math/interpolate.h
@@ -0,0 +1,21 @@
+#ifndef FRC971_MATH_INTERPOLATE_H_
+#define FRC971_MATH_INTERPOLATE_H_
+namespace frc971::math {
+
+// Takes a and b linear interpolates between the two based on the scalar t.
+// If t == 0, returns a; if t == 1.0, returns b.
+// The semantics of this should be identical to std::lerp().
+template <typename T, typename Scalar>
+T lerp(const T &a, const T &b, Scalar t) {
+ return (static_cast<Scalar>(1.0) - t) * a + t * b;
+}
+
+// For two points (x1, y1) and (x2, y2) uses a linear interpolation
+// to identify the value of y at x.
+// Will linearly extrapolate if x is outside of [x1, x2].
+template <typename T, typename Scalar>
+T Interpolate(Scalar x1, Scalar x2, const T &y1, const T &y2, Scalar x) {
+ return lerp(y1, y2, (x - x1) / (x2 - x1));
+}
+} // namespace frc971::math
+#endif // FRC971_MATH_INTERPOLATE_H_