Add equality comparator to geometry classes (#1882)

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