Add a 4 point spline to 6 point spline function
This makes a 6th order spline with the same shape as the provided 4th
order spline.
Change-Id: I14cf6ca0560c5a99ece2152c444dc7509dc7a34f
diff --git a/frc971/control_loops/drivetrain/spline.cc b/frc971/control_loops/drivetrain/spline.cc
index 941d520..609d3b1 100644
--- a/frc971/control_loops/drivetrain/spline.cc
+++ b/frc971/control_loops/drivetrain/spline.cc
@@ -4,6 +4,38 @@
namespace control_loops {
namespace drivetrain {
+::Eigen::Matrix<double, 2, 6> Spline4To6(
+ const ::Eigen::Matrix<double, 2, 4> &control_points) {
+ ::Eigen::Matrix<double, 2, 6> new_control_points;
+ // a' = a
+ // b' = (2a + 3b) / 5
+ // c' = (a + 6b + 3c) / 10
+ // d' = (d + 6c + 3b) / 10
+ // e' = (2d + 3c) / 5
+ // f' = d
+ new_control_points.block<2, 1>(0, 0) = control_points.block<2, 1>(0, 0);
+ new_control_points.block<2, 1>(0, 1) =
+ (2.0 * control_points.block<2, 1>(0, 0) +
+ 3.0 * control_points.block<2, 1>(0, 1)) /
+ 5.0;
+ new_control_points.block<2, 1>(0, 2) =
+ (control_points.block<2, 1>(0, 0) +
+ 6.0 * control_points.block<2, 1>(0, 1) +
+ 3.0 * control_points.block<2, 1>(0, 2)) /
+ 10.0;
+ new_control_points.block<2, 1>(0, 3) =
+ (control_points.block<2, 1>(0, 3) +
+ 6.0 * control_points.block<2, 1>(0, 2) +
+ 3.0 * control_points.block<2, 1>(0, 1)) /
+ 10.0;
+ new_control_points.block<2, 1>(0, 4) =
+ (2.0 * control_points.block<2, 1>(0, 3) +
+ 3.0 * control_points.block<2, 1>(0, 2)) /
+ 5.0;
+ new_control_points.block<2, 1>(0, 5) = control_points.block<2, 1>(0, 3);
+ return new_control_points;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971