[wpimath] Add RotateBy() function to pose classes (#5491)

Fixes #5472.

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

@@ -122,6 +122,15 @@ class WPILIB_DLLEXPORT Pose2d {
    */
   constexpr Pose2d operator/(double scalar) const;
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   *
+   * @return The rotated pose.
+   */
+  constexpr Pose2d RotateBy(const Rotation2d& other) const;
+
   /**
    * Transforms the pose by the given transformation and returns the new pose.
    * See + operator for the matrix multiplication performed.

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

@@ -31,6 +31,10 @@ constexpr Pose2d Pose2d::operator/(double scalar) const {
   return *this * (1.0 / scalar);
 }
 
+constexpr Pose2d Pose2d::RotateBy(const Rotation2d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 constexpr Pose2d Pose2d::TransformBy(const Transform2d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};

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

@@ -130,6 +130,15 @@ class WPILIB_DLLEXPORT Pose3d {
    */
   Pose3d operator/(double scalar) const;
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   *
+   * @return The rotated pose.
+   */
+  Pose3d RotateBy(const Rotation3d& other) const;
+
   /**
    * Transforms the pose by the given transformation and returns the new pose.
    *