moved it to wpimath and added interface

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.hal.DriverStationJNI;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.MatchInfoData;
+import edu.wpi.first.math.geometry.AllianceFlipper;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -29,6 +30,13 @@
 
 /** Provide access to the network communication data to / from the Driver Station. */
 public final class DriverStation {
+  static {
+    // i don't like this, 110% open to suggestions
+    AllianceFlipper.setOnRed(
+      () -> getAlliance().orElse(Alliance.Blue) == Alliance.Red
+    );
+  }
+
   /** Number of Joystick ports. */
   public static final int kJoystickPorts = 6;
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java → wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java

@@ -1,38 +1,14 @@
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
+package edu.wpi.first.math.geometry;
+
 import java.util.HashMap;
+import java.util.function.BooleanSupplier;
 
 /**
  * A utility to standardize flipping of coordinate data based on the current alliance across
  * different years.
  *
  * <p>If every vendor used this, the user would be able to specify the year and no matter the year
  * the vendor's code is from, the user would be able to flip as expected.
- *
- * <p>This api still allows vendors and users to match case against the flipping variant as a way to
- * specially handle cases or throw errors if a variant is explicitly not supported.
- * 
- * <p>Flipping a custom structure that is based on the field coordiante system looks like
- * <pre><code>
- * public class CustomStructure {
- *   public double x, y, vx, vy;
- *   public CustomStructure( ... ) { ... }
- * 
- *   public CustomStructure flip() {
- *     return switch (AllianceFlipper.getFlipper()) {
- *       case VERTICALLY_MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), y, -vx, vy);
- *       case ROTATIONALLY_MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), AllianceFlipper.flipY(y), -vx, -vy);
- *     };
- *   }
- * }
- * </code></pre>
  */
 public class AllianceFlipper {
   private AllianceFlipper() {
@@ -98,21 +74,44 @@ public double flipHeading(double heading) {
     public abstract double flipHeading(double heading);
   }
 
+  /**
+   * An interface for objects that can be flipped based on the current alliance.
+   */
+  public static interface Flippable<Self extends Flippable<Self>> {
+    /**
+     * Flips the object based on the supplied flipper.
+     */
+    public Self flip(Flipper flipper);
+
+    /**
+     * Flips the object based on the active flipper.
+     */
+    public default Self flip() {
+      return flip(getFlipper());
+    }
+
+    /**
+     * Flips the object if on the red alliance, otherwise returns the object unchanged.
+     */
+    @SuppressWarnings("unchecked")
+    public default Self flipIfRed() {
+        return onRed() ? flip() : (Self) this;
+    }
+  }
+
   private static record YearInfo(Flipper flipper, double fieldLength, double fieldWidth) {}
 
-  // TODO: Update and expand this map
   private static final HashMap<Integer, YearInfo> flipperMap =
       new HashMap<Integer, YearInfo>() {
         {
-          put(2020, new YearInfo(Flipper.ROTATIONALLY_MIRRORED, 16.5811, 8.19912));
-          put(2021, new YearInfo(Flipper.ROTATIONALLY_MIRRORED, 16.5811, 8.19912));
-          put(2022, new YearInfo(Flipper.ROTATIONALLY_MIRRORED, 16.5811, 8.19912));
-          put(2023, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.5811, 8.19912));
-          put(2024, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.5811, 8.19912));
+          put(2022, new YearInfo(Flipper.ROTATIONALLY_MIRRORED, 16.4592, 8.2296));
+          put(2023, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.54175, 8.0137));
+          put(2024, new YearInfo(Flipper.VERTICALLY_MIRRORED, 16.54175, 8.211));
         }
       };
 
   private static YearInfo activeYear = flipperMap.get(2024);
