Flippable

src/main/java/org/trigon/utilities/mirrorable/Mirrorable.java → src/main/java/org/trigon/utilities/flippable/Flippable.java

@@ -1,6 +1,5 @@
-package org.trigon.utilities.mirrorable;
+package org.trigon.utilities.flippable;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -10,22 +9,22 @@
 import java.util.Optional;
 
 /**
- * A class that allows for objects to be mirrored across the center of the field when the robot is on the red alliance.
- * This is useful for placing field elements and other objects that are mirrored across the field, or for mirroring the target heading to face a field element.
+ * A class that allows for objects to be flipped across the center of the field when the robot is on the red alliance.
+ * This is useful for placing field elements and other objects that are flipped across the field, or for flipping the target heading to face a field element.
+ * This either represents a field that is mirrored vertically over the center of the field, or a field that is rotationally symmetric: the red alliance side is the blue alliance side rotated by 180 degrees.
+ * The code will automatically determine which type of field the robot is on and flip the object accordingly.
  *
- * @param <T> the type of object to mirror
+ * @param <T> the type of object to flip
  */
-public abstract class Mirrorable<T> {
-    protected static final Rotation2d HALF_ROTATION = new Rotation2d(Math.PI);
-    protected static final double FIELD_LENGTH_METERS = 16.54175;
+public abstract class Flippable<T> {
     private static final Timer UPDATE_ALLIANCE_TIMER = new Timer();
     private static boolean IS_RED_ALLIANCE = notCachedIsRedAlliance();
-    protected final T nonMirroredObject, mirroredObject;
+    protected final T nonFlippedObject, flippedObject;
 
-    protected final boolean shouldMirrorWhenRedAlliance;
+    protected final boolean shouldFlipWhenRedAlliance;
 
     /**
-     * Initializes the mirrorable class. This should be called once in RobotContainer.
+     * Initializes the Flippable class. This should be called once in RobotContainer.
      */
     public static void init() {
         UPDATE_ALLIANCE_TIMER.start();
@@ -57,32 +56,32 @@ private static boolean notCachedIsRedAlliance() {
     }
 
     /**
-     * Creates a new mirrorable object.
+     * Creates a new Flippable object.
      *
-     * @param nonMirroredObject           the object when the robot is on the blue alliance, or the non-mirrored object
-     * @param shouldMirrorWhenRedAlliance should the object should be mirrored when the robot is on the red alliance
+     * @param nonFlippedObject          the object when the robot is on the blue alliance; the non-flipped object
+     * @param shouldFlipWhenRedAlliance should the object should be flipped when the robot is on the red alliance
      */
-    protected Mirrorable(T nonMirroredObject, boolean shouldMirrorWhenRedAlliance) {
-        this.nonMirroredObject = nonMirroredObject;
-        this.mirroredObject = mirror(nonMirroredObject);
-        this.shouldMirrorWhenRedAlliance = shouldMirrorWhenRedAlliance;
+    protected Flippable(T nonFlippedObject, boolean shouldFlipWhenRedAlliance) {
+        this.nonFlippedObject = nonFlippedObject;
+        this.flippedObject = flip(nonFlippedObject);
+        this.shouldFlipWhenRedAlliance = shouldFlipWhenRedAlliance;
     }
 
     /**
-     * If the robot is on the red alliance and the object should be mirrored, the mirrored object is returned.
-     * Otherwise, the non-mirrored object is returned.
+     * If the robot is on the red alliance and the object should be flipped, the flipped object is returned.
+     * Otherwise, the non-flipped object is returned.
      *
      * @return the current object
      */
     public T get() {
-        return isRedAlliance() && shouldMirrorWhenRedAlliance ? mirroredObject : nonMirroredObject;
+        return isRedAlliance() && shouldFlipWhenRedAlliance ? flippedObject : nonFlippedObject;
     }
 
     /**
-     * Mirrors the object across the center of the field.
+     * Flips the object across the center of the field.
      *
-     * @param object the object to mirror
-     * @return the mirrored object
+     * @param object the object to flip
+     * @return the flipped object
      */
-    protected abstract T mirror(T object);
+    protected abstract T flip(T object);
 }

src/main/java/org/trigon/utilities/flippable/FlippablePose2d.java

@@ -0,0 +1,58 @@
+package org.trigon.utilities.flippable;
+
+import com.pathplanner.lib.util.FlippingUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/**
+ * A class that represents a {@link Pose2d} that can be flipped when the robot is on the red alliance.
+ */
+public class FlippablePose2d extends Flippable<Pose2d> {
+    /**
+     * Creates a new FlippablePose2d with the given x, y, and rotation.
+     *
+     * @param x                         the x value of the pose
+     * @param y                         the y value of the pose
+     * @param rotation                  the rotation of the pose
+     * @param shouldFlipWhenRedAlliance should the pose be flipped when the robot is on the red alliance
+     */
+    public FlippablePose2d(double x, double y, Rotation2d rotation, boolean shouldFlipWhenRedAlliance) {
+        this(new Pose2d(x, y, rotation), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippablePose2d with the given translation and rotation.
+     *
+     * @param translation2d             the translation of the pose
+     * @param rotationRadians           the rotation of the pose in radians
+     * @param shouldFlipWhenRedAlliance should the pose be flipped when the robot is on the red alliance
+     */
+    public FlippablePose2d(Translation2d translation2d, double rotationRadians, boolean shouldFlipWhenRedAlliance) {
+        this(new Pose2d(translation2d, new Rotation2d(rotationRadians)), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippablePose2d with the given x, y, and rotation.
+     *
+     * @param nonFlippedPose            the pose when the robot is on the blue alliance
+     * @param shouldFlipWhenRedAlliance should the pose be flipped when the robot is on the red alliance
+     */
+    public FlippablePose2d(Pose2d nonFlippedPose, boolean shouldFlipWhenRedAlliance) {
+        super(nonFlippedPose, shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Gets the rotation value of the pose. The pose will be flipped if the robot is on the red alliance and {@link #shouldFlipWhenRedAlliance} is true.
+     *
+     * @return the rotation value of the pose
+     */
+    public FlippableRotation2d getRotation() {
+        return new FlippableRotation2d(nonFlippedObject.getRotation(), shouldFlipWhenRedAlliance);
+    }
+
+    @Override
+    protected Pose2d flip(Pose2d pose) {
+        return FlippingUtil.flipFieldPose(pose);
+    }
+}

src/main/java/org/trigon/utilities/flippable/FlippableRotation2d.java

@@ -0,0 +1,67 @@
+package org.trigon.utilities.flippable;
+
+import com.pathplanner.lib.util.FlippingUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * A class that represents a {@link Rotation2d} that can be flipped when the robot is on the red alliance.
+ */
+public class FlippableRotation2d extends Flippable<Rotation2d> {
+    /**
+     * Constructs and returns a FlippableRotation2d with the given degree value.
+     *
+     * @param degrees                   the value of the angle in degrees
+     * @param shouldFlipWhenRedAlliance should the rotation be flipped when the robot is on the red alliance
+     * @return the rotation object with the desired angle value
+     */
+    public static FlippableRotation2d fromDegrees(double degrees, boolean shouldFlipWhenRedAlliance) {
+        return new FlippableRotation2d(Rotation2d.fromDegrees(degrees), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Constructs and returns a FlippableRotation2d with the given radian value.
+     *
+     * @param radians                   the value of the angle in radians
+     * @param shouldFlipWhenRedAlliance should the rotation be flipped when the robot is on the red alliance
+     * @return the rotation object with the desired angle value
+     */
+    public static FlippableRotation2d fromRadians(double radians, boolean shouldFlipWhenRedAlliance) {
+        return new FlippableRotation2d(Rotation2d.fromRadians(radians), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Constructs a FlippableRotation2d with the given number of rotations.
+     *
+     * @param rotations                 the value of the angle in rotations
+     * @param shouldFlipWhenRedAlliance should the rotation be flipped when the robot is on the red alliance
+     * @return the rotation object with the desired angle value
+     */
+    public static FlippableRotation2d fromRotations(double rotations, boolean shouldFlipWhenRedAlliance) {
+        return new FlippableRotation2d(Rotation2d.fromRotations(rotations), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippableRotation2d with the given rotation value.
+     *
+     * @param radians                   the value of the angle in radians
+     * @param shouldFlipWhenRedAlliance should the rotation be flipped when the robot is on the red alliance
+     */
+    public FlippableRotation2d(double radians, boolean shouldFlipWhenRedAlliance) {
+        this(new Rotation2d(radians), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippableRotation2d with the given Rotation2d.
+     *
+     * @param nonFlippedRotation        the non flipped rotation when the robot is on the blue alliance
+     * @param shouldFlipWhenRedAlliance should the rotation be flipped when the robot is on the red alliance
+     */
+    public FlippableRotation2d(Rotation2d nonFlippedRotation, boolean shouldFlipWhenRedAlliance) {
+        super(nonFlippedRotation, shouldFlipWhenRedAlliance);
+    }
+
+    @Override
+    protected Rotation2d flip(Rotation2d rotation) {
+        return FlippingUtil.flipFieldRotation(rotation);
+    }
+}

src/main/java/org/trigon/utilities/flippable/FlippableTranslation2d.java

@@ -0,0 +1,35 @@
+package org.trigon.utilities.flippable;
+
+import com.pathplanner.lib.util.FlippingUtil;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/**
+ * A class that represents a {@link Translation2d} that can be flipped when the robot is on the red alliance.
+ */
+public class FlippableTranslation2d extends Flippable<Translation2d> {
+    /**
+     * Creates a new FlippableTranslation2d with the given x and y values.
+     *
+     * @param x                         the x value of the translation
+     * @param y                         the y value of the translation
+     * @param shouldFlipWhenRedAlliance should the position be flipped when the robot is on the red alliance
+     */
+    public FlippableTranslation2d(double x, double y, boolean shouldFlipWhenRedAlliance) {
+        this(new Translation2d(x, y), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippableTranslation2d with the given translation.
+     *
+     * @param nonFlippedTranslation     the translation to flip
+     * @param shouldFlipWhenRedAlliance should the position be flipped when the robot is on the red alliance
+     */
+    public FlippableTranslation2d(Translation2d nonFlippedTranslation, boolean shouldFlipWhenRedAlliance) {
+        super(nonFlippedTranslation, shouldFlipWhenRedAlliance);
+    }
+
+    @Override
+    protected Translation2d flip(Translation2d translation) {
+        return FlippingUtil.flipFieldPosition(translation);
+    }
+}

src/main/java/org/trigon/utilities/flippable/FlippableTranslation3d.java

@@ -0,0 +1,46 @@
+package org.trigon.utilities.flippable;
+
+import com.pathplanner.lib.util.FlippingUtil;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+
+/**
+ * A class that represents a {@link Translation3d} that can be flipped when the robot is on the red alliance.
+ * The Z value will have no change.
+ */
+public class FlippableTranslation3d extends Flippable<Translation3d> {
+    /**
+     * Creates a new FlippableTranslation3d with the given x, y, and z values.
+     *
+     * @param x                         the x value of the translation
+     * @param y                         the y value of the translation
+     * @param z                         the z value of the translation
+     * @param shouldFlipWhenRedAlliance should the position be flipped when the robot is on the red alliance
+     */
+    public FlippableTranslation3d(double x, double y, double z, boolean shouldFlipWhenRedAlliance) {
+        this(new Translation3d(x, y, z), shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Creates a new FlippableTranslation3d with the given translation.
+     *
+     * @param nonFlippedTranslation     the translation to flip
+     * @param shouldFlipWhenRedAlliance should the position be flipped when the robot is on the red alliance
+     */
+    public FlippableTranslation3d(Translation3d nonFlippedTranslation, boolean shouldFlipWhenRedAlliance) {
+        super(nonFlippedTranslation, shouldFlipWhenRedAlliance);
+    }
+
+    /**
+     * Flips the given translation. The translation will be flipped if the robot is on the red alliance and {@link #shouldFlipWhenRedAlliance} is true.
+     * The Z value will have no change.
+     *
+     * @param translation the object to flip
+     * @return the flipped translation
+     */
+    @Override
+    protected Translation3d flip(Translation3d translation) {
+        final Translation2d flippedTranslation2d = FlippingUtil.flipFieldPosition(translation.toTranslation2d());
+        return new Translation3d(flippedTranslation2d.getX(), flippedTranslation2d.getY(), translation.getZ());
+    }
+}