[choreolib] Add trajectory mirroring for x and y

choreolib/src/main/java/choreo/auto/AutoChooser.java

@@ -69,6 +69,8 @@ public AutoChooser(String doNothingName) {
   }
 
   /**
+   * Returns the name of the default do-nothing option.
+   *
    * @return the name of the default do-nothing option.
    */
   public String getDefaultName() {

choreolib/src/main/java/choreo/auto/AutoTrajectory.java

@@ -68,6 +68,7 @@ public class AutoTrajectory {
   private final Timer inactiveTimer = new Timer();
   private final Subsystem driveSubsystem;
   private final AutoRoutine routine;
+  private final AutoBindings bindings;
 
   /**
    * A way to create slightly less triggers for many actions. Not static as to not leak triggers
@@ -115,6 +116,7 @@ <SampleType extends TrajectorySample<SampleType>> AutoTrajectory(
     this.routine = routine;
     this.offTrigger = new Trigger(routine.loop(), () -> false);
     this.trajectoryLogger = trajectoryLogger;
+    this.bindings = bindings;
 
     bindings.getBindings().forEach((key, value) -> active().and(atTime(key)).onTrue(value));
   }
@@ -268,6 +270,48 @@ Trajectory<SampleType> getRawTrajectory() {
     return (Trajectory<SampleType>) trajectory;
   }
 
+  /**
+   * Returns this auto trajectory, mirrored across the field width.
+   *
+   * @param <SampleType> The type of the trajectory samples.
+   * @return this auto trajectory, mirrored across the field width.
+   */
+  @SuppressWarnings("unchecked")
+  public <SampleType extends TrajectorySample<SampleType>> AutoTrajectory mirrorX() {
+    return new AutoTrajectory(
+        name,
+        (Trajectory<SampleType>) trajectory.mirrorX(),
+        poseSupplier,
+        resetOdometry,
+        (Consumer<SampleType>) controller,
+        allianceCtx,
+        (TrajectoryLogger<SampleType>) trajectoryLogger,
+        driveSubsystem,
+        routine,
+        bindings);
+  }
+
+  /**
+   * Returns this auto trajectory, mirrored across the field length.
+   *
+   * @param <SampleType> The type of the trajectory samples.
+   * @return this auto trajectory, mirrored across the field length.
+   */
+  @SuppressWarnings("unchecked")
+  public <SampleType extends TrajectorySample<SampleType>> AutoTrajectory mirrorY() {
+    return new AutoTrajectory(
+        name,
+        (Trajectory<SampleType>) trajectory.mirrorY(),
+        poseSupplier,
+        resetOdometry,
+        (Consumer<SampleType>) controller,
+        allianceCtx,
+        (TrajectoryLogger<SampleType>) trajectoryLogger,
+        driveSubsystem,
+        routine,
+        bindings);
+  }
+
   /**
    * Will get the starting pose of the trajectory.
    *

choreolib/src/main/java/choreo/trajectory/DifferentialSample.java

@@ -171,36 +171,17 @@ public DifferentialSample interpolate(DifferentialSample endValue, double timest
   }
 
   public DifferentialSample flipped() {
-    return switch (ChoreoAllianceFlipUtil.getFlipper()) {
-      case MIRRORED ->
-          new DifferentialSample(
-              t,
-              ChoreoAllianceFlipUtil.flipX(x),
-              ChoreoAllianceFlipUtil.flipY(y), // No-op for mirroring
-              ChoreoAllianceFlipUtil.flipHeading(heading),
-              vr,
-              vl,
-              -omega,
-              ar,
-              al,
-              -alpha,
-              fr,
-              fl);
-      case ROTATE_AROUND ->
-          new DifferentialSample(
-              t,
-              ChoreoAllianceFlipUtil.flipX(x),
-              ChoreoAllianceFlipUtil.flipY(y),
-              ChoreoAllianceFlipUtil.flipHeading(heading),
-              vl,
-              vr,
-              omega,
-              al,
-              ar,
-              alpha,
-              fl,
-              fr);
-    };
+    return ChoreoAllianceFlipUtil.getFlipper().flip(this);
+  }
+
+  @Override
+  public DifferentialSample mirrorY() {
+    return ChoreoAllianceFlipUtil.getMirrorY().flip(this);
+  }
+
+  @Override
+  public DifferentialSample mirrorX() {
+    return ChoreoAllianceFlipUtil.getMirrorX().flip(this);
   }
 
   public DifferentialSample offsetBy(double timestampOffset) {
@@ -287,17 +268,17 @@ public boolean equals(Object obj) {
     }
 
     var other = (DifferentialSample) obj;
-    return this.t == other.t
-        && this.x == other.x
-        && this.y == other.y
-        && this.heading == other.heading
-        && this.vl == other.vl
-        && this.vr == other.vr
-        && this.omega == other.omega
-        && this.al == other.al
-        && this.ar == other.ar
-        && this.alpha == other.alpha
-        && this.fl == other.fl
-        && this.fr == other.fr;
+    return MathUtil.isNear(this.t, other.t, 1E-6)
+        && MathUtil.isNear(this.x, other.x, 1E-6)
+        && MathUtil.isNear(this.y, other.y, 1E-6)
+        && MathUtil.isNear(this.heading, other.heading, 1E-6)
+        && MathUtil.isNear(this.vl, other.vl, 1E-6)
+        && MathUtil.isNear(this.vr, other.vr, 1E-6)
+        && MathUtil.isNear(this.omega, other.omega, 1E-6)
+        && MathUtil.isNear(this.al, other.al, 1E-6)
+        && MathUtil.isNear(this.ar, other.ar, 1E-6)
+        && MathUtil.isNear(this.alpha, other.alpha, 1E-6)
+        && MathUtil.isNear(this.fl, other.fl, 1E-6)
+        && MathUtil.isNear(this.fr, other.fr, 1E-6);
   }
 }

choreolib/src/main/java/choreo/trajectory/SwerveSample.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.util.struct.Struct;
 import java.nio.ByteBuffer;
-import java.util.Arrays;
 
 /** A single swerve robot sample in a Trajectory. */
 public class SwerveSample implements TrajectorySample<SwerveSample> {
@@ -199,52 +198,17 @@ public SwerveSample offsetBy(double timestampOffset) {
 
   @Override
   public SwerveSample flipped() {
-    return switch (ChoreoAllianceFlipUtil.getFlipper()) {
-      case MIRRORED ->
-          new SwerveSample(
-              this.t,
-              ChoreoAllianceFlipUtil.flipX(this.x),
-              ChoreoAllianceFlipUtil.flipY(this.y),
-              ChoreoAllianceFlipUtil.flipHeading(this.heading),
-              -this.vx,
-              this.vy,
-              -this.omega,
-              -this.ax,
-              this.ay,
-              -this.alpha,
-              // FL, FR, BL, BR
-              // Mirrored
-              // -FR, -FL, -BR, -BL
-              new double[] {
-                -this.moduleForcesX()[1],
-                -this.moduleForcesX()[0],
-                -this.moduleForcesX()[3],
-                -this.moduleForcesX()[2]
-              },
-              // FL, FR, BL, BR
-              // Mirrored
-              // FR, FL, BR, BL
-              new double[] {
-                this.moduleForcesY()[1],
-                this.moduleForcesY()[0],
-                this.moduleForcesY()[3],
-                this.moduleForcesY()[2]
-              });
-      case ROTATE_AROUND ->
-          new SwerveSample(
-              this.t,
-              ChoreoAllianceFlipUtil.flipX(this.x),
-              ChoreoAllianceFlipUtil.flipY(this.y),
-              ChoreoAllianceFlipUtil.flipHeading(this.heading),
-              -this.vx,
-              -this.vy,
-              this.omega,
-              -this.ax,
-              -this.ay,
-              this.alpha,
-              Arrays.stream(this.moduleForcesX()).map(x -> -x).toArray(),
-              Arrays.stream(this.moduleForcesY()).map(y -> -y).toArray());
-    };
+    return ChoreoAllianceFlipUtil.flip(this);
+  }
+
+  @Override
+  public SwerveSample mirrorY() {
+    return ChoreoAllianceFlipUtil.getMirrorY().flip(this);
+  }
+
+  @Override
+  public SwerveSample mirrorX() {
+    return ChoreoAllianceFlipUtil.getMirrorX().flip(this);
   }
 
   /** The struct for the SwerveSample class. */
@@ -330,19 +294,19 @@ public boolean equals(Object obj) {
     }
 
     var other = (SwerveSample) obj;
-    return this.t == other.t
-        && this.x == other.x
-        && this.y == other.y
-        && this.heading == other.heading
-        && this.vx == other.vx
-        && this.vy == other.vy
-        && this.omega == other.omega
-        && this.ax == other.ax
-        && this.ay == other.ay
-        && this.alpha == other.alpha
+    return MathUtil.isNear(this.t, other.t, 1E-6)
+        && MathUtil.isNear(this.x, other.x, 1E-6)
+        && MathUtil.isNear(this.y, other.y, 1E-6)
+        && MathUtil.isNear(this.heading, other.heading, 1E-6)
+        && MathUtil.isNear(this.vx, other.vx, 1E-6)
+        && MathUtil.isNear(this.vy, other.vy, 1E-6)
+        && MathUtil.isNear(this.omega, other.omega, 1E-6)
+        && MathUtil.isNear(this.ax, other.ax, 1E-6)
+        && MathUtil.isNear(this.ay, other.ay, 1E-6)
+        && MathUtil.isNear(this.alpha, other.alpha, 1E-6)
         && ChoreoArrayUtil.zipEquals(
-            this.fx, other.fx, (a, b) -> a.doubleValue() == b.doubleValue())
+            this.fx, other.fx, (a, b) -> MathUtil.isNear(a.doubleValue(), b.doubleValue(), 1E-6))
         && ChoreoArrayUtil.zipEquals(
-            this.fy, other.fy, (a, b) -> a.doubleValue() == b.doubleValue());
+            this.fy, other.fy, (a, b) -> MathUtil.isNear(a.doubleValue(), b.doubleValue(), 1E-6));
   }
 }

