From 5d86f20592118a3e7da3ec0972990c5e604f3ba5 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 22 Sep 2024 22:14:04 -0400 Subject: [PATCH 01/10] added java alliance flipper --- .../wpi/first/wpilibj/AllianceFlipper.java | 250 ++++++++++++++++++ 1 file changed, 250 insertions(+) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java new file mode 100644 index 00000000000..04752929dd1 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java @@ -0,0 +1,250 @@ +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; +import java.util.HashMap; + +/** + * A utility to standardize flipping of coordinate data based on the current alliance across + * different years. + * + *

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. + * + *

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. + * + *

Flipping a custom structure that is based on the field coordiante system looks like + * 


+ * public class CustomStructure {
+ *   public double x, y, vx, vy;
+ *   public CustomStructure( ... ) { ... }
+ * 
+ *   public CustomStructure flip() {
+ *     return switch (AllianceFlipper.getFlipper()) {
+ *       case MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), y, -vx, vy);
+ *       case ROTATE_AROUND -> new CustomStructure(AllianceFlipper.flipX(x), AllianceFlipper.flipY(y), -vx, -vy);
+ *     };
+ *   }
+ * }
+ *
+ */ +public class AllianceFlipper { + /** The flipper to use for flipping coordinates. */ + public static enum Flipper { + /** + * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - + * heading. + */ + MIRRORED { + public double flipX(double x) { + return activeYear.fieldLength - x; + } + + public double flipY(double y) { + return y; + } + + public double flipHeading(double heading) { + return Math.PI - heading; + } + }, + /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI - heading. */ + ROTATE_AROUND { + public double flipX(double x) { + return activeYear.fieldLength - x; + } + + public double flipY(double y) { + return activeYear.fieldWidth - y; + } + + public double flipHeading(double heading) { + return Math.PI - heading; + } + }; + + /** + * Flips the X coordinate. + * + * @param x The X coordinate to flip. + * @return The flipped X coordinate. + */ + public abstract double flipX(double x); + + /** + * Flips the Y coordinate. + * + * @param y The Y coordinate to flip. + * @return The flipped Y coordinate. + */ + public abstract double flipY(double y); + + /** + * Flips the heading. + * + * @param heading The heading to flip. + * @return The flipped heading. + */ + public abstract double flipHeading(double heading); + } + + private static record YearInfo(Flipper flipper, double fieldLength, double fieldWidth) {} + + // TODO: Update and expand this map + private static final HashMap flipperMap = + new HashMap() { + { + put(2020, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); + put(2021, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); + put(2022, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); + put(2023, new YearInfo(Flipper.MIRRORED, 16.5811, 8.19912)); + put(2024, new YearInfo(Flipper.MIRRORED, 16.5811, 8.19912)); + } + }; + + private static YearInfo activeYear = flipperMap.get(2024); + + /** + * Get the flipper that is currently active for flipping coordinates. It's reccomended not to + * store this locally as the flipper may change. + * + * @return The active flipper. + */ + public static Flipper getFlipper() { + return activeYear.flipper; + } + + /** + * Returns if you are on red alliance, if the alliance is unknown it will return false. + * + * @return If you are on red alliance. + */ + public static boolean onRed() { + return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + } + + /** + * Returns if you are on blue alliance, if the alliance is unknown it will return true. + * + * @return If you are on blue alliance. + */ + public static boolean onBlue() { + return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue; + } + + /** + * Set the year to determine the Alliance Coordinate Flipper to use. + * + * @param year The year to set the flipper to. [2020 - 2024] + */ + public static void setYear(int year) { + if (!flipperMap.containsKey(year)) { + // Throw an exception instead of just reporting an error + // because not flipping correctly during an auto routine + // could cause a robot to damage itself or others. + throw new IllegalArgumentException("Year " + year + " is not supported."); + } + activeYear = flipperMap.get(year); + } + + /** + * Flips the X coordinate. + * + * @param x The X coordinate to flip. + * @return The flipped X coordinate. + */ + public static double flipX(double x) { + return activeYear.flipper.flipX(x); + } + + /** + * Flips the Y coordinate. + * + * @param y The Y coordinate to flip. + * @return The flipped Y coordinate. + */ + public static double flipY(double y) { + return activeYear.flipper.flipY(y); + } + + /** + * Flips the heading. + * + * @param heading The heading to flip. + * @return The flipped heading. + */ + public static double flipHeading(double heading) { + return activeYear.flipper.flipHeading(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 MIRRORED -> new Rotation2d(-rotation.getCos(), rotation.getSin()); + case ROTATE_AROUND -> 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. + * + * @param rotation The rotation to flip. + * @return The flipped rotation. + */ + public static Rotation3d flip(Rotation3d rotation) { + return new Rotation3d( + rotation.getX(), rotation.getY(), flip(rotation.toRotation2d()).getRadians()); + } + + /** + * Flips the pose. + * + * @param pose The pose to flip. + * @return The flipped pose. + */ + public static Pose3d flip(Pose3d pose) { + return new Pose3d(flip(pose.getTranslation()), flip(pose.getRotation())); + } +} From 6ce3c38d5d752f5f31e1d6e6043d82318f72dcb4 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Sun, 22 Sep 2024 22:18:50 -0400 Subject: [PATCH 02/10] fixed variant names in example --- .../wpi/first/wpilibj/AllianceFlipper.java | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java index 04752929dd1..89102dc56d5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java @@ -27,21 +27,25 @@ * * public CustomStructure flip() { * return switch (AllianceFlipper.getFlipper()) { - * case MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), y, -vx, vy); - * case ROTATE_AROUND -> new CustomStructure(AllianceFlipper.flipX(x), AllianceFlipper.flipY(y), -vx, -vy); + * case VERTICALLY_MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), y, -vx, vy); + * case ROTATIONALLY_MIRRORED -> new CustomStructure(AllianceFlipper.flipX(x), AllianceFlipper.flipY(y), -vx, -vy); * }; * } * } * */ public class AllianceFlipper { + private AllianceFlipper() { + throw new UnsupportedOperationException("This is a utility class!"); + } + /** The flipper to use for flipping coordinates. */ public static enum Flipper { /** * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - * heading. */ - MIRRORED { + VERTICALLY_MIRRORED { public double flipX(double x) { return activeYear.fieldLength - x; } @@ -55,7 +59,7 @@ public double flipHeading(double heading) { } }, /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI - heading. */ - ROTATE_AROUND { + ROTATIONALLY_MIRRORED { public double flipX(double x) { return activeYear.fieldLength - x; } @@ -100,11 +104,11 @@ private static record YearInfo(Flipper flipper, double fieldLength, double field private static final HashMap flipperMap = new HashMap() { { - put(2020, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); - put(2021, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); - put(2022, new YearInfo(Flipper.ROTATE_AROUND, 16.5811, 8.19912)); - put(2023, new YearInfo(Flipper.MIRRORED, 16.5811, 8.19912)); - put(2024, new YearInfo(Flipper.MIRRORED, 16.5811, 8.19912)); + 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)); } }; @@ -201,8 +205,8 @@ public static Translation2d flip(Translation2d translation) { */ public static Rotation2d flip(Rotation2d rotation) { return switch (activeYear.flipper) { - case MIRRORED -> new Rotation2d(-rotation.getCos(), rotation.getSin()); - case ROTATE_AROUND -> new Rotation2d(-rotation.getCos(), -rotation.getSin()); + case VERTICALLY_MIRRORED -> new Rotation2d(-rotation.getCos(), rotation.getSin()); + case ROTATIONALLY_MIRRORED -> new Rotation2d(-rotation.getCos(), -rotation.getSin()); }; } From f5afe29276b3f5e4e1472f7e18942a509df920c5 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Mon, 23 Sep 2024 18:23:32 -0400 Subject: [PATCH 03/10] moved it to wpimath and added interface --- .../edu/wpi/first/wpilibj/DriverStation.java | 8 + .../first/math/geometry}/AllianceFlipper.java | 151 +++++++----------- .../edu/wpi/first/math/geometry/Pose2d.java | 11 +- .../edu/wpi/first/math/geometry/Pose3d.java | 8 +- .../wpi/first/math/geometry/Rotation2d.java | 11 +- .../wpi/first/math/geometry/Rotation3d.java | 13 +- .../first/math/geometry/Translation2d.java | 8 +- .../first/math/geometry/Translation3d.java | 12 +- 8 files changed, 125 insertions(+), 97 deletions(-) rename {wpilibj/src/main/java/edu/wpi/first/wpilibj => wpimath/src/main/java/edu/wpi/first/math/geometry}/AllianceFlipper.java (56%) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 1e51b5b4c9f..6740c70e7ea 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/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; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java similarity index 56% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java rename to wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java index 89102dc56d5..7e026a3c2fd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AllianceFlipper.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java @@ -1,13 +1,7 @@ -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 @@ -15,24 +9,6 @@ * *

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. - * - *

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. - * - *

Flipping a custom structure that is based on the field coordiante system looks like - * 


- * 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);
- *     };
- *   }
- * }
- *
*/ 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> { + /** + * 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 flipperMap = new HashMap() { { - 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 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 flip(Flippable 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 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 flipIfRed(Flippable flippable) { + return flippable.flipIfRed(); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 89c404fddbd..cb9feb963e9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/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, ProtobufSerializable, StructSerializable { +public class Pose2d implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * 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(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index ab744a1408a..a4bf68de173 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/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, ProtobufSerializable, StructSerializable { +public class Pose3d implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * 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(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 3eeac17a8f8..ad368e9f57d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/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, ProtobufSerializable, StructSerializable { + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * 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(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 123badb6ca0..347fc1a1fea 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/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, ProtobufSerializable, StructSerializable { + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * 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(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index e3857eb14e6..f65d2311c9d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/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, ProtobufSerializable, StructSerializable { + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * 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(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 5ea1ab2c02b..8aee4d86969 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; +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.Distance; @@ -32,7 +33,7 @@ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation3d - implements Interpolatable, ProtobufSerializable, StructSerializable { + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * A preallocated Translation3d representing the origin. * @@ -306,6 +307,15 @@ public Translation3d interpolate(Translation3d endValue, double t) { MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); } + @Override + public Translation3d flip(Flipper flipper) { + return new Translation3d( + flipper.flipX(m_x), + flipper.flipY(m_y), + m_z + ); + } + /** Translation3d protobuf for serialization. */ public static final Translation3dProto proto = new Translation3dProto(); From 8adcf63e3bb39736e963b67e9f7087a71fc669df Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 24 Sep 2024 13:34:34 -0400 Subject: [PATCH 04/10] Removed alliance checking code from flipper --- .../edu/wpi/first/wpilibj/DriverStation.java | 8 --- .../first/math/geometry/AllianceFlipper.java | 52 ------------------- 2 files changed, 60 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 6740c70e7ea..1e51b5b4c9f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -9,7 +9,6 @@ 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; @@ -30,13 +29,6 @@ /** 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; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java index 7e026a3c2fd..279a0eef5a7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java @@ -1,7 +1,6 @@ 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 @@ -89,14 +88,6 @@ public static interface Flippable> { 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) {} @@ -111,7 +102,6 @@ private static record YearInfo(Flipper flipper, double fieldLength, double field }; 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 @@ -123,24 +113,6 @@ public static Flipper getFlipper() { return activeYear.flipper; } - /** - * Returns if you are on red alliance, if the alliance is unknown it will return false. - * - * @return If you are on red alliance. - */ - public static boolean onRed() { - return onRed.getAsBoolean(); - } - - /** - * Returns if you are on blue alliance, if the alliance is unknown it will return true. - * - * @return If you are on blue alliance. - */ - public static boolean onBlue() { - return !onRed.getAsBoolean(); - } - /** * Set the year to determine the Alliance Coordinate Flipper to use. * @@ -156,16 +128,6 @@ 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. * @@ -206,18 +168,4 @@ public static double flipHeading(double heading) { public static > T flip(Flippable flippable) { return flippable.flip(); } - - /** - * Flips the {@link Flippable} object if on the red alliance. - * - * @param 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 > T flipIfRed(Flippable flippable) { - return flippable.flipIfRed(); - } } From 448d7ace211aeca19b5d88f26ae7cfe98a214430 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 24 Sep 2024 18:32:34 +0000 Subject: [PATCH 05/10] Formatting fixes --- .../wpi/first/math/geometry/AllianceFlipper.java | 16 +++++++--------- .../java/edu/wpi/first/math/geometry/Pose2d.java | 10 ++++------ .../java/edu/wpi/first/math/geometry/Pose3d.java | 5 +++-- .../edu/wpi/first/math/geometry/Rotation2d.java | 7 +++++-- .../edu/wpi/first/math/geometry/Rotation3d.java | 15 +++++++-------- .../wpi/first/math/geometry/Translation2d.java | 7 +++++-- .../wpi/first/math/geometry/Translation3d.java | 13 ++++++------- 7 files changed, 37 insertions(+), 36 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java index 279a0eef5a7..9f73a17b996 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java @@ -1,3 +1,7 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package edu.wpi.first.math.geometry; import java.util.HashMap; @@ -73,18 +77,12 @@ public double flipHeading(double heading) { public abstract double flipHeading(double heading); } - /** - * An interface for objects that can be flipped based on the current alliance. - */ + /** An interface for objects that can be flipped based on the current alliance. */ public static interface Flippable> { - /** - * Flips the object based on the supplied flipper. - */ + /** Flips the object based on the supplied flipper. */ public Self flip(Flipper flipper); - /** - * Flips the object based on the active flipper. - */ + /** Flips the object based on the active flipper. */ public default Self flip() { return flip(getFlipper()); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index cb9feb963e9..3f77baf272a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -10,9 +10,9 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.AllianceFlipper.*; 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; @@ -25,7 +25,8 @@ /** Represents a 2D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) -public class Pose2d implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { +public class Pose2d + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * A preallocated Pose2d representing the origin. * @@ -351,10 +352,7 @@ public Pose2d interpolate(Pose2d endValue, double t) { @Override public Pose2d flip(Flipper flipper) { - return new Pose2d( - m_translation.flip(flipper), - m_rotation.flip(flipper) - ); + return new Pose2d(m_translation.flip(flipper), m_rotation.flip(flipper)); } /** Pose2d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index a4bf68de173..f42b32db0f2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -10,9 +10,9 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.AllianceFlipper.*; 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; @@ -23,7 +23,8 @@ /** Represents a 3D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) -public class Pose3d implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { +public class Pose3d + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * A preallocated Pose3d representing the origin. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index ad368e9f57d..df9b58689bb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -12,9 +12,9 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.AllianceFlipper.*; 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; @@ -32,7 +32,10 @@ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d - implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { + implements Interpolatable, + ProtobufSerializable, + StructSerializable, + Flippable { /** * A preallocated Rotation2d representing no rotation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 347fc1a1fea..da9a1c0918d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -16,9 +16,9 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceFlipper.*; 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; @@ -31,7 +31,10 @@ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d - implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { + implements Interpolatable, + ProtobufSerializable, + StructSerializable, + Flippable { /** * A preallocated Rotation3d representing no rotation. * @@ -507,12 +510,8 @@ public Rotation3d interpolate(Rotation3d endValue, double t) { @Override public Rotation3d flip(Flipper flipper) { - Rotation2d rot2d = toRotation2d().flip(flipper); - return new Rotation3d( - this.getX(), - this.getY(), - rot2d.getRadians() - ); + Rotation2d rot2d = toRotation2d().flip(flipper); + return new Rotation3d(this.getX(), this.getY(), rot2d.getRadians()); } /** Rotation3d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index f65d2311c9d..26cf64c7e4e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -13,9 +13,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceFlipper.*; 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; @@ -35,7 +35,10 @@ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation2d - implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { + implements Interpolatable, + ProtobufSerializable, + StructSerializable, + Flippable { /** * A preallocated Translation2d representing the origin. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 8aee4d86969..ab907fbef88 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -13,9 +13,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceFlipper.*; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; -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.Distance; @@ -33,7 +33,10 @@ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation3d - implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { + implements Interpolatable, + ProtobufSerializable, + StructSerializable, + Flippable { /** * A preallocated Translation3d representing the origin. * @@ -309,11 +312,7 @@ public Translation3d interpolate(Translation3d endValue, double t) { @Override public Translation3d flip(Flipper flipper) { - return new Translation3d( - flipper.flipX(m_x), - flipper.flipY(m_y), - m_z - ); + return new Translation3d(flipper.flipX(m_x), flipper.flipY(m_y), m_z); } /** Translation3d protobuf for serialization. */ From 4ff0c7f5734a01b9e9fae2106fc52b9231cde818 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 24 Sep 2024 16:30:56 -0400 Subject: [PATCH 06/10] updated naming --- ...ianceFlipper.java => AllianceSymetry.java} | 38 +++++++++++++------ .../edu/wpi/first/math/geometry/Pose2d.java | 8 ++-- .../edu/wpi/first/math/geometry/Pose3d.java | 6 +-- .../wpi/first/math/geometry/Rotation2d.java | 11 +++--- .../wpi/first/math/geometry/Rotation3d.java | 6 +-- .../first/math/geometry/Translation2d.java | 6 +-- .../first/math/geometry/Translation3d.java | 8 ++-- 7 files changed, 49 insertions(+), 34 deletions(-) rename wpimath/src/main/java/edu/wpi/first/math/geometry/{AllianceFlipper.java => AllianceSymetry.java} (79%) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java similarity index 79% rename from wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java rename to wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java index 279a0eef5a7..ea16a1a2d25 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceFlipper.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java @@ -9,18 +9,18 @@ *

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. */ -public class AllianceFlipper { - private AllianceFlipper() { +public class AllianceSymetry { + private AllianceSymetry() { throw new UnsupportedOperationException("This is a utility class!"); } - /** The flipper to use for flipping coordinates. */ - public static enum Flipper { + /** The strategy to use for flipping coordinates over axis of symetry. */ + public static enum SymetryStrategy { /** * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - * heading. */ - VERTICALLY_MIRRORED { + VERTICAL { public double flipX(double x) { return activeYear.fieldLength - x; } @@ -34,7 +34,7 @@ public double flipHeading(double heading) { } }, /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI - heading. */ - ROTATIONALLY_MIRRORED { + ROTATIONAL { public double flipX(double x) { return activeYear.fieldLength - x; } @@ -43,6 +43,20 @@ public double flipY(double y) { return activeYear.fieldWidth - y; } + public double flipHeading(double heading) { + return Math.PI - heading; + } + }, + /** Leaves the X coordinate unchanged, Y becomes fieldWidth - y, and heading becomes PI - heading. */ + HORIZONTAL { + public double flipX(double x) { + return x; + } + + public double flipY(double y) { + return activeYear.fieldWidth - y; + } + public double flipHeading(double heading) { return Math.PI - heading; } @@ -80,7 +94,7 @@ public static interface Flippable> { /** * Flips the object based on the supplied flipper. */ - public Self flip(Flipper flipper); + public Self flip(SymetryStrategy strategy); /** * Flips the object based on the active flipper. @@ -90,14 +104,14 @@ public default Self flip() { } } - private static record YearInfo(Flipper flipper, double fieldLength, double fieldWidth) {} + private static record YearInfo(SymetryStrategy flipper, double fieldLength, double fieldWidth) {} private static final HashMap flipperMap = new HashMap() { { - 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)); + put(2022, new YearInfo(SymetryStrategy.ROTATIONAL, 16.4592, 8.2296)); + put(2023, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.0137)); + put(2024, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.211)); } }; @@ -109,7 +123,7 @@ private static record YearInfo(Flipper flipper, double fieldLength, double field * * @return The active flipper. */ - public static Flipper getFlipper() { + public static SymetryStrategy getFlipper() { return activeYear.flipper; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index cb9feb963e9..5a70b8b0496 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -12,7 +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.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -350,10 +350,10 @@ public Pose2d interpolate(Pose2d endValue, double t) { } @Override - public Pose2d flip(Flipper flipper) { + public Pose2d flip(SymetryStrategy strategy) { return new Pose2d( - m_translation.flip(flipper), - m_rotation.flip(flipper) + m_translation.flip(strategy), + m_rotation.flip(strategy) ); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index a4bf68de173..31280f266b8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -12,7 +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.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; import edu.wpi.first.units.measure.Distance; @@ -373,8 +373,8 @@ public Pose3d interpolate(Pose3d endValue, double t) { } @Override - public Pose3d flip(Flipper flipper) { - return new Pose3d(m_translation.flip(flipper), m_rotation.flip(flipper)); + public Pose3d flip(SymetryStrategy strategy) { + return new Pose3d(m_translation.flip(strategy), m_rotation.flip(strategy)); } /** Pose3d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index ad368e9f57d..3b390db0f7c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -14,7 +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.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; @@ -334,10 +334,11 @@ public Rotation2d interpolate(Rotation2d endValue, double t) { } @Override - public Rotation2d flip(Flipper flipper) { - return switch (flipper) { - case VERTICALLY_MIRRORED -> new Rotation2d(-getCos(), getSin()); - case ROTATIONALLY_MIRRORED -> new Rotation2d(-getCos(), -getSin()); + public Rotation2d flip(SymetryStrategy strategy) { + return switch (strategy) { + case VERTICAL -> new Rotation2d(-getCos(), getSin()); + case ROTATIONAL -> new Rotation2d(-getCos(), -getSin()); + case HORIZONTAL -> new Rotation2d(getCos(), -getSin()); }; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 347fc1a1fea..7886cde7dcb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -18,7 +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.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Angle; @@ -506,8 +506,8 @@ public Rotation3d interpolate(Rotation3d endValue, double t) { } @Override - public Rotation3d flip(Flipper flipper) { - Rotation2d rot2d = toRotation2d().flip(flipper); + public Rotation3d flip(SymetryStrategy strategy) { + Rotation2d rot2d = toRotation2d().flip(strategy); return new Rotation3d( this.getX(), this.getY(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index f65d2311c9d..621503ac179 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -15,7 +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.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.units.measure.Distance; @@ -314,8 +314,8 @@ public Translation2d interpolate(Translation2d endValue, double t) { } @Override - public Translation2d flip(Flipper flipper) { - return new Translation2d(flipper.flipX(m_x), flipper.flipY(m_y)); + public Translation2d flip(SymetryStrategy strategy) { + return new Translation2d(strategy.flipX(m_x), strategy.flipY(m_y)); } /** Translation2d protobuf for serialization. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 8aee4d86969..c3c849b7ea5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; -import edu.wpi.first.math.geometry.AllianceFlipper.*; +import edu.wpi.first.math.geometry.AllianceSymetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; @@ -308,10 +308,10 @@ public Translation3d interpolate(Translation3d endValue, double t) { } @Override - public Translation3d flip(Flipper flipper) { + public Translation3d flip(SymetryStrategy strategy) { return new Translation3d( - flipper.flipX(m_x), - flipper.flipY(m_y), + strategy.flipX(m_x), + strategy.flipY(m_y), m_z ); } From 12db59c204aaad93e4c21ca653693f2852c98b48 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Tue, 24 Sep 2024 16:51:01 -0400 Subject: [PATCH 07/10] ran fmt --- .../java/edu/wpi/first/math/geometry/AllianceSymetry.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java index ea16a1a2d25..1d405a164c9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java @@ -1,3 +1,7 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package edu.wpi.first.math.geometry; import java.util.HashMap; From e611e40fd3a70e284c76fd75536253c89a6eb301 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps <55303619+oh-yes-0-fps@users.noreply.github.com> Date: Wed, 25 Sep 2024 14:42:27 -0400 Subject: [PATCH 08/10] Update wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java Co-authored-by: Tim Winters --- .../main/java/edu/wpi/first/math/geometry/AllianceSymetry.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java index 10a8aeb09de..7d8c89d8952 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java @@ -13,7 +13,7 @@ *

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. */ -public class AllianceSymetry { +public class AllianceSymmetry { private AllianceSymetry() { throw new UnsupportedOperationException("This is a utility class!"); } From 25b2e1780caa19be0f862ab0f4ff74a0a95f1b74 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 25 Sep 2024 14:46:09 -0400 Subject: [PATCH 09/10] fixed typo --- .../geometry/{AllianceSymetry.java => AllianceSymmetry.java} | 4 ++-- wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java | 2 +- wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java | 2 +- .../src/main/java/edu/wpi/first/math/geometry/Rotation2d.java | 2 +- .../src/main/java/edu/wpi/first/math/geometry/Rotation3d.java | 2 +- .../main/java/edu/wpi/first/math/geometry/Translation2d.java | 2 +- .../main/java/edu/wpi/first/math/geometry/Translation3d.java | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) rename wpimath/src/main/java/edu/wpi/first/math/geometry/{AllianceSymetry.java => AllianceSymmetry.java} (98%) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java similarity index 98% rename from wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java rename to wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java index 10a8aeb09de..d1164ee85af 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymetry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java @@ -13,8 +13,8 @@ *

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. */ -public class AllianceSymetry { - private AllianceSymetry() { +public class AllianceSymmetry { + private AllianceSymmetry() { throw new UnsupportedOperationException("This is a utility class!"); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index ed1cbf2edc8..9c4c01ac039 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -12,7 +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.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index cd2c8b048ac..9934469881b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -12,7 +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.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; import edu.wpi.first.units.measure.Distance; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index c75c7d7edaa..2d1f849d664 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -14,7 +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.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 7b93db0fa2c..546974f8492 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -18,7 +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.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Angle; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index fe1544bb247..364089740f2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -15,7 +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.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.units.measure.Distance; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 1a29af5fb08..5267fe2b7fc 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -15,7 +15,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; -import edu.wpi.first.math.geometry.AllianceSymetry.*; +import edu.wpi.first.math.geometry.AllianceSymmetry.*; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; From 98d3549031766324f3e0b502fb5277ff6ec5e7fd Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Wed, 25 Sep 2024 17:19:33 -0400 Subject: [PATCH 10/10] Fixed typos --- .../first/math/geometry/AllianceSymmetry.java | 16 ++++++++-------- .../java/edu/wpi/first/math/geometry/Pose2d.java | 2 +- .../java/edu/wpi/first/math/geometry/Pose3d.java | 2 +- .../edu/wpi/first/math/geometry/Rotation2d.java | 2 +- .../edu/wpi/first/math/geometry/Rotation3d.java | 2 +- .../wpi/first/math/geometry/Translation2d.java | 2 +- .../wpi/first/math/geometry/Translation3d.java | 2 +- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java index d1164ee85af..95a342bf4cd 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java @@ -18,8 +18,8 @@ private AllianceSymmetry() { throw new UnsupportedOperationException("This is a utility class!"); } - /** The strategy to use for flipping coordinates over axis of symetry. */ - public static enum SymetryStrategy { + /** The strategy to use for flipping coordinates over axis of symmetry. */ + public static enum SymmetryStrategy { /** * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - * heading. @@ -96,7 +96,7 @@ public static interface Flippable> { /** * Flips the object based on the supplied flipper. */ - public Self flip(SymetryStrategy strategy); + public Self flip(SymmetryStrategy strategy); /** Flips the object based on the active flipper. */ public default Self flip() { @@ -104,14 +104,14 @@ public default Self flip() { } } - private static record YearInfo(SymetryStrategy flipper, double fieldLength, double fieldWidth) {} + private static record YearInfo(SymmetryStrategy flipper, double fieldLength, double fieldWidth) {} private static final HashMap flipperMap = new HashMap() { { - put(2022, new YearInfo(SymetryStrategy.ROTATIONAL, 16.4592, 8.2296)); - put(2023, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.0137)); - put(2024, new YearInfo(SymetryStrategy.VERTICAL, 16.54175, 8.211)); + put(2022, new YearInfo(SymmetryStrategy.ROTATIONAL, 16.4592, 8.2296)); + put(2023, new YearInfo(SymmetryStrategy.VERTICAL, 16.54175, 8.0137)); + put(2024, new YearInfo(SymmetryStrategy.VERTICAL, 16.54175, 8.211)); } }; @@ -123,7 +123,7 @@ private static record YearInfo(SymetryStrategy flipper, double fieldLength, doub * * @return The active flipper. */ - public static SymetryStrategy getFlipper() { + public static SymmetryStrategy getFlipper() { return activeYear.flipper; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 9c4c01ac039..b97397c1ed5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -351,7 +351,7 @@ public Pose2d interpolate(Pose2d endValue, double t) { } @Override - public Pose2d flip(SymetryStrategy strategy) { + public Pose2d flip(SymmetryStrategy strategy) { return new Pose2d( m_translation.flip(strategy), m_rotation.flip(strategy) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 9934469881b..fd9c4bdc15c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -374,7 +374,7 @@ public Pose3d interpolate(Pose3d endValue, double t) { } @Override - public Pose3d flip(SymetryStrategy strategy) { + public Pose3d flip(SymmetryStrategy strategy) { return new Pose3d(m_translation.flip(strategy), m_rotation.flip(strategy)); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 2d1f849d664..3e13477da76 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -337,7 +337,7 @@ public Rotation2d interpolate(Rotation2d endValue, double t) { } @Override - public Rotation2d flip(SymetryStrategy strategy) { + public Rotation2d flip(SymmetryStrategy strategy) { return switch (strategy) { case VERTICAL -> new Rotation2d(-getCos(), getSin()); case ROTATIONAL -> new Rotation2d(-getCos(), -getSin()); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 546974f8492..85aa1df3b57 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -509,7 +509,7 @@ public Rotation3d interpolate(Rotation3d endValue, double t) { } @Override - public Rotation3d flip(SymetryStrategy strategy) { + public Rotation3d flip(SymmetryStrategy strategy) { Rotation2d rot2d = toRotation2d().flip(strategy); return new Rotation3d( this.getX(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 364089740f2..75f630459b5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -317,7 +317,7 @@ public Translation2d interpolate(Translation2d endValue, double t) { } @Override - public Translation2d flip(SymetryStrategy strategy) { + public Translation2d flip(SymmetryStrategy strategy) { return new Translation2d(strategy.flipX(m_x), strategy.flipY(m_y)); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 5267fe2b7fc..5243329969e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -311,7 +311,7 @@ public Translation3d interpolate(Translation3d endValue, double t) { } @Override - public Translation3d flip(SymetryStrategy strategy) { + public Translation3d flip(SymmetryStrategy strategy) { return new Translation3d( strategy.flipX(m_x), strategy.flipY(m_y),