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

Fixes #5472.

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

@@ -138,6 +138,16 @@ public class Pose2d implements Interpolatable<Pose2d> {
     return times(1.0 / scalar);
   }
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose2d rotateBy(Rotation2d other) {
+    return new Pose2d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new pose. See + operator for
    * the matrix multiplication performed.

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

@@ -150,6 +150,16 @@ public class Pose3d implements Interpolatable<Pose3d> {
     return times(1.0 / scalar);
   }
 
+  /**
+   * Rotates the pose around the origin and returns the new pose.
+   *
+   * @param other The rotation to transform the pose by.
+   * @return The transformed pose.
+   */
+  public Pose3d rotateBy(Rotation3d other) {
+    return new Pose3d(m_translation.rotateBy(other), m_rotation.rotateBy(other));
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new pose.
    *

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -60,6 +60,10 @@ Pose3d Pose3d::operator/(double scalar) const {
   return *this * (1.0 / scalar);
 }
 
+Pose3d Pose3d::RotateBy(const Rotation3d& other) const {
+  return {m_translation.RotateBy(other), m_rotation.RotateBy(other)};
+}
+
 Pose3d Pose3d::TransformBy(const Transform3d& other) const {
   return {m_translation + (other.Translation().RotateBy(m_rotation)),
           other.Rotation() + m_rotation};

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.
    *

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

@@ -14,6 +14,28 @@ import org.junit.jupiter.api.Test;
 class Pose2dTest {
   private static final double kEpsilon = 1E-9;
 
+  @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial = new Pose2d(new Translation2d(x, y), Rotation2d.fromDegrees(45.0));
+
+    var rotation = Rotation2d.fromDegrees(5.0);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = rotation.getCos();
+    double s = rotation.getSin();
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getDegrees() + rotation.getDegrees(),
+                rotated.getRotation().getDegrees(),
+                kEpsilon));
+  }
+
   @Test
   void testTransformBy() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));

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

@@ -17,6 +17,38 @@ import org.junit.jupiter.api.Test;
 class Pose3dTest {
   private static final double kEpsilon = 1E-9;
 
+  @Test
+  void testRotateBy() {
+    final double x = 1.0;
+    final double y = 2.0;
+    var initial =
+        new Pose3d(
+            new Translation3d(x, y, 0.0),
+            new Rotation3d(
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(0.0),
+                Units.degreesToRadians(45.0)));
+
+    double yaw = Units.degreesToRadians(5.0);
+    var rotation = new Rotation3d(Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), yaw);
+    var rotated = initial.rotateBy(rotation);
+
+    // Translation is rotated by CCW rotation matrix
+    double c = Math.cos(yaw);
+    double s = Math.sin(yaw);
+    assertAll(
+        () -> assertEquals(c * x - s * y, rotated.getX(), kEpsilon),
+        () -> assertEquals(s * x + c * y, rotated.getY(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getZ(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getX(), kEpsilon),
+        () -> assertEquals(0.0, rotated.getRotation().getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getZ() + rotation.getZ(),
+                rotated.getRotation().getZ(),
+                kEpsilon));
+  }
+
   @Test
   void testTransformByRotations() {
     var initialPose =

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

@@ -10,6 +10,24 @@
 
 using namespace frc;
 
+TEST(Pose2dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose2d initial{x, y, 45_deg};
+
+  const Rotation2d rotation{5_deg};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = rotation.Cos();
+  double s = rotation.Sin();
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(
+      initial.Rotation().Degrees().value() + rotation.Degrees().value(),
+      rotated.Rotation().Degrees().value());
+}
+
 TEST(Pose2dTest, TransformBy) {
   const Pose2d initial{1_m, 2_m, 45_deg};
   const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};

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

@@ -11,6 +11,27 @@
 
 using namespace frc;
 
+TEST(Pose3dTest, RotateBy) {
+  constexpr auto x = 1_m;
+  constexpr auto y = 2_m;
+  const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+
+  constexpr units::radian_t yaw = 5_deg;
+  const Rotation3d rotation{0_deg, 0_deg, yaw};
+  const auto rotated = initial.RotateBy(rotation);
+
+  // Translation is rotated by CCW rotation matrix
+  double c = std::cos(yaw.value());
+  double s = std::sin(yaw.value());
+  EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
+  EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Z().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().X().value());
+  EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().Y().value());
+  EXPECT_DOUBLE_EQ(initial.Rotation().Z().value() + rotation.Z().value(),
+                   rotated.Rotation().Z().value());
+}
+
 TEST(Pose3dTest, TestTransformByRotations) {
   const double kEpsilon = 1E-9;