[wpimath] Implement Translation3d.RotateAround (#7661)

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -216,6 +216,17 @@ public Translation3d rotateBy(Rotation3d other) {
     return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
   }
 
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  public Translation3d rotateAround(Translation3d other, Rotation3d rot) {
+    return this.minus(other).rotateBy(rot).plus(other);
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the X-Y plane.
    *

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -148,6 +148,18 @@ class WPILIB_DLLEXPORT Translation3d {
                          units::meter_t{qprime.Z()}};
   }
 
+  /**
+   * Rotates this translation around another translation in 3D space.
+   *
+   * @param other The other translation to rotate around.
+   * @param rot The rotation to rotate the translation by.
+   * @return The new rotated translation.
+   */
+  constexpr Translation3d RotateAround(const Translation3d& other,
+                                       const Rotation3d& rot) const {
+    return (*this - other).RotateBy(rot) + other;
+  }
+
   /**
    * Returns a Translation2d representing this Translation3d projected into the
    * X-Y plane.

wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java

@@ -78,6 +78,40 @@ void testRotateBy() {
         () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
   }
 
+  @Test
+  void testRotateAround() {
+    var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
+    var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
+    var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
+
+    var translation = new Translation3d(1.0, 2.0, 3.0);
+    var around = new Translation3d(3.0, 2.0, 1.0);
+
+    var rotated1 =
+        translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(1.0, rotated1.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated1.getY(), kEpsilon),
+        () -> assertEquals(1.0, rotated1.getZ(), kEpsilon));
+
+    var rotated2 =
+        translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(5.0, rotated2.getX(), kEpsilon),
+        () -> assertEquals(2.0, rotated2.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated2.getZ(), kEpsilon));
+
+    var rotated3 =
+        translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
+
+    assertAll(
+        () -> assertEquals(3.0, rotated3.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated3.getY(), kEpsilon),
+        () -> assertEquals(3.0, rotated3.getZ(), kEpsilon));
+  }
+
   @Test
   void testToTranslation2d() {
     var translation = new Translation3d(1.0, 2.0, 3.0);

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

@@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) {
   const auto rotated = another.RotateBy(90_deg);
 
   EXPECT_NEAR(0.0, rotated.X().value(), 1e-9);
-  EXPECT_DOUBLE_EQ(3.0, rotated.Y().value());
+  EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
+}
+
+TEST(Translation2dTest, RotateAround) {
+  const Translation2d translation{2_m, 1_m};
+  const Translation2d other{3_m, 2_m};
+  const auto rotated = translation.RotateAround(other, 180_deg);
+
+  EXPECT_NEAR(4.0, rotated.X().value(), 1e-9);
+  EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9);
 }
 
 TEST(Translation2dTest, Multiplication) {

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

@@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) {
   EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
 }
 
+TEST(Translation3dTest, RotateAround) {
+  Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
+  Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
+  Eigen::Vector3d zAxis{0.0, 0.0, 1.0};
+
+  const Translation3d translation{1_m, 2_m, 3_m};
+  const Translation3d around{3_m, 2_m, 1_m};
+
+  const auto rotated1 =
+      translation.RotateAround(around, Rotation3d{xAxis, 90_deg});
+  EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon);
+
+  const auto rotated2 =
+      translation.RotateAround(around, Rotation3d{yAxis, 90_deg});
+  EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon);
+  EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon);
+
+  const auto rotated3 =
+      translation.RotateAround(around, Rotation3d{zAxis, 90_deg});
+  EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon);
+}
+
 TEST(Translation3dTest, ToTranslation2d) {
   Translation3d translation{1_m, 2_m, 3_m};
   Translation2d expected{1_m, 2_m};