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/BUILD b/frc971/math/BUILD
index f33b9f9..dd160d0 100644
--- a/frc971/math/BUILD
+++ b/frc971/math/BUILD
@@ -36,3 +36,19 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "interpolate",
+ hdrs = ["interpolate.h"],
+ visibility = ["//visibility:public"],
+)
+
+cc_test(
+ name = "interpolate_test",
+ srcs = ["interpolate_test.cc"],
+ deps = [
+ ":interpolate",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
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_
diff --git a/frc971/math/interpolate_test.cc b/frc971/math/interpolate_test.cc
new file mode 100644
index 0000000..4d52e64
--- /dev/null
+++ b/frc971/math/interpolate_test.cc
@@ -0,0 +1,20 @@
+#include "frc971/math/interpolate.h"
+
+#include "gtest/gtest.h"
+#include <Eigen/Core>
+
+namespace frc971::math::testing {
+// Tests that when we "interpolate" exactly to the ends of the interpolation
+// that we get the correct result.
+TEST(InterpolateTest, ExactEnds) {
+ const Eigen::Vector3d a(1.0, 2.0, 3.0), b(4.0, 5.0, 6.0);
+ ASSERT_EQ(a, lerp(a, b, 0.0));
+ ASSERT_EQ(Eigen::Vector3d(2.5, 3.5, 4.5), lerp(a, b, 0.5));
+ ASSERT_EQ(b, lerp(a, b, 1.0));
+
+ ASSERT_EQ(a, Interpolate(10.0, 20.0, a, b, 10.0));
+ ASSERT_EQ(b, Interpolate(10.0, 20.0, a, b, 20.0));
+ ASSERT_EQ(Eigen::Vector3d(2.5, 3.5, 4.5),
+ Interpolate(10.0, 20.0, a, b, 15.0));
+}
+} // namespace frc971::math::testing