[wpimath] Add Pose2d and Pose3d rotateAround() (#7659)

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

@@ -238,6 +238,17 @@ public Pose2d relativeTo(Pose2d other) {
     return new Pose2d(transform.getTranslation(), transform.getRotation());
   }
 
+  /**
+   * Rotates the current pose around a point in 2D space.
+   *
+   * @param point The point in 2D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose2d rotateAround(Translation2d point, Rotation2d rot) {
+    return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
   /**
    * Obtain a new Pose2d from a (constant curvature) velocity.
    *

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

@@ -271,6 +271,17 @@ public Pose3d relativeTo(Pose3d other) {
     return new Pose3d(transform.getTranslation(), transform.getRotation());
   }
 
+  /**
+   * Rotates the current pose around a point in 3D space.
+   *
+   * @param point The point in 3D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   * @return The new rotated pose.
+   */
+  public Pose3d rotateAround(Translation3d point, Rotation3d rot) {
+    return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
+  }
+
   /**
    * Obtain a new Pose3d from a (constant curvature) velocity.
    *

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

@@ -184,6 +184,19 @@ class WPILIB_DLLEXPORT Pose2d {
    */
   constexpr Pose2d RelativeTo(const Pose2d& other) const;
 
+  /**
+   * Rotates the current pose around a point in 2D space.
+   *
+   * @param point The point in 2D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   *
+   * @return The new rotated pose.
+   */
+  constexpr Pose2d RotateAround(const Translation2d& point,
+                                const Rotation2d& rot) const {
+    return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
+  }
+
   /**
    * Obtain a new Pose2d from a (constant curvature) velocity.
    *

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

@@ -207,6 +207,19 @@ class WPILIB_DLLEXPORT Pose3d {
    */
   constexpr Pose3d RelativeTo(const Pose3d& other) const;
 
+  /**
+   * Rotates the current pose around a point in 3D space.
+   *
+   * @param point The point in 3D space to rotate around.
+   * @param rot The rotation to rotate the pose by.
+   *
+   * @return The new rotated pose.
+   */
+  constexpr Pose3d RotateAround(const Translation3d& point,
+                                const Rotation3d& rot) const {
+    return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
+  }
+
   /**
    * Obtain a new Pose3d from a (constant curvature) velocity.
    *

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

@@ -72,6 +72,19 @@ void testRelativeTo() {
         () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getDegrees(), kEpsilon));
   }
 
+  @Test
+  void testRotateAround() {
+    var initial = new Pose2d(5, 0, Rotation2d.kZero);
+    var point = Translation2d.kZero;
+
+    var rotated = initial.rotateAround(point, Rotation2d.kPi);
+
+    assertAll(
+        () -> assertEquals(-5.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getY(), kEpsilon),
+        () -> assertEquals(180.0, rotated.getRotation().getDegrees(), kEpsilon));
+  }
+
   @Test
   void testEquality() {
     var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));

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

@@ -136,6 +136,19 @@ void testRelativeTo() {
         () -> assertEquals(0.0, finalRelativeToInitial.getRotation().getZ(), kEpsilon));
   }
 
+  @Test
+  void testRotateAround() {
+    var initial = new Pose3d(new Translation3d(5, 0, 0), Rotation3d.kZero);
+    var point = Translation3d.kZero;
+
+    var rotated = initial.rotateAround(point, new Rotation3d(0, 0, Math.PI));
+
+    assertAll(
+        () -> assertEquals(-5.0, rotated.getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getY(), kEpsilon),
+        () -> assertEquals(Math.PI, rotated.getRotation().getZ(), kEpsilon));
+  }
+
   @Test
   void testEquality() {
     var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);

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

@@ -51,6 +51,17 @@ TEST(Pose2dTest, RelativeTo) {
   EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Degrees().value(), 1e-9);
 }
 
+TEST(Pose2dTest, RotateAround) {
+  const Pose2d initial{5_m, 0_m, 0_deg};
+  const Translation2d point{0_m, 0_m};
+
+  const auto rotated = initial.RotateAround(point, Rotation2d{180_deg});
+
+  EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
+  EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
+  EXPECT_NEAR(180.0, rotated.Rotation().Degrees().value(), 1e-9);
+}
+
 TEST(Pose2dTest, Equality) {
   const Pose2d a{0_m, 5_m, 43_deg};
   const Pose2d b{0_m, 5_m, 43_deg};

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

@@ -83,6 +83,19 @@ TEST(Pose3dTest, RelativeTo) {
   EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Z().value(), 1e-9);
 }
 
+TEST(Pose3dTest, RotateAround) {
+  const Pose3d initial{5_m, 0_m, 0_m, Rotation3d{}};
+  const Translation3d point{0_m, 0_m, 0_m};
+
+  const auto rotated =
+      initial.RotateAround(point, Rotation3d{0_deg, 0_deg, 180_deg});
+
+  EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9);
+  EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9);
+  EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(),
+              1e-9);
+}
+
 TEST(Pose3dTest, Equality) {
   Eigen::Vector3d zAxis{0.0, 0.0, 1.0};