Add equality comparator to geometry classes (#1882)

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

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

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

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;

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;

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.
    *

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.
    *

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;

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.
    *

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

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

@@ -20,8 +20,7 @@ std::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::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) <<

wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp

@@ -40,3 +40,15 @@ TEST(Pose2dTest, RelativeTo) {
   EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 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);
+}

wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp

@@ -53,3 +53,15 @@ TEST(Rotation2dTest, Minus) {
 
   EXPECT_NEAR((one - two).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);
+}
+
+TEST(Rotation2dTest, Inequality) {
+  const auto one = Rotation2d(43_deg);
+  const auto two = Rotation2d(43.5_deg);
+  EXPECT_TRUE(one != two);
+}

wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp

@@ -76,3 +76,15 @@ TEST(Translation2dTest, UnaryMinus) {
   EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
   EXPECT_NEAR(inverted.Y().to<double>(), -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);
+}

wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp

@@ -44,3 +44,15 @@ TEST(Twist2dTest, DiagonalNoDtheta) {
   EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
   EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 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);
+}

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

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

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

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

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

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

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());

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

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

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

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