diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 0705d61ea7f..a8f250fba25 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/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. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 02c2b9ba4eb..16de1dc33bb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 0664161d6d3..2d8825caa5b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/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. * diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 176f2c03ab9..110faf73559 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/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. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 81f81509929..895c20de925 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index 00c63eae080..f8fadea6a59 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/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); diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 9a038590b1a..eb5c3bef992 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/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}; diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index f0168e95435..ee3f8fa53a8 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/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};