From e37c35746a7952f8ef8ff0607ab3bad85dbbcaad Mon Sep 17 00:00:00 2001 From: HarryXChen <51322624+HarryXChen3@users.noreply.github.com> Date: Thu, 9 Jan 2025 23:31:34 -0500 Subject: [PATCH] [wpimath] Implement Translation3d.RotateAround (#7661) --- .../first/math/geometry/Translation3d.java | 11 ++++++ .../include/frc/geometry/Translation3d.h | 12 +++++++ .../math/geometry/Translation3dTest.java | 34 +++++++++++++++++++ .../native/cpp/geometry/Translation2dTest.cpp | 11 +++++- .../native/cpp/geometry/Translation3dTest.cpp | 27 +++++++++++++++ 5 files changed, 94 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 20cede3d3da..efaf92a6707 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -216,6 +216,17 @@ public Translation3d rotateBy(Rotation3d other) { return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ()); } + /** + * Rotates this translation around another translation in 3D space. + * + * @param other The other translation to rotate around. + * @param rot The rotation to rotate the translation by. + * @return The new rotated translation. + */ + public Translation3d rotateAround(Translation3d other, Rotation3d rot) { + return this.minus(other).rotateBy(rot).plus(other); + } + /** * Returns a Translation2d representing this Translation3d projected into the X-Y plane. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index c16fc56a5b8..72e220f08c0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -148,6 +148,18 @@ class WPILIB_DLLEXPORT Translation3d { units::meter_t{qprime.Z()}}; } + /** + * Rotates this translation around another translation in 3D space. + * + * @param other The other translation to rotate around. + * @param rot The rotation to rotate the translation by. + * @return The new rotated translation. + */ + constexpr Translation3d RotateAround(const Translation3d& other, + const Rotation3d& rot) const { + return (*this - other).RotateBy(rot) + other; + } + /** * Returns a Translation2d representing this Translation3d projected into the * X-Y plane. diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java index 3dd54227ea8..49b146fdd0a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation3dTest.java @@ -78,6 +78,40 @@ void testRotateBy() { () -> assertEquals(3.0, rotated3.getZ(), kEpsilon)); } + @Test + void testRotateAround() { + var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); + var yAxis = VecBuilder.fill(0.0, 1.0, 0.0); + var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); + + var translation = new Translation3d(1.0, 2.0, 3.0); + var around = new Translation3d(3.0, 2.0, 1.0); + + var rotated1 = + translation.rotateAround(around, new Rotation3d(xAxis, Units.degreesToRadians(90.0))); + + assertAll( + () -> assertEquals(1.0, rotated1.getX(), kEpsilon), + () -> assertEquals(0.0, rotated1.getY(), kEpsilon), + () -> assertEquals(1.0, rotated1.getZ(), kEpsilon)); + + var rotated2 = + translation.rotateAround(around, new Rotation3d(yAxis, Units.degreesToRadians(90.0))); + + assertAll( + () -> assertEquals(5.0, rotated2.getX(), kEpsilon), + () -> assertEquals(2.0, rotated2.getY(), kEpsilon), + () -> assertEquals(3.0, rotated2.getZ(), kEpsilon)); + + var rotated3 = + translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0))); + + assertAll( + () -> assertEquals(3.0, rotated3.getX(), kEpsilon), + () -> assertEquals(0.0, rotated3.getY(), kEpsilon), + () -> assertEquals(3.0, rotated3.getZ(), kEpsilon)); + } + @Test void testToTranslation2d() { var translation = new Translation3d(1.0, 2.0, 3.0); diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 46d383ea584..294c213a3d6 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -35,7 +35,16 @@ TEST(Translation2dTest, RotateBy) { const auto rotated = another.RotateBy(90_deg); EXPECT_NEAR(0.0, rotated.X().value(), 1e-9); - EXPECT_DOUBLE_EQ(3.0, rotated.Y().value()); + EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9); +} + +TEST(Translation2dTest, RotateAround) { + const Translation2d translation{2_m, 1_m}; + const Translation2d other{3_m, 2_m}; + const auto rotated = translation.RotateAround(other, 180_deg); + + EXPECT_NEAR(4.0, rotated.X().value(), 1e-9); + EXPECT_NEAR(3.0, rotated.Y().value(), 1e-9); } TEST(Translation2dTest, Multiplication) { diff --git a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp index 56eaf6da753..f463c0bfa42 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation3dTest.cpp @@ -57,6 +57,33 @@ TEST(Translation3dTest, RotateBy) { EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon); } +TEST(Translation3dTest, RotateAround) { + Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; + Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; + Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; + + const Translation3d translation{1_m, 2_m, 3_m}; + const Translation3d around{3_m, 2_m, 1_m}; + + const auto rotated1 = + translation.RotateAround(around, Rotation3d{xAxis, 90_deg}); + EXPECT_NEAR(rotated1.X().value(), 1.0, kEpsilon); + EXPECT_NEAR(rotated1.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(rotated1.Z().value(), 1.0, kEpsilon); + + const auto rotated2 = + translation.RotateAround(around, Rotation3d{yAxis, 90_deg}); + EXPECT_NEAR(rotated2.X().value(), 5.0, kEpsilon); + EXPECT_NEAR(rotated2.Y().value(), 2.0, kEpsilon); + EXPECT_NEAR(rotated2.Z().value(), 3.0, kEpsilon); + + const auto rotated3 = + translation.RotateAround(around, Rotation3d{zAxis, 90_deg}); + EXPECT_NEAR(rotated3.X().value(), 3.0, kEpsilon); + EXPECT_NEAR(rotated3.Y().value(), 0.0, kEpsilon); + EXPECT_NEAR(rotated3.Z().value(), 3.0, kEpsilon); +} + TEST(Translation3dTest, ToTranslation2d) { Translation3d translation{1_m, 2_m, 3_m}; Translation2d expected{1_m, 2_m};