Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Rotation2d equality operator (#3128)

Browse Source
This commit is contained in:
Tyler Veness
2021-01-25 21:41:34 -08:00
committed by GitHub
parent fb5c8c39ae
commit 2c2ccb3618
4 changed files with 45 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
View File

@@ -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;
}

2
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
View File

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

38
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
View File

@@ -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);
}
}

38
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -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<double>(), 60.0, kEpsilon);
EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
EXPECT_NEAR(rot1.Degrees().to<double>(), 60.0, kEpsilon);
EXPECT_NEAR(rot2.Degrees().to<double>(), 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<double>(), wpi::math::pi / 4.0, kEpsilon);
EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
EXPECT_NEAR(rot1.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
EXPECT_NEAR(rot2.Radians().to<double>(), 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<double>(), 40.0, kEpsilon);
EXPECT_NEAR((rot1 - rot2).Degrees().to<double>(), 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);
}