Skip to content

Commit

Permalink
ran formatters
Browse files Browse the repository at this point in the history
  • Loading branch information
oh-yes-0-fps committed Oct 1, 2024
1 parent 98d3549 commit a6ea87d
Show file tree
Hide file tree
Showing 7 changed files with 50 additions and 37 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
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
Expand All @@ -13,54 +14,66 @@
* <p>If every vendor used this, the user would be able to specify the year and no matter the year
* the vendor's code is from, the user would be able to flip as expected.
*/
public class AllianceSymmetry {
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 static enum SymmetryStrategy {
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. */
/**
* 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;
}
Expand Down Expand Up @@ -92,28 +105,33 @@ public double flipHeading(double heading) {
}

/** An interface for objects that can be flipped based on the current alliance. */
public static interface Flippable<Self extends Flippable<Self>> {
public interface Flippable<Self extends Flippable<Self>> {
/**
* Flips the object based on the supplied flipper.
* Flips the object based on the supplied {@link SymmetryStrategy} .
*
* @param strategy The type of symmetry to use for flipping.
* @return The flipped object.
*/
public Self flip(SymmetryStrategy strategy);
Self flip(SymmetryStrategy strategy);

/** Flips the object based on the active flipper. */
public default Self flip() {
/**
* Flips the object based on the active flipper.
*
* @return The flipped object.
*/
default Self flip() {
return flip(getFlipper());
}
}

private static record YearInfo(SymmetryStrategy flipper, double fieldLength, double fieldWidth) {}
private record YearInfo(SymmetryStrategy flipper, double fieldLength, double fieldWidth) {}

private static final HashMap<Integer, YearInfo> flipperMap =
new HashMap<Integer, YearInfo>() {
{
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));
}
};
new HashMap<Integer, YearInfo>(
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);

Expand Down
8 changes: 3 additions & 5 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
Expand Down Expand Up @@ -352,10 +353,7 @@ public Pose2d interpolate(Pose2d endValue, double t) {

@Override
public Pose2d flip(SymmetryStrategy strategy) {
return new Pose2d(
m_translation.flip(strategy),
m_rotation.flip(strategy)
);
return new Pose2d(m_translation.flip(strategy), m_rotation.flip(strategy));
}

/** Pose2d protobuf for serialization. */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.jni.Pose3dJNI;
import edu.wpi.first.units.measure.Distance;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.Angle;
Expand Down Expand Up @@ -510,12 +511,8 @@ public Rotation3d interpolate(Rotation3d endValue, double t) {

@Override
public Rotation3d flip(SymmetryStrategy strategy) {
Rotation2d rot2d = toRotation2d().flip(strategy);
return new Rotation3d(
this.getX(),
this.getY(),
rot2d.getRadians()
);
Rotation2d rot2d = toRotation2d().flip(strategy);
return new Rotation3d(this.getX(), this.getY(), rot2d.getRadians());
}

/** Rotation3d protobuf for serialization. */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.units.measure.Distance;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,10 @@
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.geometry.AllianceSymmetry.*;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.units.measure.Distance;
Expand Down Expand Up @@ -312,11 +313,7 @@ public Translation3d interpolate(Translation3d endValue, double t) {

@Override
public Translation3d flip(SymmetryStrategy strategy) {
return new Translation3d(
strategy.flipX(m_x),
strategy.flipY(m_y),
m_z
);
return new Translation3d(strategy.flipX(m_x), strategy.flipY(m_y), m_z);
}

/** Translation3d protobuf for serialization. */
Expand Down

0 comments on commit a6ea87d

Please sign in to comment.