diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java index 57331771df..9e827c3e48 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java @@ -124,7 +124,9 @@ public class CoordinateSystem { */ public static Transform3d convert( Transform3d transform, CoordinateSystem from, CoordinateSystem to) { + var coordRot = from.m_rotation.minus(to.m_rotation); return new Transform3d( - convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to)); + convert(transform.getTranslation(), from, to), + coordRot.unaryMinus().plus(transform.getRotation().rotateBy(coordRot))); } } diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp index d1ee93d417..4b3c20a4c0 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp @@ -68,6 +68,8 @@ Pose3d CoordinateSystem::Convert(const Pose3d& pose, Transform3d CoordinateSystem::Convert(const Transform3d& transform, const CoordinateSystem& from, const CoordinateSystem& to) { - return Transform3d{Convert(transform.Translation(), from, to), - Convert(transform.Rotation(), from, to)}; + const auto coordRot = from.m_rotation - to.m_rotation; + return Transform3d{ + Convert(transform.Translation(), from, to), + (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java index 68babdd813..4dc18263b4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java @@ -38,18 +38,12 @@ class CoordinateSystemTest { assertEquals( transformTo.getTranslation(), CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo)); - assertEquals( - transformTo.getRotation(), - CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo)); assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo)); // "to" to "from" assertEquals( transformFrom.getTranslation(), CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom)); - assertEquals( - transformFrom.getRotation(), - CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom)); assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom)); } @@ -158,9 +152,7 @@ class CoordinateSystemTest { // No rotation from EDN to NWU checkTransform3dConvert( new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()), - new Transform3d( - new Translation3d(3.0, -1.0, -2.0), - new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))), + new Transform3d(new Translation3d(3.0, -1.0, -2.0), new Rotation3d()), CoordinateSystem.EDN(), CoordinateSystem.NWU()); @@ -171,7 +163,7 @@ class CoordinateSystemTest { new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), new Transform3d( new Translation3d(3.0, -1.0, -2.0), - new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))), + new Rotation3d(0.0, Units.degreesToRadians(-45.0), 0.0)), CoordinateSystem.EDN(), CoordinateSystem.NWU()); @@ -182,7 +174,7 @@ class CoordinateSystemTest { new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)), new Transform3d( new Translation3d(3.0, -1.0, -2.0), - new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(-45.0))), CoordinateSystem.EDN(), CoordinateSystem.NWU()); @@ -193,10 +185,7 @@ class CoordinateSystemTest { new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))), new Transform3d( new Translation3d(3.0, -1.0, -2.0), - new Rotation3d( - Units.degreesToRadians(-90.0), - Units.degreesToRadians(45.0), - Units.degreesToRadians(-90.0))), + new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), CoordinateSystem.EDN(), CoordinateSystem.NWU()); } @@ -206,9 +195,7 @@ class CoordinateSystemTest { // No rotation from EDN to NED checkTransform3dConvert( new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()), - new Transform3d( - new Translation3d(3.0, 1.0, 2.0), - new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))), + new Transform3d(new Translation3d(3.0, 1.0, 2.0), new Rotation3d()), CoordinateSystem.EDN(), CoordinateSystem.NED()); @@ -219,7 +206,7 @@ class CoordinateSystemTest { new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), new Transform3d( new Translation3d(3.0, 1.0, 2.0), - new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))), + new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)), CoordinateSystem.EDN(), CoordinateSystem.NED()); @@ -230,7 +217,7 @@ class CoordinateSystemTest { new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)), new Transform3d( new Translation3d(3.0, 1.0, 2.0), - new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))), CoordinateSystem.EDN(), CoordinateSystem.NED()); @@ -241,10 +228,7 @@ class CoordinateSystemTest { new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))), new Transform3d( new Translation3d(3.0, 1.0, 2.0), - new Rotation3d( - Units.degreesToRadians(90.0), - Units.degreesToRadians(-45.0), - Units.degreesToRadians(90.0))), + new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), CoordinateSystem.EDN(), CoordinateSystem.NED()); } diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp index fc44fa52ed..28d0dd0fff 100644 --- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp @@ -37,9 +37,6 @@ void CheckTransform3dConvert(const Transform3d& transformFrom, EXPECT_EQ(transformTo.Translation(), CoordinateSystem::Convert(transformFrom.Translation(), coordFrom, coordTo)); - EXPECT_EQ( - transformTo.Rotation(), - CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo)); EXPECT_EQ(transformTo, CoordinateSystem::Convert(transformFrom, coordFrom, coordTo)); @@ -47,9 +44,6 @@ void CheckTransform3dConvert(const Transform3d& transformFrom, EXPECT_EQ( transformFrom.Translation(), CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom)); - EXPECT_EQ( - transformFrom.Rotation(), - CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom)); EXPECT_EQ(transformFrom, CoordinateSystem::Convert(transformTo, coordTo, coordFrom)); } @@ -106,29 +100,28 @@ TEST(CoordinateSystemTest, Transform3dEDNtoNWU) { // No rotation from EDN to NWU CheckTransform3dConvert( Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}}, - Transform3d{Translation3d{3_m, -1_m, -2_m}, - Rotation3d{-90_deg, 0_deg, -90_deg}}, + Transform3d{Translation3d{3_m, -1_m, -2_m}, Rotation3d{}}, CoordinateSystem::EDN(), CoordinateSystem::NWU()); // 45° roll from EDN to NWU CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{45_deg, 0_deg, 0_deg}}, Transform3d{Translation3d{3_m, -1_m, -2_m}, - Rotation3d{-45_deg, 0_deg, -90_deg}}, + Rotation3d{0_deg, -45_deg, 0_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NWU()); // 45° pitch from EDN to NWU CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{0_deg, 45_deg, 0_deg}}, Transform3d{Translation3d{3_m, -1_m, -2_m}, - Rotation3d{-90_deg, 0_deg, -135_deg}}, + Rotation3d{0_deg, 0_deg, -45_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NWU()); // 45° yaw from EDN to NWU CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{0_deg, 0_deg, 45_deg}}, Transform3d{Translation3d{3_m, -1_m, -2_m}, - Rotation3d{-90_deg, 45_deg, -90_deg}}, + Rotation3d{45_deg, 0_deg, 0_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NWU()); } @@ -136,28 +129,27 @@ TEST(CoordinateSystemTest, Transform3dEDNtoNED) { // No rotation from EDN to NED CheckTransform3dConvert( Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}}, - Transform3d{Translation3d{3_m, 1_m, 2_m}, - Rotation3d{90_deg, 0_deg, 90_deg}}, + Transform3d{Translation3d{3_m, 1_m, 2_m}, Rotation3d{}}, CoordinateSystem::EDN(), CoordinateSystem::NED()); // 45° roll from EDN to NED CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{45_deg, 0_deg, 0_deg}}, Transform3d{Translation3d{3_m, 1_m, 2_m}, - Rotation3d{135_deg, 0_deg, 90_deg}}, + Rotation3d{0_deg, 45_deg, 0_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NED()); // 45° pitch from EDN to NED CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{0_deg, 45_deg, 0_deg}}, Transform3d{Translation3d{3_m, 1_m, 2_m}, - Rotation3d{90_deg, 0_deg, 135_deg}}, + Rotation3d{0_deg, 0_deg, 45_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NED()); // 45° yaw from EDN to NED CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{0_deg, 0_deg, 45_deg}}, Transform3d{Translation3d{3_m, 1_m, 2_m}, - Rotation3d{90_deg, -45_deg, 90_deg}}, + Rotation3d{45_deg, 0_deg, 0_deg}}, CoordinateSystem::EDN(), CoordinateSystem::NED()); }