[wpimath] Fix quintic spline generation from control vectors (#2762)

This does not introduce any breaking changes for teams that used the TrajectoryGenerator API for
quintic splines with poses.

The GetQuinticControlVectorsFromWaypoints() method was removed because it is not possible for us (or
would require breaking changes to the shape of the splines) to generate only one quintic control vector
per Pose2d.  Users who actually have control vectors directly (i.e. not from Pose2d objects, but a
dashboard such as PathWeaver) should have the number of control vectors correspond to the number of
"waypoints" and can call GetQuinticSplinesFromControlVectors() directly.

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

@@ -7,7 +7,6 @@
 
 package edu.wpi.first.wpilibj.spline;
 
-import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 
@@ -53,26 +52,28 @@ public final class SplineHelper {
   }
 
   /**
-   * Returns quintic control vectors from a set of waypoints.
+   * Returns quintic splines from a set of waypoints.
    *
    * @param waypoints The waypoints
-   * @return List of control vectors
+   * @return List of splines.
    */
-  public static List<Spline.ControlVector> getQuinticControlVectorsFromWaypoints(
-      List<Pose2d> waypoints
-  ) {
-    List<Spline.ControlVector> vectors = new ArrayList<>();
-    for (int i = 0; i < waypoints.size() - 1; i++) {
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
+    for (int i = 0; i < waypoints.size() - 1; ++i) {
       var p0 = waypoints.get(i);
       var p1 = waypoints.get(i + 1);
 
       // This just makes the splines look better.
       final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
 
-      vectors.add(getQuinticControlVector(scalar, p0));
-      vectors.add(getQuinticControlVector(scalar, p1));
+      var controlVecA = getQuinticControlVector(scalar, p0);
+      var controlVecB = getQuinticControlVector(scalar, p1);
+
+      splines[i]
+          = new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
     }
-    return vectors;
+    return splines;
   }
 
   /**
@@ -216,15 +217,14 @@ public final class SplineHelper {
   @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
   public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
       Spline.ControlVector[] controlVectors) {
-    // There are twice as many control vectors are there are splines.
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length / 2];
-    for (int i = 0; i < controlVectors.length - 1; i += 2) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
       var xInitial = controlVectors[i].x;
       var xFinal = controlVectors[i + 1].x;
       var yInitial = controlVectors[i].y;
       var yFinal = controlVectors[i + 1].y;
-      splines[i / 2] = new QuinticHermiteSpline(xInitial, xFinal,
-          yInitial, yFinal);
+      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
+                                            yInitial, yFinal);
     }
     return splines;
   }

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

@@ -196,9 +196,38 @@ public final class TrajectoryGenerator {
    */
   @SuppressWarnings("LocalVariableName")
   public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
-    var originalList = SplineHelper.getQuinticControlVectorsFromWaypoints(waypoints);
-    var newList = new ControlVectorList(originalList);
-    return generateTrajectory(newList, config);
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    List<Pose2d> newWaypoints = new ArrayList<>();
+    if (config.isReversed()) {
+      for (Pose2d originalWaypoint : waypoints) {
+        newWaypoints.add(originalWaypoint.plus(flip));
+      }
+    } else {
+      newWaypoints.addAll(waypoints);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
+        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
+        config.getMaxAcceleration(), config.isReversed());
   }
 
   /**