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_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