From 86b666bba91ea64a9ecfbda43aa1b4892b1213b6 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sun, 8 Sep 2019 14:20:26 -0400 Subject: [PATCH] Add equality comparator to geometry classes (#1882) --- .../src/main/native/cpp/geometry/Pose2d.cpp | 8 ++++++ .../main/native/cpp/geometry/Rotation2d.cpp | 8 ++++++ .../main/native/cpp/geometry/Transform2d.cpp | 8 ++++++ .../native/cpp/geometry/Translation2d.cpp | 9 +++++++ .../cpp/kinematics/MecanumDriveKinematics.cpp | 3 +-- .../main/native/include/frc/geometry/Pose2d.h | 16 ++++++++++++ .../native/include/frc/geometry/Rotation2d.h | 16 ++++++++++++ .../native/include/frc/geometry/Transform2d.h | 16 ++++++++++++ .../include/frc/geometry/Translation2d.h | 16 ++++++++++++ .../native/include/frc/geometry/Twist2d.h | 20 +++++++++++++++ .../frc/kinematics/SwerveDriveKinematics.inc | 3 +-- .../test/native/cpp/geometry/Pose2dTest.cpp | 12 +++++++++ .../native/cpp/geometry/Rotation2dTest.cpp | 12 +++++++++ .../native/cpp/geometry/Translation2dTest.cpp | 12 +++++++++ .../test/native/cpp/geometry/Twist2dTest.cpp | 12 +++++++++ .../wpi/first/wpilibj/geometry/Pose2d.java | 22 ++++++++++++++++ .../first/wpilibj/geometry/Rotation2d.java | 25 +++++++++++++++++-- .../first/wpilibj/geometry/Transform2d.java | 22 ++++++++++++++++ .../first/wpilibj/geometry/Translation2d.java | 22 ++++++++++++++++ .../wpi/first/wpilibj/geometry/Twist2d.java | 23 +++++++++++++++++ .../kinematics/MecanumDriveKinematics.java | 3 +-- .../kinematics/SwerveDriveKinematics.java | 3 +-- .../first/wpilibj/geometry/Pose2dTest.java | 15 +++++++++++ .../wpilibj/geometry/Rotation2dTest.java | 15 +++++++++++ .../wpilibj/geometry/Translation2dTest.java | 15 +++++++++++ .../first/wpilibj/geometry/Twist2dTest.java | 15 +++++++++++ 26 files changed, 341 insertions(+), 10 deletions(-) diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp index 8ff9b5e366..6b4c2f0547 100644 --- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp @@ -27,6 +27,14 @@ Pose2d& Pose2d::operator+=(const Transform2d& other) { return *this; } +bool Pose2d::operator==(const Pose2d& other) const { + return m_translation == other.m_translation && m_rotation == other.m_rotation; +} + +bool Pose2d::operator!=(const Pose2d& other) const { + return !operator==(other); +} + Pose2d Pose2d::TransformBy(const Transform2d& other) const { return {m_translation + (other.Translation().RotateBy(m_rotation)), m_rotation + other.Rotation()}; diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp index 7e170c40bd..7aeb8a91c5 100644 --- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp @@ -52,6 +52,14 @@ Rotation2d& Rotation2d::operator-=(const Rotation2d& other) { Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); } +bool Rotation2d::operator==(const Rotation2d& other) const { + return units::math::abs(m_value - other.m_value) < 1E-9_rad; +} + +bool Rotation2d::operator!=(const Rotation2d& other) const { + return !operator==(other); +} + Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { return {Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos()}; diff --git a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp index b184d1b98f..04f0419ec0 100644 --- a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp @@ -23,3 +23,11 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) { Transform2d::Transform2d(Translation2d translation, Rotation2d rotation) : m_translation(translation), m_rotation(rotation) {} + +bool Transform2d::operator==(const Transform2d& other) const { + return m_translation == other.m_translation && m_rotation == other.m_rotation; +} + +bool Transform2d::operator!=(const Transform2d& other) const { + return !operator==(other); +} diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp index 1de2b1171c..ca287d120e 100644 --- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp @@ -60,6 +60,15 @@ Translation2d Translation2d::operator/(double scalar) const { return *this * (1.0 / scalar); } +bool Translation2d::operator==(const Translation2d& other) const { + return units::math::abs(m_x - other.m_x) < 1E-9_m && + units::math::abs(m_y - other.m_y) < 1E-9_m; +} + +bool Translation2d::operator!=(const Translation2d& other) const { + return !operator==(other); +} + Translation2d& Translation2d::operator/=(double scalar) { *this *= (1.0 / scalar); return *this; diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index bbe06470cb..cf6ebb5b43 100644 --- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -12,8 +12,7 @@ using namespace frc; MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds( const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) { // We have a new center of rotation. We need to compute the matrix again. - if (centerOfRotation.X() != m_previousCoR.X() || - centerOfRotation.Y() != m_previousCoR.Y()) { + if (centerOfRotation != m_previousCoR) { auto fl = m_frontLeftWheel - centerOfRotation; auto fr = m_frontRightWheel - centerOfRotation; auto rl = m_rearLeftWheel - centerOfRotation; diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h index 92364c2893..75f06bfd3a 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h @@ -68,6 +68,22 @@ class Pose2d { */ Pose2d& operator+=(const Transform2d& other); + /** + * Checks equality between this Pose2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Pose2d& other) const; + + /** + * Checks inequality between this Pose2d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Pose2d& other) const; + /** * Returns the underlying translation. * diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h index 37bc295c30..2c5773e028 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h @@ -97,6 +97,22 @@ class Rotation2d { */ Rotation2d operator-() const; + /** + * Checks equality between this Rotation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Rotation2d& other) const; + + /** + * Checks inequality between this Rotation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Rotation2d& other) const; + /** * Adds the new rotation to the current rotation using a rotation matrix. * diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h index 1fbfcf4e09..24dea15673 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h @@ -53,6 +53,22 @@ class Transform2d { */ const Rotation2d& Rotation() const { return m_rotation; } + /** + * Checks equality between this Transform2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Transform2d& other) const; + + /** + * Checks inequality between this Transform2d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Transform2d& other) const; + private: Translation2d m_translation; Rotation2d m_rotation; diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h index 296ba35eaf..a53abf118d 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h @@ -180,6 +180,22 @@ class Translation2d { */ Translation2d operator/(double scalar) const; + /** + * Checks equality between this Translation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Translation2d& other) const; + + /** + * Checks inequality between this Translation2d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Translation2d& other) const; + /* * Divides the current translation by a scalar. * diff --git a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h index 97897f18c4..ab246a0898 100644 --- a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h @@ -31,5 +31,25 @@ struct Twist2d { * Angular "dtheta" component (radians) */ units::radian_t dtheta = 0_rad; + + /** + * Checks equality between this Twist2d and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const Twist2d& other) const { + return units::math::abs(dx - other.dx) < 1E-9_m && + units::math::abs(dy - other.dy) < 1E-9_m && + units::math::abs(dtheta - other.dtheta) < 1E-9_rad; + } + + /** + * Checks inequality between this Twist2d and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const Twist2d& other) const { return !operator==(other); } }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index 0fe3b232f9..138954d2ac 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -20,8 +20,7 @@ std::array SwerveDriveKinematics::ToSwerveModuleStates( const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) { // We have a new center of rotation. We need to compute the matrix again. - if (centerOfRotation.X() != m_previousCoR.X() || - centerOfRotation.Y() != m_previousCoR.Y()) { + if (centerOfRotation != m_previousCoR) { for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) << diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp index cfb74ba5e8..74d43fc237 100644 --- a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -40,3 +40,15 @@ TEST(Pose2dTest, RelativeTo) { EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to(), 0.0, kEpsilon); } + +TEST(Pose2dTest, Equality) { + const Pose2d a{0_m, 5_m, Rotation2d(43_deg)}; + const Pose2d b{0_m, 5_m, Rotation2d(43_deg)}; + EXPECT_TRUE(a == b); +} + +TEST(Pose2dTest, Inequality) { + const Pose2d a{0_m, 5_m, Rotation2d(43_deg)}; + const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)}; + EXPECT_TRUE(a != b); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp index 4876fa47a1..ba807870b0 100644 --- a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -53,3 +53,15 @@ TEST(Rotation2dTest, Minus) { EXPECT_NEAR((one - two).Degrees().to(), 40.0, kEpsilon); } + +TEST(Rotation2dTest, Equality) { + const auto one = Rotation2d(43_deg); + const auto two = Rotation2d(43_deg); + EXPECT_TRUE(one == two); +} + +TEST(Rotation2dTest, Inequality) { + const auto one = Rotation2d(43_deg); + const auto two = Rotation2d(43.5_deg); + EXPECT_TRUE(one != two); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp index 18009e98a3..99fced719e 100644 --- a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -76,3 +76,15 @@ TEST(Translation2dTest, UnaryMinus) { EXPECT_NEAR(inverted.X().to(), 4.5, kEpsilon); EXPECT_NEAR(inverted.Y().to(), -7, kEpsilon); } + +TEST(Translation2dTest, Equality) { + const Translation2d one{9_m, 5.5_m}; + const Translation2d two{9_m, 5.5_m}; + EXPECT_TRUE(one == two); +} + +TEST(Translation2dTest, Inequality) { + const Translation2d one{9_m, 5.5_m}; + const Translation2d two{9_m, 5.7_m}; + EXPECT_TRUE(one != two); +} diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp index eb779f8489..9b9e2a7284 100644 --- a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -44,3 +44,15 @@ TEST(Twist2dTest, DiagonalNoDtheta) { EXPECT_NEAR(diagonalPose.Translation().Y().to(), 2.0, kEpsilon); EXPECT_NEAR(diagonalPose.Rotation().Degrees().to(), 0.0, kEpsilon); } + +TEST(Twist2dTest, Equality) { + const Twist2d one{5.0_m, 1.0_m, 3.0_rad}; + const Twist2d two{5.0_m, 1.0_m, 3.0_rad}; + EXPECT_TRUE(one == two); +} + +TEST(Twist2dTest, Inequality) { + const Twist2d one{5.0_m, 1.0_m, 3.0_rad}; + const Twist2d two{5.0_m, 1.2_m, 3.0_rad}; + EXPECT_TRUE(one != two); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java index 2d55ea0930..982275e9f1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.geometry; +import java.util.Objects; + /** * Represents a 2d pose containing translational and rotational elements. */ @@ -154,4 +156,24 @@ public class Pose2d { return this.plus(transform); } + + /** + * Checks equality between this Pose2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Pose2d) { + return ((Pose2d) obj).m_translation.equals(m_translation) + && ((Pose2d) obj).m_rotation.equals(m_rotation); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_translation, m_rotation); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index bca5c9291a..d6f2548423 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.geometry; +import java.util.Objects; + /** * A rotation in a 2d coordinate frame represented a point on the unit circle * (cosine and sine). @@ -118,8 +120,8 @@ public class Rotation2d { */ public Rotation2d rotateBy(Rotation2d other) { return new Rotation2d( - m_cos * other.m_cos - m_sin * other.m_sin, - m_cos * other.m_sin + m_sin * other.m_cos + m_cos * other.m_cos - m_sin * other.m_sin, + m_cos * other.m_sin + m_sin * other.m_cos ); } @@ -167,4 +169,23 @@ public class Rotation2d { public double getTan() { return m_sin / m_cos; } + + /** + * Checks equality between this Rotation2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Rotation2d) { + return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_value); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java index ddb90b040c..49fa726310 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.geometry; +import java.util.Objects; + /** * Represents a transformation for a Pose2d. */ @@ -66,4 +68,24 @@ public class Transform2d { public Rotation2d getRotation() { return m_rotation; } + + /** + * Checks equality between this Transform2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Transform2d) { + return ((Transform2d) obj).m_translation.equals(m_translation) + && ((Transform2d) obj).m_rotation.equals(m_rotation); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_translation, m_rotation); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java index fd175c267d..83028d3863 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.geometry; +import java.util.Objects; + /** * Represents a translation in 2d space. * This object can be used to represent a point or a vector. @@ -162,4 +164,24 @@ public class Translation2d { public Translation2d div(double scalar) { return new Translation2d(m_x / scalar, m_y / scalar); } + + /** + * Checks equality between this Translation2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Translation2d) { + return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9 + && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_x, m_y); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java index 65351f2068..191a616e96 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.geometry; +import java.util.Objects; + /** * A change in distance along arc since the last pose update. We can use ideas * from differential calculus to create new Pose2ds from a Twist2d and vise @@ -45,4 +47,25 @@ public class Twist2d { this.dy = dy; this.dtheta = dtheta; } + + /** + * Checks equality between this Twist2d and another object. + * + * @param obj The other object. + * @return Whether the two objects are equal or not. + */ + @Override + public boolean equals(Object obj) { + if (obj instanceof Twist2d) { + return Math.abs(((Twist2d) obj).dx - dx) < 1E-9 + && Math.abs(((Twist2d) obj).dy - dy) < 1E-9 + && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(dx, dy, dtheta); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java index 4b0370e014..9caad5922f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java @@ -94,8 +94,7 @@ public class MecanumDriveKinematics { public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) { // We have a new center of rotation. We need to compute the matrix again. - if (centerOfRotationMeters.getX() != m_prevCoR.getX() - || centerOfRotationMeters.getY() != m_prevCoR.getY()) { + if (!centerOfRotationMeters.equals(m_prevCoR)) { var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters); var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters); var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java index ab0700920f..f09c5cc1a8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java @@ -96,8 +96,7 @@ public class SwerveDriveKinematics { @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) { - if (centerOfRotationMeters.getX() != m_prevCoR.getX() - || centerOfRotationMeters.getY() != m_prevCoR.getY()) { + if (!centerOfRotationMeters.equals(m_prevCoR)) { for (int i = 0; i < m_numModules; i++) { m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY()); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java index 21377d004d..03fdec7f01 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; class Pose2dTest { private static final double kEpsilon = 1E-9; @@ -44,4 +45,18 @@ class Pose2dTest { () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon) ); } + + @Test + void testEquality() { + var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0)); + var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0)); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0)); + var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0)); + assertNotEquals(one, two); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java index d1265db8ef..6c4b1b3a4d 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; class Rotation2dTest { private static final double kEpsilon = 1E-9; @@ -63,4 +64,18 @@ class Rotation2dTest { assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon); } + + @Test + void testEquality() { + var one = Rotation2d.fromDegrees(43.0); + var two = Rotation2d.fromDegrees(43.0); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = Rotation2d.fromDegrees(43.0); + var two = Rotation2d.fromDegrees(43.5); + assertNotEquals(one, two); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java index a2385f283f..b4e29475f2 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; class Translation2dTest { private static final double kEpsilon = 1E-9; @@ -97,4 +98,18 @@ class Translation2dTest { () -> assertEquals(inverted.getY(), -7, kEpsilon) ); } + + @Test + void testEquality() { + var one = new Translation2d(9, 5.5); + var two = new Translation2d(9, 5.5); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = new Translation2d(9, 5.5); + var two = new Translation2d(9, 5.7); + assertNotEquals(one, two); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java index 59a20aae54..d356f339a4 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; class Twist2dTest { private static final double kEpsilon = 1E-9; @@ -50,4 +51,18 @@ class Twist2dTest { () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon) ); } + + @Test + void testEquality() { + var one = new Twist2d(5, 1, 3); + var two = new Twist2d(5, 1, 3); + assertEquals(one, two); + } + + @Test + void testInequality() { + var one = new Twist2d(5, 1, 3); + var two = new Twist2d(5, 1.2, 3); + assertNotEquals(one, two); + } }