[wpimath] Fix C++ Odometry ResetRotation missing a negation (#8949)

The original implementation was `m_gyroOffset + (rotation -
m_pose.Rotation())`, which means the first `RotateBy` should be using
`-m_pose.Rotation()` (see `ResetPose()`). Java is already correct. This
also adds new tests to catch this particular error.

wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp

@@ -95,7 +95,7 @@ class WPILIB_DLLEXPORT Odometry {
    * @param rotation The rotation to reset to.
    */
   void ResetRotation(const Rotation2d& rotation) {
-    m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
+    m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
     m_pose = Pose2d{m_pose.Translation(), rotation};
     m_previousAngle = rotation;
   }

wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometryTest.java

@@ -101,6 +101,52 @@ class SwerveDriveOdometryTest {
         () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
   }
 
+  @Test
+  void testResetTranslation() {
+    var modulePositions = new SwerveModulePosition[] {zero, zero, zero, zero};
+    var gyro = Rotation2d.fromDegrees(30.0);
+
+    m_odometry.resetPosition(
+        Rotation2d.kZero, modulePositions, new Pose2d(Translation2d.kZero, Rotation2d.kZero));
+    var initialPose = m_odometry.update(gyro, modulePositions);
+
+    assertAll(
+        () -> assertEquals(0.00, initialPose.getX(), 0.1),
+        () -> assertEquals(0.00, initialPose.getY(), 0.1),
+        () -> assertEquals(30.0, initialPose.getRotation().getDegrees(), 0.1));
+
+    m_odometry.resetTranslation(new Translation2d(0.5, 0));
+    var newPose = m_odometry.update(gyro, modulePositions);
+
+    assertAll(
+        () -> assertEquals(0.50, newPose.getX(), 0.1),
+        () -> assertEquals(0.00, newPose.getY(), 0.1),
+        () -> assertEquals(30.0, newPose.getRotation().getDegrees(), 0.1));
+  }
+
+  @Test
+  void testResetRotation() {
+    var modulePositions = new SwerveModulePosition[] {zero, zero, zero, zero};
+    var gyro = Rotation2d.fromDegrees(30.0);
+
+    m_odometry.resetPosition(
+        Rotation2d.kZero, modulePositions, new Pose2d(0.5, 0.0, Rotation2d.kZero));
+    var initialPose = m_odometry.update(gyro, modulePositions);
+
+    assertAll(
+        () -> assertEquals(0.50, initialPose.getX(), 0.1),
+        () -> assertEquals(0.00, initialPose.getY(), 0.1),
+        () -> assertEquals(30.0, initialPose.getRotation().getDegrees(), 0.1));
+
+    m_odometry.resetRotation(Rotation2d.kCCW_90deg);
+    var newPose = m_odometry.update(gyro, modulePositions);
+
+    assertAll(
+        () -> assertEquals(0.50, newPose.getX(), 0.1),
+        () -> assertEquals(0.00, newPose.getY(), 0.1),
+        () -> assertEquals(90.0, newPose.getRotation().getDegrees(), 0.1));
+  }
+
   @Test
   void testAccuracyFacingTrajectory() {
     var kinematics =

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

@@ -73,6 +73,44 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
   EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
 
+TEST_F(SwerveDriveOdometryTest, ResetTranslation) {
+  wpi::util::array<SwerveModulePosition, 4> wheelPositions{zero, zero, zero,
+                                                           zero};
+
+  m_odometry.ResetPosition(0_deg, wheelPositions, Pose2d{});
+  auto pose = m_odometry.Update(30_deg, wheelPositions);
+
+  EXPECT_NEAR(0.0, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
+
+  m_odometry.ResetTranslation({0.5_m, 0_m});
+  pose = m_odometry.Update(30_deg, wheelPositions);
+
+  EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, ResetRotation) {
+  wpi::util::array<SwerveModulePosition, 4> wheelPositions{zero, zero, zero,
+                                                           zero};
+
+  m_odometry.ResetPosition(0_deg, wheelPositions, Pose2d{0.5_m, 0_m, 0_rad});
+  auto pose = m_odometry.Update(30_deg, wheelPositions);
+
+  EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
+
+  m_odometry.ResetRotation(90_deg);
+  pose = m_odometry.Update(30_deg, wheelPositions);
+
+  EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
+}
+
 TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
   SwerveDriveKinematics<4> kinematics{
       Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},