Fix SplineHelper bug (#2018)

Add unit test to check interior waypoints

wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp

@@ -73,6 +73,22 @@ class CubicHermiteSplineTest : public ::testing::Test {
     EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
                 a.Rotation().Radians().to<double>(), 1E-9);
 
+    // Check interior waypoints
+    bool interiorsGood = true;
+    for (auto& waypoint : waypoints) {
+      bool found = false;
+      for (auto& state : poses) {
+        if (std::abs(
+                waypoint.Distance(state.first.Translation()).to<double>()) <
+            1E-9) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    EXPECT_TRUE(interiorsGood);
+
     // Check last point.
     EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
                 b.Translation().X().to<double>(), 1E-9);
@@ -97,3 +113,10 @@ TEST_F(CubicHermiteSplineTest, SCurve) {
   Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
   Run(start, waypoints, end);
 }
+
+TEST_F(CubicHermiteSplineTest, OneInterior) {
+  Pose2d start{0_m, 0_m, 0_rad};
+  std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+  Pose2d end{4_m, 0_m, 0_rad};
+  Run(start, waypoints, end);
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java

@@ -175,7 +175,7 @@ public final class SplineHelper {
           / 4.0;
 
       double[] midXControlVector = {waypoints[0].getX(), xDeriv};
-      double[] midYControlVector = {waypoints[0].getX(), yDeriv};
+      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
 
       splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
           yInitial, midYControlVector);

wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java

@@ -76,6 +76,20 @@ class CubicHermiteSplineTest {
             poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
     );
 
+    // Check interior waypoints
+    boolean interiorsGood = true;
+    for (var waypoint : waypoints) {
+      boolean found = false;
+      for (var state : poses) {
+        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    assertTrue(interiorsGood);
+
     // Check last point
     assertAll(
         () -> assertEquals(b.getTranslation().getX(),
@@ -104,4 +118,15 @@ class CubicHermiteSplineTest {
 
     run(start, waypoints, end);
   }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testOneInterior() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(2.0, 0.0));
+    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
 }