[wpimath] Implement Pose2d and Pose3d rotateAround()

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) {
+    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) {
+    return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)};
+  }
+
   /**
    * Obtain a new Pose3d from a (constant curvature) velocity.
    *