From 0fa3f24948679aae1ab76e1c0bbcc83da345e3e5 Mon Sep 17 00:00:00 2001 From: "harry.chen" Date: Wed, 8 Jan 2025 14:47:02 -0500 Subject: [PATCH 1/4] Implement Translation3d.RotateAround --- .../first/math/geometry/Translation3d.java | 11 ++++++ .../include/frc/geometry/Translation3d.h | 12 +++++++ .../math/geometry/Translation3dTest.java | 34 +++++++++++++++++++ .../native/cpp/geometry/Translation2dTest.cpp | 9 +++++ .../native/cpp/geometry/Translation3dTest.cpp | 27 +++++++++++++++ 5 files changed, 93 insertions(+) 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..7e8f4f7b942 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 operator-(other).RotateBy(rot).operator+(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..502d2ad02d1 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, 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, 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..76d27c4200a 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -38,6 +38,15 @@ TEST(Translation2dTest, RotateBy) { EXPECT_DOUBLE_EQ(3.0, rotated.Y().value()); } +TEST(Translation2dTest, RotateAround) { + const Translation2d another{2_m, 1_m}; + const Translation2d other{3_m, 2_m}; + const auto rotated = another.RotateAround(other, 180_deg); + + EXPECT_NEAR(4.0, rotated.X().value(), 1e-9); + EXPECT_DOUBLE_EQ(3.0, rotated.Y().value()); +} + TEST(Translation2dTest, Multiplication) { const Translation2d original{3_m, 5_m}; const auto mult = original * 3; 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}; From feb82d53534622eede1de65eee4c48310e3dba87 Mon Sep 17 00:00:00 2001 From: "harry.chen" Date: Wed, 8 Jan 2025 20:17:46 -0500 Subject: [PATCH 2/4] Make requested changes --- wpimath/src/main/native/include/frc/geometry/Translation3d.h | 2 +- .../java/edu/wpi/first/math/geometry/Translation3dTest.java | 4 ++-- wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 7e8f4f7b942..72e220f08c0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -157,7 +157,7 @@ class WPILIB_DLLEXPORT Translation3d { */ constexpr Translation3d RotateAround(const Translation3d& other, const Rotation3d& rot) const { - return operator-(other).RotateBy(rot).operator+(other); + return (*this - other).RotateBy(rot) + other; } /** 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 502d2ad02d1..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 @@ -92,7 +92,7 @@ void testRotateAround() { assertAll( () -> assertEquals(1.0, rotated1.getX(), kEpsilon), - () -> assertEquals(0, rotated1.getY(), kEpsilon), + () -> assertEquals(0.0, rotated1.getY(), kEpsilon), () -> assertEquals(1.0, rotated1.getZ(), kEpsilon)); var rotated2 = @@ -107,7 +107,7 @@ void testRotateAround() { translation.rotateAround(around, new Rotation3d(zAxis, Units.degreesToRadians(90.0))); assertAll( - () -> assertEquals(3, rotated3.getX(), kEpsilon), + () -> assertEquals(3.0, rotated3.getX(), kEpsilon), () -> assertEquals(0.0, rotated3.getY(), kEpsilon), () -> assertEquals(3.0, rotated3.getZ(), kEpsilon)); } diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index 76d27c4200a..ca5075bccc7 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -39,9 +39,9 @@ TEST(Translation2dTest, RotateBy) { } TEST(Translation2dTest, RotateAround) { - const Translation2d another{2_m, 1_m}; + const Translation2d translation{2_m, 1_m}; const Translation2d other{3_m, 2_m}; - const auto rotated = another.RotateAround(other, 180_deg); + const auto rotated = translation.RotateAround(other, 180_deg); EXPECT_NEAR(4.0, rotated.X().value(), 1e-9); EXPECT_DOUBLE_EQ(3.0, rotated.Y().value()); From cdd861d1b93ba2eb322ed5060d6d1ff757b6dc4c Mon Sep 17 00:00:00 2001 From: "harry.chen" Date: Wed, 8 Jan 2025 20:36:22 -0500 Subject: [PATCH 3/4] Make requested changes --- wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp index ca5075bccc7..294c213a3d6 100644 --- a/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp @@ -35,7 +35,7 @@ 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) { @@ -44,7 +44,7 @@ TEST(Translation2dTest, RotateAround) { const auto rotated = translation.RotateAround(other, 180_deg); EXPECT_NEAR(4.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, Multiplication) { From 9ba16e3ed19e04ffc7e57f14338250455d2880fc Mon Sep 17 00:00:00 2001 From: "harry.chen" Date: Wed, 8 Jan 2025 21:33:19 -0500 Subject: [PATCH 4/4] Trigger CI