From 35a8b129d9e9e086b32f32d668c3d3799e705ef3 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 31 Jul 2023 21:16:44 -0700 Subject: [PATCH] [wpimath] Add RotateBy() function to pose classes (#5491) Fixes #5472. --- .../edu/wpi/first/math/geometry/Pose2d.java | 10 ++++++ .../edu/wpi/first/math/geometry/Pose3d.java | 10 ++++++ .../src/main/native/cpp/geometry/Pose3d.cpp | 4 +++ .../main/native/include/frc/geometry/Pose2d.h | 9 ++++++ .../native/include/frc/geometry/Pose2d.inc | 4 +++ .../main/native/include/frc/geometry/Pose3d.h | 9 ++++++ .../wpi/first/math/geometry/Pose2dTest.java | 22 +++++++++++++ .../wpi/first/math/geometry/Pose3dTest.java | 32 +++++++++++++++++++ .../test/native/cpp/geometry/Pose2dTest.cpp | 18 +++++++++++ .../test/native/cpp/geometry/Pose3dTest.cpp | 21 ++++++++++++ 10 files changed, 139 insertions(+) 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 e744ffe656..43985865dd 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 @@ -138,6 +138,16 @@ public class Pose2d implements Interpolatable { 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. 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 68b3199f77..fb141b9ed3 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 @@ -150,6 +150,16 @@ public class Pose3d implements Interpolatable { 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. * diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index d70a3419cb..845ea7d0e4 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/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}; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 5c5c65454d..b890a9b47e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/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. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc b/wpimath/src/main/native/include/frc/geometry/Pose2d.inc index c549f26963..2764d16cbf 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.inc +++ b/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}; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 7bc03e4304..cfe484913b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/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. * 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 19fc76aac5..d57362d349 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 @@ -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)); 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 f59189d6a6..ee1d98c8c1 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 @@ -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 = diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index e496a09057..ba250624d0 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/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}; diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index b9702a3f41..8f0f91ef11 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/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;