[wpimath] Fix swerve kinematics util classes equals function (#4907)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java

@@ -32,14 +32,15 @@ public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModulePosition) {
-      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+      SwerveModulePosition other = (SwerveModulePosition) obj;
+      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(distanceMeters);
+    return Objects.hash(distanceMeters, angle);
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -32,15 +32,16 @@ public class SwerveModuleState implements Comparable<SwerveModuleState> {
   @Override
   public boolean equals(Object obj) {
     if (obj instanceof SwerveModuleState) {
-      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
-          == 0;
+      SwerveModuleState other = (SwerveModuleState) obj;
+      return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
+          && angle.equals(other.angle);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(speedMetersPerSecond);
+    return Objects.hash(speedMetersPerSecond, angle);
   }
 
   /**

wpimath/src/main/native/cpp/kinematics/SwerveModulePosition.cpp

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModulePosition.h"
+
+#include "frc/kinematics/SwerveModuleState.h"
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModulePosition::operator==(const SwerveModulePosition& other) const {
+  return units::math::abs(distance - other.distance) < 1E-9_m &&
+         angle == other.angle;
+}

wpimath/src/main/native/cpp/kinematics/SwerveModuleState.cpp

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/SwerveModuleState.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool SwerveModuleState::operator==(const SwerveModuleState& other) const {
+  return units::math::abs(speed - other.speed) < 1E-9_mps &&
+         angle == other.angle;
+}
+
+SwerveModuleState SwerveModuleState::Optimize(
+    const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
+  auto delta = desiredState.angle - currentAngle;
+  if (units::math::abs(delta.Degrees()) > 90_deg) {
+    return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+  } else {
+    return {desiredState.speed, desiredState.angle};
+  }
+}

wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h

@@ -25,5 +25,13 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Checks equality between this SwerveModulePosition and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModulePosition& other) const;
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h

@@ -26,6 +26,14 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    */
   Rotation2d angle;
 
+  /**
+   * Checks equality between this SwerveModuleState and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const SwerveModuleState& other) const;
+
   /**
    * Minimize the change in heading the desired swerve module state would
    * require by potentially reversing the direction the wheel spins. If this is
@@ -36,13 +44,6 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    * @param currentAngle The current module angle.
    */
   static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
-                                    const Rotation2d& currentAngle) {
-    auto delta = desiredState.angle - currentAngle;
-    if (units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
-    } else {
-      return {desiredState.speed, desiredState.angle};
-    }
-  }
+                                    const Rotation2d& currentAngle);
 };
 }  // namespace frc

wpimath/src/test/native/cpp/kinematics/SwerveModulePositionTest.cpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModulePosition.h"
+#include "gtest/gtest.h"
+
+TEST(SwerveModulePositionTest, Equality) {
+  frc::SwerveModulePosition position1{2_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+
+  EXPECT_EQ(position1, position2);
+}
+
+TEST(SwerveModulePositionTest, Inequality) {
+  frc::SwerveModulePosition position1{1_m, 90_deg};
+  frc::SwerveModulePosition position2{2_m, 90_deg};
+  frc::SwerveModulePosition position3{1_m, 89_deg};
+
+  EXPECT_NE(position1, position2);
+  EXPECT_NE(position1, position3);
+}

wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp

@@ -39,3 +39,19 @@ TEST(SwerveModuleStateTest, NoOptimize) {
   EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
   EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }
+
+TEST(SwerveModuleStateTest, Equality) {
+  frc::SwerveModuleState state1{2_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+
+  EXPECT_EQ(state1, state2);
+}
+
+TEST(SwerveModuleStateTest, Inequality) {
+  frc::SwerveModuleState state1{1_mps, 90_deg};
+  frc::SwerveModuleState state2{2_mps, 90_deg};
+  frc::SwerveModuleState state3{1_mps, 89_deg};
+
+  EXPECT_NE(state1, state2);
+  EXPECT_NE(state1, state3);
+}