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 49cf4c3c7d..b633a45ef3 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 @@ -23,10 +23,6 @@ import java.util.Objects; /** * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). - * - *

The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will - * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the - * rotations as it sweeps past from 360 to 0 on the second time around. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -81,13 +77,11 @@ public class Rotation2d */ public static final Rotation2d k180deg = kPi; - private final double m_value; private final double m_cos; private final double m_sin; /** Constructs a Rotation2d with a default angle of 0 degrees. */ public Rotation2d() { - m_value = 0.0; m_cos = 1.0; m_sin = 0.0; } @@ -99,7 +93,6 @@ public class Rotation2d */ @JsonCreator public Rotation2d(@JsonProperty(required = true, value = "radians") double value) { - m_value = value; m_cos = Math.cos(value); m_sin = Math.sin(value); } @@ -121,7 +114,6 @@ public class Rotation2d MathSharedStore.reportError( "x and y components of Rotation2d are zero\n", Thread.currentThread().getStackTrace()); } - m_value = Math.atan2(m_sin, m_cos); } /** @@ -196,7 +188,7 @@ public class Rotation2d * @return The inverse of the current rotation. */ public Rotation2d unaryMinus() { - return new Rotation2d(-m_value); + return new Rotation2d(m_cos, -m_sin); } /** @@ -206,7 +198,7 @@ public class Rotation2d * @return The new scaled Rotation2d. */ public Rotation2d times(double scalar) { - return new Rotation2d(m_value * scalar); + return new Rotation2d(getRadians() * scalar); } /** @@ -248,25 +240,22 @@ public class Rotation2d } /** - * Returns the radian value of the Rotation2d. + * Returns the radian value of the Rotation2d constrained within [-π, π]. * - * @return The radian value of the Rotation2d. - * @see edu.wpi.first.math.MathUtil#angleModulus(double) to constrain the angle within (-π, π] + * @return The radian value of the Rotation2d constrained within [-π, π]. */ @JsonProperty public double getRadians() { - return m_value; + return Math.atan2(m_sin, m_cos); } /** - * Returns the degree value of the Rotation2d. + * Returns the degree value of the Rotation2d constrained within [-180, 180]. * - * @return The degree value of the Rotation2d. - * @see edu.wpi.first.math.MathUtil#inputModulus(double, double, double) to constrain the angle - * within (-180, 180] + * @return The degree value of the Rotation2d constrained within [-180, 180]. */ public double getDegrees() { - return Math.toDegrees(m_value); + return Math.toDegrees(getRadians()); } /** @@ -275,7 +264,7 @@ public class Rotation2d * @return The number of rotations of the Rotation2d. */ public double getRotations() { - return Units.radiansToRotations(m_value); + return Units.radiansToRotations(getRadians()); } /** @@ -307,7 +296,7 @@ public class Rotation2d @Override public String toString() { - return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value)); + return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees()); } /** @@ -324,7 +313,7 @@ public class Rotation2d @Override public int hashCode() { - return Objects.hash(m_value); + return Objects.hash(getRadians()); } @Override diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index a8899304f2..f94d06216c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -19,11 +19,6 @@ namespace frc { /** * A rotation in a 2D coordinate frame represented by a point on the unit circle * (cosine and sine). - * - * The angle is continuous, that is if a Rotation2d is constructed with 361 - * degrees, it will return 361 degrees. This allows algorithms that wouldn't - * want to see a discontinuity in the rotations as it sweeps past from 360 to 0 - * on the second time around. */ class WPILIB_DLLEXPORT Rotation2d { public: @@ -38,8 +33,7 @@ class WPILIB_DLLEXPORT Rotation2d { * @param value The value of the angle. */ constexpr Rotation2d(units::angle_unit auto value) // NOLINT - : m_value{value}, - m_cos{gcem::cos(value.template convert().value())}, + : m_cos{gcem::cos(value.template convert().value())}, m_sin{gcem::sin(value.template convert().value())} {} /** @@ -63,7 +57,6 @@ class WPILIB_DLLEXPORT Rotation2d { wpi::GetStackTrace(1)); } } - m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; } /** @@ -102,7 +95,7 @@ class WPILIB_DLLEXPORT Rotation2d { * * @return The inverse of the current rotation. */ - constexpr Rotation2d operator-() const { return Rotation2d{-m_value}; } + constexpr Rotation2d operator-() const { return Rotation2d{m_cos, -m_sin}; } /** * Multiplies the current rotation by a scalar. @@ -112,7 +105,7 @@ class WPILIB_DLLEXPORT Rotation2d { * @return The new scaled Rotation2d. */ constexpr Rotation2d operator*(double scalar) const { - return Rotation2d{m_value * scalar}; + return Rotation2d{Radians() * scalar}; } /** @@ -155,20 +148,20 @@ class WPILIB_DLLEXPORT Rotation2d { } /** - * Returns the radian value of the rotation. + * Returns the radian value of the rotation constrained within [-π, π]. * - * @return The radian value of the rotation. - * @see AngleModulus to constrain the angle within (-π, π] + * @return The radian value of the rotation constrained within [-π, π]. */ - constexpr units::radian_t Radians() const { return m_value; } + constexpr units::radian_t Radians() const { + return units::radian_t{gcem::atan2(m_sin, m_cos)}; + } /** - * Returns the degree value of the rotation. + * Returns the degree value of the rotation constrained within [-180, 180]. * - * @return The degree value of the rotation. - * @see InputModulus to constrain the angle within (-180, 180] + * @return The degree value of the rotation constrained within [-180, 180]. */ - constexpr units::degree_t Degrees() const { return m_value; } + constexpr units::degree_t Degrees() const { return Radians(); } /** * Returns the cosine of the rotation. @@ -192,7 +185,6 @@ class WPILIB_DLLEXPORT Rotation2d { constexpr double Tan() const { return Sin() / Cos(); } private: - units::radian_t m_value = 0_rad; double m_cos = 1; double m_sin = 0; }; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index b75182edb7..c91df94a70 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -79,7 +79,7 @@ class Rotation2dTest { var rot = Rotation2d.fromDegrees(10.0); assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon); - assertEquals(410.0, rot.times(41.0).getDegrees(), kEpsilon); + assertEquals(50.0, rot.times(41.0).getDegrees(), kEpsilon); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 546db37314..293d94e554 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -138,7 +138,7 @@ class SwerveDriveKinematicsTest { () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon), - () -> assertEquals(270.0, moduleStates[3].angle.getDegrees(), kEpsilon)); + () -> assertEquals(-90.0, moduleStates[3].angle.getDegrees(), kEpsilon)); } @Test diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 65d3016d18..60d372cdc6 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -59,7 +59,7 @@ TEST(Rotation2dTest, Multiply) { const auto rot = Rotation2d{10_deg}; EXPECT_DOUBLE_EQ(30.0, (rot * 3.0).Degrees().value()); - EXPECT_DOUBLE_EQ(410.0, (rot * 41.0).Degrees().value()); + EXPECT_DOUBLE_EQ(50.0, (rot * 41.0).Degrees().value()); } TEST(Rotation2dTest, Equality) { @@ -90,9 +90,9 @@ TEST(Rotation2dTest, Constexpr) { constexpr auto subtracted = cartesianCtor - degreeCtor; static_assert(defaultCtor.Radians() == 0_rad); - static_assert(degreeCtor.Degrees() == 270_deg); - static_assert(negated.Radians() == -5_rad); - static_assert(multiplied.Radians() == 10_rad); + static_assert(degreeCtor.Degrees() == -90_deg); + static_assert(negated.Radians() == -5_rad + 1_tr); + static_assert(multiplied.Radians() == 10_rad - 2_tr); static_assert(subtracted == rotation45); static_assert(radianCtor != degreeCtor); } diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index e7726e6d96..a851e562c2 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -143,7 +143,7 @@ TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) { EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon); EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon); EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon); - EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon); + EXPECT_NEAR(brMod.angle.Degrees().value(), -90.0, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {