[wpimath] Implement Rotation3d interpolation as slerp instead of lerp (#8529)

Also replace arithmetic operators since they're not commutative, which
is confusing for users.

wpimath/src/test/java/org/wpilib/math/geometry/QuaternionTest.java

@@ -219,8 +219,8 @@ class QuaternionTest {
     var start = new Quaternion(1, 2, 3, 4);
     var expect = new Quaternion(5, 6, 7, 8);
 
-    var twist = start.log(expect);
-    var actual = start.exp(twist);
+    var twist = expect.times(start.inverse()).log();
+    var actual = twist.exp().times(start);
 
     assertEquals(expect, actual);
   }

wpimath/src/test/java/org/wpilib/math/geometry/Rotation3dTest.java

@@ -24,7 +24,7 @@ class Rotation3dTest {
     var rot1 = new Rotation3d(0, 0, Math.PI / 2);
     var rot2 = new Rotation3d(Math.PI, 0, 0);
     var rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
-    final var result1 = rot1.plus(rot2).plus(rot3);
+    final var result1 = rot1.rotateBy(rot2).rotateBy(rot3);
     final var expected1 = new Rotation3d(0, -Math.PI / 2, Math.PI / 2);
     assertAll(
         () -> assertEquals(expected1, result1),
@@ -34,7 +34,7 @@ class Rotation3dTest {
     rot1 = new Rotation3d(0, 0, Math.PI / 2);
     rot2 = new Rotation3d(-Math.PI, 0, 0);
     rot3 = new Rotation3d(Math.PI / 2, 0, 0);
-    final var result2 = rot1.plus(rot2).plus(rot3);
+    final var result2 = rot1.rotateBy(rot2).rotateBy(rot3);
     final var expected2 = new Rotation3d(0, Math.PI / 2, Math.PI / 2);
     assertAll(
         () -> assertEquals(expected2, result2),
@@ -44,7 +44,7 @@ class Rotation3dTest {
     rot1 = new Rotation3d(0, 0, Math.PI / 2);
     rot2 = new Rotation3d(0, Math.PI / 3, 0);
     rot3 = new Rotation3d(-Math.PI / 2, 0, 0);
-    final var result3 = rot1.plus(rot2).plus(rot3);
+    final var result3 = rot1.rotateBy(rot2).rotateBy(rot3);
     final var expected3 = new Rotation3d(0, Math.PI / 2, Math.PI / 6);
     assertAll(
         () -> assertEquals(expected3, result3),
@@ -196,22 +196,22 @@ class Rotation3dTest {
   void testRotationLoop() {
     var rot = Rotation3d.kZero;
 
-    rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
+    rot = rot.rotateBy(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
     var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
     assertEquals(expected, rot);
 
-    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
+    rot = rot.rotateBy(new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
     expected =
         new Rotation3d(
             VecBuilder.fill(1.0 / Math.sqrt(3), 1.0 / Math.sqrt(3), -1.0 / Math.sqrt(3)),
             Units.degreesToRadians(120.0));
     assertEquals(expected, rot);
 
-    rot = rot.plus(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
+    rot = rot.rotateBy(new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));
     expected = new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0);
     assertEquals(expected, rot);
 
-    rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
+    rot = rot.rotateBy(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
     assertEquals(Rotation3d.kZero, rot);
   }
 
@@ -253,7 +253,7 @@ class Rotation3dTest {
     final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
 
     var rot = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
-    rot = rot.plus(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
+    rot = rot.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(30.0)));
 
     var expected = new Rotation3d(xAxis, Units.degreesToRadians(120.0));
     assertEquals(expected, rot);
@@ -264,7 +264,7 @@ class Rotation3dTest {
     final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
 
     var rot = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
-    rot = rot.plus(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
+    rot = rot.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(30.0)));
 
     var expected = new Rotation3d(yAxis, Units.degreesToRadians(120.0));
     assertEquals(expected, rot);
@@ -275,7 +275,7 @@ class Rotation3dTest {
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
 
     var rot = new Rotation3d(zAxis, Units.degreesToRadians(90.0));
-    rot = rot.plus(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
+    rot = rot.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(30.0)));
 
     var expected = new Rotation3d(zAxis, Units.degreesToRadians(120.0));
     assertEquals(expected, rot);
@@ -297,16 +297,6 @@ class Rotation3dTest {
     assertEquals(expected, result);
   }
 
-  @Test
-  void testMinus() {
-    final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
-
-    var rot1 = new Rotation3d(zAxis, Units.degreesToRadians(70.0));
-    var rot2 = new Rotation3d(zAxis, Units.degreesToRadians(30.0));
-
-    assertEquals(rot1.minus(rot2).getZ(), Units.degreesToRadians(40.0), kEpsilon);
-  }
-
   @Test
   void testEquality() {
     final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

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

@@ -200,8 +200,8 @@ TEST(QuaternionTest, LogarithmAndExponentialInverse) {
   Quaternion start{1, 2, 3, 4};
   Quaternion expect{5, 6, 7, 8};
 
-  auto twist = start.Log(expect);
-  auto actual = start.Exp(twist);
+  auto twist = (expect * start.Inverse()).Log();
+  auto actual = twist.Exp() * start;
 
   EXPECT_EQ(expect, actual);
 }

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

@@ -20,7 +20,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
   auto rot2 = Rotation3d{wpi::units::radian_t{std::numbers::pi}, 0_rad, 0_rad};
   auto rot3 =
       Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
-  const auto result1 = rot1 + rot2 + rot3;
+  const auto result1 = rot1.RotateBy(rot2).RotateBy(rot3);
   const auto expected1 =
       Rotation3d{0_rad, -wpi::units::radian_t{std::numbers::pi / 2},
                  wpi::units::radian_t{std::numbers::pi / 2}};
@@ -31,7 +31,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
   rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
   rot2 = Rotation3d{wpi::units::radian_t{-std::numbers::pi}, 0_rad, 0_rad};
   rot3 = Rotation3d{wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
-  const auto result2 = rot1 + rot2 + rot3;
+  const auto result2 = rot1.RotateBy(rot2).RotateBy(rot3);
   const auto expected2 =
       Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
                  wpi::units::radian_t{std::numbers::pi / 2}};
@@ -42,7 +42,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) {
   rot1 = Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}};
   rot2 = Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 3}, 0_rad};
   rot3 = Rotation3d{-wpi::units::radian_t{std::numbers::pi / 2}, 0_rad, 0_rad};
-  const auto result3 = rot1 + rot2 + rot3;
+  const auto result3 = rot1.RotateBy(rot2).RotateBy(rot3);
   const auto expected3 =
       Rotation3d{0_rad, wpi::units::radian_t{std::numbers::pi / 2},
                  wpi::units::radian_t{std::numbers::pi / 6}};
@@ -184,20 +184,20 @@ TEST(Rotation3dTest, DegreesToRadians) {
 TEST(Rotation3dTest, RotationLoop) {
   Rotation3d rot;
 
-  rot = rot + Rotation3d{90_deg, 0_deg, 0_deg};
+  rot = rot.RotateBy(Rotation3d{90_deg, 0_deg, 0_deg});
   Rotation3d expected{90_deg, 0_deg, 0_deg};
   EXPECT_EQ(expected, rot);
 
-  rot = rot + Rotation3d{0_deg, 90_deg, 0_deg};
+  rot = rot.RotateBy(Rotation3d{0_deg, 90_deg, 0_deg});
   expected = Rotation3d{
       {1.0 / std::sqrt(3), 1.0 / std::sqrt(3), -1.0 / std::sqrt(3)}, 120_deg};
   EXPECT_EQ(expected, rot);
 
-  rot = rot + Rotation3d{0_deg, 0_deg, 90_deg};
+  rot = rot.RotateBy(Rotation3d{0_deg, 0_deg, 90_deg});
   expected = Rotation3d{0_deg, 90_deg, 0_deg};
   EXPECT_EQ(expected, rot);
 
-  rot = rot + Rotation3d{0_deg, -90_deg, 0_deg};
+  rot = rot.RotateBy(Rotation3d{0_deg, -90_deg, 0_deg});
   EXPECT_EQ(Rotation3d{}, rot);
 }
 
@@ -205,7 +205,7 @@ TEST(Rotation3dTest, RotateByFromZeroX) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
 
   const Rotation3d zero;
-  auto rotated = zero + Rotation3d{xAxis, 90_deg};
+  auto rotated = zero.RotateBy(Rotation3d{xAxis, 90_deg});
 
   Rotation3d expected{xAxis, 90_deg};
   EXPECT_EQ(expected, rotated);
@@ -215,7 +215,7 @@ TEST(Rotation3dTest, RotateByFromZeroY) {
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
 
   const Rotation3d zero;
-  auto rotated = zero + Rotation3d{yAxis, 90_deg};
+  auto rotated = zero.RotateBy(Rotation3d{yAxis, 90_deg});
 
   Rotation3d expected{yAxis, 90_deg};
   EXPECT_EQ(expected, rotated);
@@ -225,7 +225,7 @@ TEST(Rotation3dTest, RotateByFromZeroZ) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
   const Rotation3d zero;
-  auto rotated = zero + Rotation3d{zAxis, 90_deg};
+  auto rotated = zero.RotateBy(Rotation3d{zAxis, 90_deg});
 
   Rotation3d expected{zAxis, 90_deg};
   EXPECT_EQ(expected, rotated);
@@ -235,7 +235,7 @@ TEST(Rotation3dTest, RotateByNonZeroX) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
 
   auto rot = Rotation3d{xAxis, 90_deg};
-  rot = rot + Rotation3d{xAxis, 30_deg};
+  rot = rot.RotateBy(Rotation3d{xAxis, 30_deg});
 
   Rotation3d expected{xAxis, 120_deg};
   EXPECT_EQ(expected, rot);
@@ -245,7 +245,7 @@ TEST(Rotation3dTest, RotateByNonZeroY) {
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
 
   auto rot = Rotation3d{yAxis, 90_deg};
-  rot = rot + Rotation3d{yAxis, 30_deg};
+  rot = rot.RotateBy(Rotation3d{yAxis, 30_deg});
 
   Rotation3d expected{yAxis, 120_deg};
   EXPECT_EQ(expected, rot);
@@ -255,7 +255,7 @@ TEST(Rotation3dTest, RotateByNonZeroZ) {
   const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
 
   auto rot = Rotation3d{zAxis, 90_deg};
-  rot = rot + Rotation3d{zAxis, 30_deg};
+  rot = rot.RotateBy(Rotation3d{zAxis, 30_deg});
 
   Rotation3d expected{zAxis, 120_deg};
   EXPECT_EQ(expected, rot);
@@ -276,15 +276,6 @@ TEST(Rotation3dTest, RelativeTo) {
   EXPECT_EQ(expected, result);
 }
 
-TEST(Rotation3dTest, Minus) {
-  const Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
-
-  const auto rot1 = Rotation3d{zAxis, 70_deg};
-  const auto rot2 = Rotation3d{zAxis, 30_deg};
-
-  EXPECT_DOUBLE_EQ(40.0, wpi::units::degree_t{(rot1 - rot2).Z()}.value());
-}
-
 TEST(Rotation3dTest, AxisAngle) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
@@ -376,7 +367,7 @@ TEST(Rotation3dTest, Interpolate) {
   rot2 = Rotation3d{yAxis, -160_deg};
   interpolated = wpi::util::Lerp(rot1, rot2, 0.5);
   EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.X()}.value());
-  EXPECT_DOUBLE_EQ(-5.0, wpi::units::degree_t{interpolated.Y()}.value());
+  EXPECT_NEAR(-5.0, wpi::units::degree_t{interpolated.Y()}.value(), 1e-9);
   EXPECT_DOUBLE_EQ(180.0, wpi::units::degree_t{interpolated.Z()}.value());
 
   // 50 + (70 - 50) * 0.5 = 60

wpimath/src/test/python/geometry/test_quaternion.py

@@ -204,8 +204,8 @@ def test_logarithm_and_exponential_inverse():
     start = Quaternion(1, 2, 3, 4)
     expect = Quaternion(5, 6, 7, 8)
 
-    twist = start.log(expect)
-    actual = start.exp(twist)
+    twist = (expect * start.inverse()).log()
+    actual = twist.exp() * start
 
     assert actual == expect
 

wpimath/src/test/python/geometry/test_rotation3d.py

@@ -9,7 +9,7 @@ def test_gimbal_lock_accuracy():
     rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
     rot2 = Rotation3d(roll=math.pi, pitch=0, yaw=0)
     rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0)
-    result1 = rot1 + rot2 + rot3
+    result1 = rot1.rotateBy(rot2).rotateBy(rot3)
     expected1 = Rotation3d(roll=0, pitch=-math.pi / 2, yaw=math.pi / 2)
     assert expected1 == result1
     assert (result1.x + result1.z) == pytest.approx(math.pi / 2)
@@ -18,7 +18,7 @@ def test_gimbal_lock_accuracy():
     rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
     rot2 = Rotation3d(roll=-math.pi, pitch=0, yaw=0)
     rot3 = Rotation3d(roll=math.pi / 2, pitch=0, yaw=0)
-    result2 = rot1 + rot2 + rot3
+    result2 = rot1.rotateBy(rot2).rotateBy(rot3)
     expected2 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 2)
     assert expected2 == result2
     assert (result2.z - result2.x) == pytest.approx(math.pi / 2)
@@ -27,7 +27,7 @@ def test_gimbal_lock_accuracy():
     rot1 = Rotation3d(roll=0, pitch=0, yaw=math.pi / 2)
     rot2 = Rotation3d(roll=0, pitch=math.pi / 3, yaw=0)
     rot3 = Rotation3d(roll=-math.pi / 2, pitch=0, yaw=0)
-    result3 = rot1 + rot2 + rot3
+    result3 = rot1.rotateBy(rot2).rotateBy(rot3)
     expected3 = Rotation3d(roll=0, pitch=math.pi / 2, yaw=math.pi / 6)
     assert expected3 == result3
     assert (result3.z - result3.x) == pytest.approx(math.pi / 6)
@@ -170,22 +170,22 @@ def test_degrees_to_radians():
 def test_rotation_loop():
     rot = Rotation3d()
 
-    rot = rot + Rotation3d(math.radians(90), 0, 0)
+    rot = rot.rotateBy(Rotation3d(math.radians(90), 0, 0))
     expected = Rotation3d(math.radians(90), 0, 0)
     assert expected == rot
 
-    rot = rot + Rotation3d(0, math.radians(90), 0)
+    rot = rot.rotateBy(Rotation3d(0, math.radians(90), 0))
     expected = Rotation3d(
         np.array([1.0 / math.sqrt(3), 1.0 / math.sqrt(3), -1.0 / math.sqrt(3)]),
         math.radians(120),
     )
     assert expected == rot
 
-    rot = rot + Rotation3d(0, 0, math.radians(90))
+    rot = rot.rotateBy(Rotation3d(0, 0, math.radians(90)))
     expected = Rotation3d(0, math.radians(90), 0)
     assert expected == rot
 
-    rot = rot + Rotation3d(0, math.radians(-90), 0)
+    rot = rot.rotateBy(Rotation3d(0, math.radians(-90), 0))
     assert Rotation3d() == rot
 
 
@@ -193,7 +193,7 @@ def test_rotate_by_from_zero_x():
     x_axis = np.array([1.0, 0.0, 0.0])
 
     zero = Rotation3d()
-    rotated = zero + Rotation3d(x_axis, math.radians(90))
+    rotated = zero.rotateBy(Rotation3d(x_axis, math.radians(90)))
 
     expected = Rotation3d(x_axis, math.radians(90))
     assert expected == rotated
@@ -203,7 +203,7 @@ def test_rotate_by_from_zero_y():
     y_axis = np.array([0.0, 1.0, 0.0])
 
     zero = Rotation3d()
-    rotated = zero + Rotation3d(y_axis, math.radians(90))
+    rotated = zero.rotateBy(Rotation3d(y_axis, math.radians(90)))
 
     expected = Rotation3d(y_axis, math.radians(90))
     assert expected == rotated
@@ -213,7 +213,7 @@ def test_rotate_by_from_zero_z():
     z_axis = np.array([0.0, 0.0, 1.0])
 
     zero = Rotation3d()
-    rotated = zero + Rotation3d(z_axis, math.radians(90))
+    rotated = zero.rotateBy(Rotation3d(z_axis, math.radians(90)))
 
     expected = Rotation3d(z_axis, math.radians(90))
     assert expected == rotated
@@ -223,7 +223,7 @@ def test_rotate_by_non_zero_x():
     x_axis = np.array([1.0, 0.0, 0.0])
 
     rot = Rotation3d(x_axis, math.radians(90))
-    rot = rot + Rotation3d(x_axis, math.radians(30))
+    rot = rot.rotateBy(Rotation3d(x_axis, math.radians(30)))
 
     expected = Rotation3d(x_axis, math.radians(120))
     assert expected == rot
@@ -233,7 +233,7 @@ def test_rotate_by_non_zero_y():
     y_axis = np.array([0.0, 1.0, 0.0])
 
     rot = Rotation3d(y_axis, math.radians(90))
-    rot = rot + Rotation3d(y_axis, math.radians(30))
+    rot = rot.rotateBy(Rotation3d(y_axis, math.radians(30)))
 
     expected = Rotation3d(y_axis, math.radians(120))
     assert expected == rot
@@ -243,21 +243,12 @@ def test_rotate_by_non_zero_z():
     z_axis = np.array([0.0, 0.0, 1.0])
 
     rot = Rotation3d(z_axis, math.radians(90))
-    rot = rot + Rotation3d(z_axis, math.radians(30))
+    rot = rot.rotateBy(Rotation3d(z_axis, math.radians(30)))
 
     expected = Rotation3d(z_axis, math.radians(120))
     assert expected == rot
 
 
-def test_minus():
-    z_axis = np.array([0.0, 0.0, 1.0])
-
-    rot1 = Rotation3d(z_axis, math.radians(70))
-    rot2 = Rotation3d(z_axis, math.radians(30))
-
-    assert math.degrees((rot1 - rot2).z) == pytest.approx(40.0)
-
-
 def test_axis_angle():
     x_axis = np.array([1.0, 0.0, 0.0])
     y_axis = np.array([0.0, 1.0, 0.0])