[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/java/edu/wpi/first/math/spline/Spline.java

@@ -7,6 +7,7 @@ package edu.wpi.first.math.spline;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.Arrays;
+import java.util.Optional;
 import org.ejml.simple.SimpleMatrix;
 
 /** Represents a two-dimensional parametric spline that interpolates between two points. */
@@ -49,7 +50,7 @@ public abstract class Spline {
    * @param t The point t
    * @return The pose and curvature at that point.
    */
-  public PoseWithCurvature getPoint(double t) {
+  public Optional<PoseWithCurvature> getPoint(double t) {
     SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
     final var coefficients = getCoefficients();
 
@@ -86,10 +87,14 @@ public abstract class Spline {
       ddy = combined.get(5, 0) / t / t;
     }
 
+    if (Math.hypot(dx, dy) < 1e-6) {
+      return Optional.empty();
+    }
+
     // Find the curvature.
     final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
 
-    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
+    return Optional.of(new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature));
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java

@@ -38,6 +38,11 @@ public final class SplineParameterizer {
   private static final double kMaxDy = 0.00127;
   private static final double kMaxDtheta = 0.0872;
 
+  private static final String 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.";
+
   /**
    * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size stays
    * at a relatively small number (e.g. 30) and never decreases. Because of this, we must count
@@ -99,39 +104,41 @@ public final class SplineParameterizer {
     var splinePoints = new ArrayList<PoseWithCurvature>();
 
     // The parameterization does not add the initial point. Let's add that.
-    splinePoints.add(spline.getPoint(t0));
+    splinePoints.add(spline.getPoint(t0).get());
 
     // We use an "explicit stack" to simulate recursion, instead of a recursive function call
     // This give us greater control, instead of a stack overflow
     var stack = new ArrayDeque<StackContents>();
     stack.push(new StackContents(t0, t1));
 
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
     int iterations = 0;
 
     while (!stack.isEmpty()) {
-      current = stack.removeFirst();
-      start = spline.getPoint(current.t0);
-      end = spline.getPoint(current.t1);
+      var current = stack.removeFirst();
 
-      final var twist = start.poseMeters.log(end.poseMeters);
+      var start = spline.getPoint(current.t0);
+      if (!start.isPresent()) {
+        throw new MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      var end = spline.getPoint(current.t1);
+      if (!end.isPresent()) {
+        throw new MalformedSplineException(kMalformedSplineExceptionMsg);
+      }
+
+      final var twist = start.get().poseMeters.log(end.get().poseMeters);
       if (Math.abs(twist.dy) > kMaxDy
           || Math.abs(twist.dx) > kMaxDx
           || Math.abs(twist.dtheta) > kMaxDtheta) {
         stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
         stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
       } else {
-        splinePoints.add(spline.getPoint(current.t1));
+        splinePoints.add(end.get());
       }
 
       iterations++;
       if (iterations >= kMaxIterations) {
-        throw new 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 new MalformedSplineException(kMalformedSplineExceptionMsg);
       }
     }
 

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java

@@ -246,7 +246,7 @@ public final class TrajectoryGenerator {
     var splinePoints = new ArrayList<PoseWithCurvature>();
 
     // Add the first point to the vector.
-    splinePoints.add(splines[0].getPoint(0.0));
+    splinePoints.add(splines[0].getPoint(0.0).get());
 
     // Iterate through the vector and parameterize each spline, adding the
     // parameterized points to the final vector.