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) {