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

[wpimath] Add an operator for composing two Transform2ds (#3527)

Browse Source
This commit is contained in:
Tyler Veness
2021-08-23 20:22:48 -07:00
committed by GitHub
parent 2edd510ab7
commit e0c6cd3dcc
5 changed files with 60 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -15,8 +15,8 @@ using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Transform2dTest, Inverse) {
const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{{5_m, 0_m}, 5_deg};
auto transformed = initial + transform;
auto untransformed = transformed + transform.Inverse();
@@ -28,3 +28,19 @@ TEST(Transform2dTest, Inverse) {
EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
untransformed.Rotation().Degrees().to<double>(), kEpsilon);
}
TEST(Transform2dTest, Composition) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform1{{5_m, 0_m}, 5_deg};
const Transform2d transform2{{0_m, 2_m}, 5_deg};
auto transformedSeparate = initial + transform1 + transform2;
auto transformedCombined = initial + (transform1 + transform2);
EXPECT_NEAR(transformedSeparate.X().to<double>(),
transformedCombined.X().to<double>(), kEpsilon);
EXPECT_NEAR(transformedSeparate.Y().to<double>(),
transformedCombined.Y().to<double>(), kEpsilon);
EXPECT_NEAR(transformedSeparate.Rotation().Degrees().to<double>(),
transformedCombined.Rotation().Degrees().to<double>(), kEpsilon);
}