Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix CoordinateSystem.convert() Transform3d overload (#5532)

Browse Source
This commit is contained in:
amquake
2023-08-12 15:20:22 -07:00
committed by GitHub
parent b542e01a0b
commit 8121566258
4 changed files with 23 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
View File

@@ -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());
}