+  private static BooleanSupplier onRed = () -> false;
 
   /**
    * Get the flipper that is currently active for flipping coordinates. It's reccomended not to
@@ -130,7 +129,7 @@ public static Flipper getFlipper() {
    * @return If you are on red alliance.
    */
   public static boolean onRed() {
-    return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;
+    return onRed.getAsBoolean();
   }
 
   /**
@@ -139,13 +138,13 @@ public static boolean onRed() {
    * @return If you are on blue alliance.
    */
   public static boolean onBlue() {
-    return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue;
+    return !onRed.getAsBoolean();
   }
 
   /**
    * Set the year to determine the Alliance Coordinate Flipper to use.
    *
-   * @param year The year to set the flipper to. [2020 - 2024]
+   * @param year The year to set the flipper to. [2022 - 2024]
    */
   public static void setYear(int year) {
     if (!flipperMap.containsKey(year)) {
@@ -157,6 +156,16 @@ public static void setYear(int year) {
     activeYear = flipperMap.get(year);
   }
 
+  /**
+   * Set the `onRed` resolver to determine if the robot is on the red alliance.
+   * 
+   * @param onRedResolver The resolver to determine if the robot is on the red alliance.
+   */
+  public static void setOnRed(BooleanSupplier onRedResolver) {
+    // cannot access driverstation in wpimath
+    onRed = onRedResolver;
+  }
+
   /**
    * Flips the X coordinate.
    *
@@ -188,67 +197,27 @@ public static double flipHeading(double heading) {
   }
 
   /**
-   * Flips the translation.
-   *
-   * @param translation The translation to flip.
-   * @return The flipped translation.
-   */
-  public static Translation2d flip(Translation2d translation) {
-    return new Translation2d(flipX(translation.getX()), flipY(translation.getY()));
-  }
-
-  /**
-   * Flips the rotation.
-   *
-   * @param rotation The rotation to flip.
-   * @return The flipped rotation.
-   */
-  public static Rotation2d flip(Rotation2d rotation) {
-    return switch (activeYear.flipper) {
-      case VERTICALLY_MIRRORED -> new Rotation2d(-rotation.getCos(), rotation.getSin());
-      case ROTATIONALLY_MIRRORED -> new Rotation2d(-rotation.getCos(), -rotation.getSin());
-    };
-  }
-
-  /**
-   * Flips the pose.
-   *
-   * @param pose The pose to flip.
-   * @return The flipped pose.
-   */
-  public static Pose2d flip(Pose2d pose) {
-    return new Pose2d(flip(pose.getTranslation()), flip(pose.getRotation()));
-  }
-
-  /**
-   * Flips the translation.
-   *
-   * @param translation The translation to flip.
-   * @return The flipped translation.
-   */
-  public static Translation3d flip(Translation3d translation) {
-    return new Translation3d(
-        flipX(translation.getX()), flipY(translation.getY()), translation.getZ());
-  }
-
-  /**
-   * Flips the rotation.
+   * Flips the {@link Flippable} object.
    *
-   * @param rotation The rotation to flip.
-   * @return The flipped rotation.
+   * @param <T> The type of the object to flip.
+   * @param flippable The object to flip.
+   * @return The flipped object.
    */
-  public static Rotation3d flip(Rotation3d rotation) {
-    return new Rotation3d(
-        rotation.getX(), rotation.getY(), flip(rotation.toRotation2d()).getRadians());
+  public static <T extends Flippable<T>> T flip(Flippable<T> flippable) {
+    return flippable.flip();
   }
 
   /**
-   * Flips the pose.
+   * Flips the {@link Flippable} object if on the red alliance.
    *
-   * @param pose The pose to flip.
-   * @return The flipped pose.
+   * @param <T> The type of the object to flip.
+   * @param flippable The object to flip.
+   * @return The flipped object.
+   * 
+   * @see #onRed() A way of determining if you are on the red alliance.
+   * @see Flippable#flipIfRed() An instance method that does the same thing.
    */
-  public static Pose3d flip(Pose3d pose) {
-    return new Pose3d(flip(pose.getTranslation()), flip(pose.getRotation()));
+  public static <T extends Flippable<T>> T flipIfRed(Flippable<T> flippable) {
+    return flippable.flipIfRed();
   }
 }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -12,6 +12,7 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.proto.Pose2dProto;
 import edu.wpi.first.math.geometry.struct.Pose2dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -24,7 +25,7 @@
 /** Represents a 2D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
+public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable, Flippable<Pose2d> {
   /**
    * A preallocated Pose2d representing the origin.
    *
@@ -348,6 +349,14 @@ public Pose2d interpolate(Pose2d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose2d flip(Flipper flipper) {
+    return new Pose2d(
+        m_translation.flip(flipper),
+        m_rotation.flip(flipper)
+    );
+  }
+
   /** Pose2d protobuf for serialization. */
   public static final Pose2dProto proto = new Pose2dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -12,6 +12,7 @@
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.proto.Pose3dProto;
 import edu.wpi.first.math.geometry.struct.Pose3dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.jni.Pose3dJNI;
 import edu.wpi.first.units.measure.Distance;
@@ -22,7 +23,7 @@
 /** Represents a 3D pose containing translational and rotational elements. */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
+public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable, Flippable<Pose3d> {
   /**
    * A preallocated Pose3d representing the origin.
    *
@@ -371,6 +372,11 @@ public Pose3d interpolate(Pose3d endValue, double t) {
     }
   }
 
+  @Override
+  public Pose3d flip(Flipper flipper) {
+    return new Pose3d(m_translation.flip(flipper), m_rotation.flip(flipper));
+  }
+
   /** Pose3d protobuf for serialization. */
   public static final Pose3dProto proto = new Pose3dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -31,7 +32,7 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation2d
-    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable, Flippable<Rotation2d> {
   /**
    * A preallocated Rotation2d representing no rotation.
    *
@@ -332,6 +333,14 @@ public Rotation2d interpolate(Rotation2d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation2d flip(Flipper flipper) {
+    return switch (flipper) {
+      case VERTICALLY_MIRRORED -> new Rotation2d(-getCos(), getSin());
+      case ROTATIONALLY_MIRRORED -> new Rotation2d(-getCos(), -getSin());
+    };
+  }
+
   /** Rotation2d protobuf for serialization. */
   public static final Rotation2dProto proto = new Rotation2dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -18,6 +18,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Rotation3dProto;
 import edu.wpi.first.math.geometry.struct.Rotation3dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Angle;
@@ -30,7 +31,7 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d
-    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable, Flippable<Rotation3d> {
   /**
    * A preallocated Rotation3d representing no rotation.
    *
@@ -504,6 +505,16 @@ public Rotation3d interpolate(Rotation3d endValue, double t) {
     return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
   }
 
+  @Override
+  public Rotation3d flip(Flipper flipper) {
+      Rotation2d rot2d = toRotation2d().flip(flipper);
+      return new Rotation3d(
+          this.getX(),
+          this.getY(),
+          rot2d.getRadians()
+      );
+  }
+
   /** Rotation3d protobuf for serialization. */
   public static final Rotation3dProto proto = new Rotation3dProto();
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.proto.Translation2dProto;
 import edu.wpi.first.math.geometry.struct.Translation2dStruct;
+import edu.wpi.first.math.geometry.AllianceFlipper.*;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.measure.Distance;
@@ -34,7 +35,7 @@
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation2d
-    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable, Flippable<Translation2d> {
   /**
    * A preallocated Translation2d representing the origin.
    *
@@ -312,6 +313,11 @@ public Translation2d interpolate(Translation2d endValue, double t) {
         MathUtil.interpolate(this.getY(), endValue.getY(), t));
   }
 
+  @Override
+  public Translation2d flip(Flipper flipper) {
+    return new Translation2d(flipper.flipX(m_x), flipper.flipY(m_y));
+  }
+
   /** Translation2d protobuf for serialization. */
   public static final Translation2dProto proto = new Translation2dProto();