[wpimath] Fix Rotation3d interpolation and document extrinsic vs intrinsic (#8544)

Documents the extrinsic vs intrinsic semantics of `plus()` and
`minus()`. (`rotateBy()` was documented in [a previous
PR](https://github.com/wpilibsuite/allwpilib/pull/5508))
Fixes usage of `plus()` and `minus()` in `Rotation3d.interpolate()`.
(Fixes #8523)
Fixes incorrect usages of `plus()`, `minus()`, and `rotateBy()`
throughout `Odometry3d`.
Adds explanatory comments for some `plus()`, `minus()`, and `rotateBy()`
operations.
Fixes `TimeInterpolatableBuffer` not using twists for `Pose3d` (this was
just because I happened to notice it, it isn't really related to the PR)

To check all of our usages of `plus()`, `minus()`, and `rotateBy()`, I
marked them as deprecated, checked compile errors from `./gradlew
compileJava`, and then undeprecated them. You can see all of the spots
that showed up (at least on the Java side) by viewing the diff for
241109c.

I wanted to present this alternative to #8526 because the change has its
own quirks, there's little time before kickoff, and there would be no
code-side warning to teams (and mentors) already used to the current
behavior.

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

@@ -42,6 +42,15 @@ TEST(Rotation2dTest, RotateByNonZero) {
   EXPECT_DOUBLE_EQ(120.0, rot.Degrees().value());
 }
 
+TEST(Rotation2dTest, RelativeTo) {
+  auto start = Rotation2d{30_deg};
+  auto end = Rotation2d{90_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_DOUBLE_EQ(60.0, result.Degrees().value());
+}
+
 TEST(Rotation2dTest, Minus) {
   const auto rot1 = Rotation2d{70_deg};
   const auto rot2 = Rotation2d{30_deg};

wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp

@@ -253,6 +253,21 @@ TEST(Rotation3dTest, RotateByNonZeroZ) {
   EXPECT_EQ(expected, rot);
 }
 
+TEST(Rotation3dTest, RelativeTo) {
+  const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  auto start = Rotation3d{yAxis, -90_deg};
+  auto end = Rotation3d{zAxis, 90_deg};
+
+  const Eigen::Vector3d intrinsicAxis{1.0, 1.0, 1.0};
+  auto expected = Rotation3d{intrinsicAxis, 120_deg};
+
+  auto result = end.RelativeTo(start);
+
+  EXPECT_EQ(expected, result);
+}
+
 TEST(Rotation3dTest, Minus) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
@@ -371,4 +386,32 @@ TEST(Rotation3dTest, Interpolate) {
   EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.X()}.value());
   EXPECT_DOUBLE_EQ(0.0, units::degree_t{interpolated.Y()}.value());
   EXPECT_DOUBLE_EQ(-175.0, units::degree_t{interpolated.Z()}.value());
+
+  // t value of 0 should always produce the start
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 0.0);
+  EXPECT_DOUBLE_EQ(rot1.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(rot1.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(rot1.Z().value(), interpolated.Z().value());
+
+  // The full rotation from rot1 to rot2 is 120 degrees around extrinsic
+  // <-1.0, 1.0, 1.0>
+  const Eigen::Vector3d extrinsicAxis{-1.0, 1.0, 1.0};
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  EXPECT_EQ(rot2, rot1.RotateBy(Rotation3d{extrinsicAxis, 120_deg}));
+  interpolated = wpi::Lerp(rot1, rot2, 0.5);
+  auto expected = rot1.RotateBy(Rotation3d{extrinsicAxis, 60_deg});
+  EXPECT_DOUBLE_EQ(expected.X().value(), interpolated.X().value());
+  EXPECT_DOUBLE_EQ(expected.Y().value(), interpolated.Y().value());
+  EXPECT_DOUBLE_EQ(expected.Z().value(), interpolated.Z().value());
+
+  // t value of 1 should always produce the end
+  rot1 = Rotation3d{yAxis, -90_deg};
+  rot2 = Rotation3d{zAxis, 90_deg};
+  interpolated = wpi::Lerp(rot1, rot2, 1.0);
+  EXPECT_NEAR(rot2.X().value(), interpolated.X().value(), 1e-9);
+  EXPECT_NEAR(rot2.Y().value(), interpolated.Y().value(), 1e-9);
+  EXPECT_NEAR(rot2.Z().value(), interpolated.Z().value(), 1e-9);
 }

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

@@ -38,3 +38,18 @@ TEST(DifferentialDriveOdometry3dTest, EncoderDistances) {
   EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
   EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon);
 }
+
+TEST(DifferentialDriveOdometry3dTest, GyroOffset) {
+  DifferentialDriveOdometry3d odometry{
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, 0_m, 0_m,
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}}};
+  const auto& pose =
+      odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, 0_m, 0_m);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, kEpsilon);
+}

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

@@ -221,3 +221,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(MecanumDriveOdometry3dTest, GyroOffset) {
+  frc::MecanumDriveWheelPositions wheelPositions;
+  odometry.ResetPosition(
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, wheelPositions,
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose =
+      odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg}, wheelPositions);
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}

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

@@ -222,3 +222,18 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) {
   EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06);
   EXPECT_LT(maxError, 0.125);
 }
+
+TEST_F(SwerveDriveOdometry3dTest, GyroOffset) {
+  m_odometry.ResetPosition(
+      frc::Rotation3d{0_deg, 5_deg, 0_deg}, {zero, zero, zero, zero},
+      frc::Pose3d{frc::Translation3d{}, frc::Rotation3d{0_deg, 0_deg, 90_deg}});
+  auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 10_deg, 0_deg},
+                                {zero, zero, zero, zero});
+
+  EXPECT_NEAR(pose.X().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 1e-9);
+  EXPECT_NEAR(pose.Z().value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().X()}.value(), 0.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Y()}.value(), 5.0, 1e-9);
+  EXPECT_NEAR(units::degree_t{pose.Rotation().Z()}.value(), 90.0, 1e-9);
+}