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 new file mode 100644 index 00000000000..e9053f465b1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/AllianceSymmetry.java @@ -0,0 +1,207 @@ +// 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; +import java.util.Map; + +/** + * 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. + */ +public final class AllianceSymmetry { + private AllianceSymmetry() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** The strategy to use for flipping coordinates over axis of symmetry. */ + public enum SymmetryStrategy { + /** + * X becomes fieldLength - x, leaves the y coordinate unchanged, and heading becomes PI - + * heading. + */ + VERTICAL { + @Override + public double flipX(double x) { + return activeYear.fieldLength - x; + } + + @Override + public double flipY(double y) { + return y; + } + + @Override + public double flipHeading(double heading) { + return Math.PI - heading; + } + }, + /** X becomes fieldLength - x, Y becomes fieldWidth - y, and heading becomes PI - heading. */ + ROTATIONAL { + @Override + public double flipX(double x) { + return activeYear.fieldLength - x; + } + + @Override + public double flipY(double y) { + return activeYear.fieldWidth - y; + } + + @Override + public double flipHeading(double heading) { + return Math.PI - heading; + } + }, + /** + * Leaves the X coordinate unchanged, Y becomes fieldWidth - y, and heading becomes PI - + * heading. + */ + HORIZONTAL { + @Override + public double flipX(double x) { + return x; + } + + @Override + public double flipY(double y) { + return activeYear.fieldWidth - y; + } + + @Override + 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); + } + + /** + * An interface for objects that can be flipped based on the current alliance. + * + * @param The type of the object that is flippable. + */ + public interface Flippable> { + /** + * Flips the object based on the supplied {@link SymmetryStrategy} . + * + * @param strategy The type of symmetry to use for flipping. + * @return The flipped object. + */ + Self flip(SymmetryStrategy strategy); + + /** + * Flips the object based on the active flipper. + * + * @return The flipped object. + */ + default Self flip() { + return flip(getFlipper()); + } + } + + private record YearInfo(SymmetryStrategy flipper, double fieldLength, double fieldWidth) {} + + private static final HashMap flipperMap = + new HashMap( + Map.of( + 2022, new YearInfo(SymmetryStrategy.ROTATIONAL, 16.4592, 8.2296), + 2023, new YearInfo(SymmetryStrategy.VERTICAL, 16.54175, 8.0137), + 2024, new YearInfo(SymmetryStrategy.VERTICAL, 16.54175, 8.211))); + + 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 SymmetryStrategy getFlipper() { + return activeYear.flipper; + } + + /** + * Set the year to determine the Alliance Coordinate Flipper to use. + * + * @param year The year to set the flipper to. [2022 - 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 {@link Flippable} object. + * + * @param The type of the object to flip. + * @param flippable The object to flip. + * @return The flipped object. + */ + public static > T flip(Flippable flippable) { + return flippable.flip(); + } +} 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..3978065c9d0 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,6 +10,8 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Pose2dProto; import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -24,7 +26,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 { +public class Pose2d + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * A preallocated Pose2d representing the origin. * @@ -348,6 +351,11 @@ public Pose2d interpolate(Pose2d endValue, double t) { } } + @Override + public Pose2d flip(SymmetryStrategy strategy) { + return new Pose2d(m_translation.flip(strategy), m_rotation.flip(strategy)); + } + /** 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..fcee8129d1d 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,6 +10,8 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Pose3dProto; import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -22,7 +24,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 { +public class Pose3d + implements Interpolatable, ProtobufSerializable, StructSerializable, Flippable { /** * A preallocated Pose3d representing the origin. * @@ -371,6 +374,11 @@ public Pose3d interpolate(Pose3d endValue, double t) { } } + @Override + public Pose3d flip(SymmetryStrategy strategy) { + return new Pose3d(m_translation.flip(strategy), m_rotation.flip(strategy)); + } + /** 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..9a5b9c45ff0 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,6 +12,8 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Rotation2dProto; import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -31,7 +33,10 @@ @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 +337,15 @@ public Rotation2d interpolate(Rotation2d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } + @Override + public Rotation2d flip(SymmetryStrategy strategy) { + return switch (strategy) { + case VERTICAL -> new Rotation2d(-getCos(), getSin()); + case ROTATIONAL -> new Rotation2d(-getCos(), -getSin()); + case HORIZONTAL -> 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..6689892bbc8 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,6 +16,8 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Rotation3dProto; import edu.wpi.first.math.geometry.struct.Rotation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -30,7 +32,10 @@ @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 +509,12 @@ public Rotation3d interpolate(Rotation3d endValue, double t) { return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1))); } + @Override + public Rotation3d flip(SymmetryStrategy strategy) { + Rotation2d rot2d = toRotation2d().flip(strategy); + 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..0c9469614f8 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,6 +13,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Translation2dProto; import edu.wpi.first.math.geometry.struct.Translation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -34,7 +36,10 @@ @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 +317,11 @@ public Translation2d interpolate(Translation2d endValue, double t) { MathUtil.interpolate(this.getY(), endValue.getY(), t)); } + @Override + public Translation2d flip(SymmetryStrategy strategy) { + return new Translation2d(strategy.flipX(m_x), strategy.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..982745242b4 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,6 +13,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.AllianceSymmetry.Flippable; +import edu.wpi.first.math.geometry.AllianceSymmetry.SymmetryStrategy; import edu.wpi.first.math.geometry.proto.Translation3dProto; import edu.wpi.first.math.geometry.struct.Translation3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; @@ -32,7 +34,10 @@ @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 +311,11 @@ public Translation3d interpolate(Translation3d endValue, double t) { MathUtil.interpolate(this.getZ(), endValue.getZ(), t)); } + @Override + public Translation3d flip(SymmetryStrategy strategy) { + return new Translation3d(strategy.flipX(m_x), strategy.flipY(m_y), m_z); + } + /** Translation3d protobuf for serialization. */ public static final Translation3dProto proto = new Translation3dProto();