diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index 90e3624afd..c408de859c 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -183,7 +183,8 @@ public class Rotation2d { @Override public boolean equals(Object obj) { if (obj instanceof Rotation2d) { - return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9; + var other = (Rotation2d) obj; + return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9; } return false; } diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 9f1e3afdf5..efcc520e4f 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -65,7 +65,7 @@ Rotation2d Rotation2d::operator*(double scalar) const { } bool Rotation2d::operator==(const Rotation2d& other) const { - return units::math::abs(m_value - other.m_value) < 1E-9_rad; + return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9; } bool Rotation2d::operator!=(const Rotation2d& other) const { diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java index 51a4245857..468d2a2042 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java @@ -15,22 +15,22 @@ class Rotation2dTest { @Test void testRadiansToDegrees() { - var one = new Rotation2d(Math.PI / 3); - var two = new Rotation2d(Math.PI / 4); + var rot1 = new Rotation2d(Math.PI / 3); + var rot2 = new Rotation2d(Math.PI / 4); assertAll( - () -> assertEquals(one.getDegrees(), 60.0, kEpsilon), - () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)); + () -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon), + () -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon)); } @Test void testRadiansAndDegrees() { - var one = Rotation2d.fromDegrees(45.0); - var two = Rotation2d.fromDegrees(30.0); + var rot1 = Rotation2d.fromDegrees(45.0); + var rot2 = Rotation2d.fromDegrees(30.0); assertAll( - () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon), - () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)); + () -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon), + () -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon)); } @Test @@ -53,23 +53,27 @@ class Rotation2dTest { @Test void testMinus() { - var one = Rotation2d.fromDegrees(70.0); - var two = Rotation2d.fromDegrees(30.0); + var rot1 = Rotation2d.fromDegrees(70.0); + var rot2 = Rotation2d.fromDegrees(30.0); - assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon); + assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon); } @Test void testEquality() { - var one = Rotation2d.fromDegrees(43.0); - var two = Rotation2d.fromDegrees(43.0); - assertEquals(one, two); + var rot1 = Rotation2d.fromDegrees(43.0); + var rot2 = Rotation2d.fromDegrees(43.0); + assertEquals(rot1, rot2); + + var rot3 = Rotation2d.fromDegrees(-180.0); + var rot4 = Rotation2d.fromDegrees(180.0); + assertEquals(rot3, rot4); } @Test void testInequality() { - var one = Rotation2d.fromDegrees(43.0); - var two = Rotation2d.fromDegrees(43.5); - assertNotEquals(one, two); + var rot1 = Rotation2d.fromDegrees(43.0); + var rot2 = Rotation2d.fromDegrees(43.5); + assertNotEquals(rot1, rot2); } } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 0d69e10a18..15d683310f 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -14,19 +14,19 @@ using namespace frc; static constexpr double kEpsilon = 1E-9; TEST(Rotation2dTest, RadiansToDegrees) { - const Rotation2d one{units::radian_t(wpi::math::pi / 3)}; - const Rotation2d two{units::radian_t(wpi::math::pi / 4)}; + const Rotation2d rot1{units::radian_t(wpi::math::pi / 3)}; + const Rotation2d rot2{units::radian_t(wpi::math::pi / 4)}; - EXPECT_NEAR(one.Degrees().to(), 60.0, kEpsilon); - EXPECT_NEAR(two.Degrees().to(), 45.0, kEpsilon); + EXPECT_NEAR(rot1.Degrees().to(), 60.0, kEpsilon); + EXPECT_NEAR(rot2.Degrees().to(), 45.0, kEpsilon); } TEST(Rotation2dTest, DegreesToRadians) { - const auto one = Rotation2d(45.0_deg); - const auto two = Rotation2d(30.0_deg); + const auto rot1 = Rotation2d(45.0_deg); + const auto rot2 = Rotation2d(30.0_deg); - EXPECT_NEAR(one.Radians().to(), wpi::math::pi / 4.0, kEpsilon); - EXPECT_NEAR(two.Radians().to(), wpi::math::pi / 6.0, kEpsilon); + EXPECT_NEAR(rot1.Radians().to(), wpi::math::pi / 4.0, kEpsilon); + EXPECT_NEAR(rot2.Radians().to(), wpi::math::pi / 6.0, kEpsilon); } TEST(Rotation2dTest, RotateByFromZero) { @@ -45,20 +45,24 @@ TEST(Rotation2dTest, RotateByNonZero) { } TEST(Rotation2dTest, Minus) { - const auto one = Rotation2d(70.0_deg); - const auto two = Rotation2d(30.0_deg); + const auto rot1 = Rotation2d(70.0_deg); + const auto rot2 = Rotation2d(30.0_deg); - EXPECT_NEAR((one - two).Degrees().to(), 40.0, kEpsilon); + EXPECT_NEAR((rot1 - rot2).Degrees().to(), 40.0, kEpsilon); } TEST(Rotation2dTest, Equality) { - const auto one = Rotation2d(43_deg); - const auto two = Rotation2d(43_deg); - EXPECT_TRUE(one == two); + const auto rot1 = Rotation2d(43_deg); + const auto rot2 = Rotation2d(43_deg); + EXPECT_EQ(rot1, rot2); + + const auto rot3 = Rotation2d(-180_deg); + const auto rot4 = Rotation2d(180_deg); + EXPECT_EQ(rot3, rot4); } TEST(Rotation2dTest, Inequality) { - const auto one = Rotation2d(43_deg); - const auto two = Rotation2d(43.5_deg); - EXPECT_TRUE(one != two); + const auto rot1 = Rotation2d(43_deg); + const auto rot2 = Rotation2d(43.5_deg); + EXPECT_NE(rot1, rot2); }