choreolib/src/main/java/choreo/trajectory/Trajectory.java

@@ -221,6 +221,32 @@ public Trajectory<SampleType> flipped() {
     return new Trajectory<SampleType>(this.name, flippedStates, this.splits, this.events);
   }
 
+  /**
+   * Returns this trajectory, mirrored across the field width.
+   *
+   * @return this trajectory, mirrored across the field width.
+   */
+  public Trajectory<SampleType> mirrorX() {
+    var flippedStates = new ArrayList<SampleType>();
+    for (var state : samples) {
+      flippedStates.add(state.mirrorX());
+    }
+    return new Trajectory<SampleType>(this.name, flippedStates, this.splits, this.events);
+  }
+
+  /**
+   * Returns this trajectory, mirrored across the field length.
+   *
+   * @return this trajectory, mirrored across the field length.
+   */
+  public Trajectory<SampleType> mirrorY() {
+    var flippedStates = new ArrayList<SampleType>();
+    for (var state : samples) {
+      flippedStates.add(state.mirrorY());
+    }
+    return new Trajectory<SampleType>(this.name, flippedStates, this.splits, this.events);
+  }
+
   /**
    * Returns a list of all events with the given name in the trajectory.
    *

choreolib/src/main/java/choreo/trajectory/TrajectorySample.java

@@ -42,6 +42,20 @@ public interface TrajectorySample<Self extends TrajectorySample<Self>>
    */
   Self flipped();
 
+  /**
+   * Returns this sample, mirrored across the field length.
+   *
+   * @return this sample, mirrored across the field length.
+   */
+  Self mirrorY();
+
+  /**
+   * Returns this sample, mirrored across the field width.
+   *
+   * @return this sample, mirrored across the field width.
+   */
+  Self mirrorX();
+
   /**
    * Returns this sample, offset by the given timestamp.
    *