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