From 01941def1dd830fdc706c895189a4e401825c28f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Tue, 7 Jan 2025 22:29:02 -0600 Subject: [PATCH 01/12] Do Pose2d Java --- .../java/edu/wpi/first/math/geometry/Pose2d.java | 13 +++++++++++++ 1 file changed, 13 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 0705d61ea7f..90734b91eff 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,19 @@ public Pose2d relativeTo(Pose2d other) { return new Pose2d(transform.getTranslation(), transform.getRotation()); } + /** + * Rotates the current pose around a translation in 2D space. + * + *

TODO: I feel like pivot is a bad name? + * + * @param pivot The translation to rotate around. + * @param rot The rotation to rotate the pose by. + * @return The new rotated pose. + */ + public Pose2d rotateAround(Translation2d pivot, Rotation2d rot) { + return new Pose2d(m_translation.rotateAround(pivot, rot), m_rotation.rotateBy(rot)); + } + /** * Obtain a new Pose2d from a (constant curvature) velocity. * From 67778f8de89dcb7b841fa7aabdc187698d829a4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Thu, 9 Jan 2025 18:56:43 -0600 Subject: [PATCH 02/12] Rename pivot to point --- .../main/java/edu/wpi/first/math/geometry/Pose2d.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) 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 90734b91eff..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 @@ -239,16 +239,14 @@ public Pose2d relativeTo(Pose2d other) { } /** - * Rotates the current pose around a translation in 2D space. + * Rotates the current pose around a point in 2D space. * - *

TODO: I feel like pivot is a bad name? - * - * @param pivot The translation to rotate around. + * @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 pivot, Rotation2d rot) { - return new Pose2d(m_translation.rotateAround(pivot, rot), m_rotation.rotateBy(rot)); + public Pose2d rotateAround(Translation2d point, Rotation2d rot) { + return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot)); } /** From c0ae64b180bc82d3d5f7fc07f2800334a6264e92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 09:41:58 -0600 Subject: [PATCH 03/12] Do Pose3d Java --- .../main/java/edu/wpi/first/math/geometry/Pose3d.java | 11 +++++++++++ 1 file changed, 11 insertions(+) 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. * From c46a753abd7b388f27846a85383bdccb540bc0aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 09:52:38 -0600 Subject: [PATCH 04/12] Do Pose2d C++ --- .../src/main/native/include/frc/geometry/Pose2d.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 0664161d6d3..17abe591f00 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) { + return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)}; + } + /** * Obtain a new Pose2d from a (constant curvature) velocity. * From 0e7fca71c7f523e3ccf28ca32e6afdde385a3bf5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 09:56:49 -0600 Subject: [PATCH 05/12] Do Pose3d C++ --- .../src/main/native/include/frc/geometry/Pose3d.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 176f2c03ab9..53812705e47 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) { + return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)}; + } + /** * Obtain a new Pose3d from a (constant curvature) velocity. * From 8a141285a9f3c4720d9b952a27f4a34ef7d64029 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 13:52:47 -0600 Subject: [PATCH 06/12] Make Pose2d.RotateAround const --- wpimath/src/main/native/include/frc/geometry/Pose2d.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 17abe591f00..2d8825caa5b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -193,7 +193,7 @@ class WPILIB_DLLEXPORT Pose2d { * @return The new rotated pose. */ constexpr Pose2d RotateAround(const Translation2d& point, - const Rotation2d& rot) { + const Rotation2d& rot) const { return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)}; } From 869933a71a57dd3613c356d628626c1ec0107e1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 13:58:07 -0600 Subject: [PATCH 07/12] Do Java and C++ unit tests for Pose2d --- .../edu/wpi/first/math/geometry/Pose2dTest.java | 13 +++++++++++++ wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp | 11 +++++++++++ 2 files changed, 24 insertions(+) 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/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 9a038590b1a..63e98360d05 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}; From 39691463e331d75f6354c7be9c96c1d7d658567e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 14:05:53 -0600 Subject: [PATCH 08/12] Make Pose3d.RotateAround const --- wpimath/src/main/native/include/frc/geometry/Pose3d.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 53812705e47..110faf73559 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -216,7 +216,7 @@ class WPILIB_DLLEXPORT Pose3d { * @return The new rotated pose. */ constexpr Pose3d RotateAround(const Translation3d& point, - const Rotation3d& rot) { + const Rotation3d& rot) const { return {m_translation.RotateAround(point, rot), m_rotation.RotateBy(rot)}; } From 0a2109b295ed44071edee80451a6e482e76cad0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 14:31:35 -0600 Subject: [PATCH 09/12] Do Java and C++ unit tests for Pose3d --- .../edu/wpi/first/math/geometry/Pose3dTest.java | 13 +++++++++++++ wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp | 12 ++++++++++++ 2 files changed, 25 insertions(+) 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/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index f0168e95435..bad03d1ac86 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -8,6 +8,7 @@ #include #include "frc/geometry/Pose3d.h" +#include using namespace frc; @@ -83,6 +84,17 @@ 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(M_PI, rotated.Rotation().Z().value(), 1e-9); +} + TEST(Pose3dTest, Equality) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; From 26957a144ef964b80970fefc7062ef235b681a8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 14:36:51 -0600 Subject: [PATCH 10/12] Formatting --- wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp | 2 +- wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index 63e98360d05..eb5c3bef992 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -56,7 +56,7 @@ TEST(Pose2dTest, RotateAround) { 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); diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index bad03d1ac86..e78aa6bf56b 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -2,13 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include #include #include "frc/geometry/Pose3d.h" -#include using namespace frc; @@ -88,7 +89,8 @@ 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}); + 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); From accecb9541b18d065a740fccf0b6ea0c13697b68 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 15:06:59 -0600 Subject: [PATCH 11/12] Fix compiling failing --- wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index e78aa6bf56b..4886e09c547 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -2,8 +2,6 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include - #include #include @@ -94,7 +92,7 @@ TEST(Pose3dTest, RotateAround) { EXPECT_NEAR(-5.0, rotated.X().value(), 1e-9); EXPECT_NEAR(0.0, rotated.Y().value(), 1e-9); - EXPECT_NEAR(M_PI, rotated.Rotation().Z().value(), 1e-9); + EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(), 1e-9); } TEST(Pose3dTest, Equality) { From 92060d5e2e4d9dba0f471237c0cb262e6e7336b9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E3=83=8F=E3=82=A4=E3=83=89=E3=83=A9=E3=83=B3=E3=83=88?= Date: Fri, 10 Jan 2025 15:12:45 -0600 Subject: [PATCH 12/12] Formatting again --- wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 4886e09c547..ee3f8fa53a8 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -92,7 +92,8 @@ TEST(Pose3dTest, RotateAround) { 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); + EXPECT_NEAR(units::radian_t{180_deg}.value(), rotated.Rotation().Z().value(), + 1e-9); } TEST(Pose3dTest, Equality) {