[wpimath] Exit early when parameterizing malformed spline (#6827)

Currently, a max iteration heuristic is used to determine when a spline
is malformed. Instead, we can report a failure immediately if dx and dy
are too small, because the heading won't be accurate either.

Fixes #6826.

wpimath/src/main/native/include/frc/spline/Spline.h

@@ -4,8 +4,8 @@
 
 #pragma once
 
+#include <optional>
 #include <utility>
-#include <vector>
 
 #include <wpi/array.h>
 
@@ -57,7 +57,7 @@ class Spline {
    * @param t The point t
    * @return The pose and curvature at that point.
    */
-  PoseWithCurvature GetPoint(double t) const {
+  std::optional<PoseWithCurvature> GetPoint(double t) const {
     Vectord<Degree + 1> polynomialBases;
 
     // Populate the polynomial bases
@@ -88,11 +88,15 @@ class Spline {
       ddy = combined(5) / t / t;
     }
 
+    if (std::hypot(dx, dy) < 1e-6) {
+      return std::nullopt;
+    }
+
     // Find the curvature.
     const auto curvature =
         (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
 
-    return {
+    return PoseWithCurvature{
         {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
         units::curvature_t{curvature}};
   }

wpimath/src/main/native/include/frc/spline/SplineParameterizer.h

@@ -72,28 +72,41 @@ class WPILIB_DLLEXPORT SplineParameterizer {
   static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
                                                      double t0 = 0.0,
                                                      double t1 = 1.0) {
+    constexpr const char* kMalformedSplineExceptionMsg =
+        "Could not parameterize a malformed spline. This means that you "
+        "probably had two or more adjacent waypoints that were very close "
+        "together with headings in opposing directions.";
     std::vector<PoseWithCurvature> splinePoints;
 
     // The parameterization does not add the initial point. Let's add that.
-    splinePoints.push_back(spline.GetPoint(t0));
+    if (auto point = spline.GetPoint(t0)) {
+      splinePoints.push_back(point.value());
+    } else {
+      throw MalformedSplineException(kMalformedSplineExceptionMsg);
+    }
 
     // We use an "explicit stack" to simulate recursion, instead of a recursive
     // function call This give us greater control, instead of a stack overflow
     std::stack<StackContents> stack;
     stack.emplace(StackContents{t0, t1});
 
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
     int iterations = 0;
 
     while (!stack.empty()) {
-      current = stack.top();
+      auto current = stack.top();
       stack.pop();
-      start = spline.GetPoint(current.t0);
-      end = spline.GetPoint(current.t1);
 
-      const auto twist = start.first.Log(end.first);
+      auto start = spline.GetPoint(current.t0);
+      if (!start) {
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      auto end = spline.GetPoint(current.t1);
+      if (!end) {
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      const auto twist = start.value().first.Log(end.value().first);
 
       if (units::math::abs(twist.dy) > kMaxDy ||
           units::math::abs(twist.dx) > kMaxDx ||
@@ -101,15 +114,11 @@ class WPILIB_DLLEXPORT SplineParameterizer {
         stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
         stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
       } else {
-        splinePoints.push_back(spline.GetPoint(current.t1));
+        splinePoints.push_back(end.value());
       }
 
       if (iterations++ >= kMaxIterations) {
-        throw MalformedSplineException(
-            "Could not parameterize a malformed spline. "
-            "This means that you probably had two or more adjacent "
-            "waypoints that were very close together with headings "
-            "in opposing directions.");
+        throw MalformedSplineException(kMalformedSplineExceptionMsg);
       }
     }
 

wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

@@ -99,7 +99,7 @@ class WPILIB_DLLEXPORT TrajectoryGenerator {
     std::vector<PoseWithCurvature> splinePoints;
 
     // Add the first point to the vector.
-    splinePoints.push_back(splines.front().GetPoint(0.0));
+    splinePoints.push_back(splines.front().GetPoint(0.0).value());
 
     // Iterate through the vector and parameterize each spline, adding the
     // parameterized points to the final vector.