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 RotateBy() function to pose classes (#5491)

Browse Source 
Fixes #5472.
This commit is contained in:
Tyler Veness
2023-07-31 21:16:44 -07:00
committed by GitHub
parent 26d6e68c8f
commit 35a8b129d9
10 changed files with 139 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -10,6 +10,24 @@
using namespace frc;
TEST(Pose2dTest, RotateBy) {
constexpr auto x = 1_m;
constexpr auto y = 2_m;
const Pose2d initial{x, y, 45_deg};
const Rotation2d rotation{5_deg};
const auto rotated = initial.RotateBy(rotation);
// Translation is rotated by CCW rotation matrix
double c = rotation.Cos();
double s = rotation.Sin();
EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
EXPECT_DOUBLE_EQ(
initial.Rotation().Degrees().value() + rotation.Degrees().value(),
rotated.Rotation().Degrees().value());
}
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1_m, 2_m, 45_deg};
const Transform2d transform{Translation2d{5_m, 0_m}, 5_deg};

21
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -11,6 +11,27 @@
using namespace frc;
TEST(Pose3dTest, RotateBy) {
constexpr auto x = 1_m;
constexpr auto y = 2_m;
const Pose3d initial{x, y, 0_m, Rotation3d{0_deg, 0_deg, 45_deg}};
constexpr units::radian_t yaw = 5_deg;
const Rotation3d rotation{0_deg, 0_deg, yaw};
const auto rotated = initial.RotateBy(rotation);
// Translation is rotated by CCW rotation matrix
double c = std::cos(yaw.value());
double s = std::sin(yaw.value());
EXPECT_DOUBLE_EQ(c * x.value() - s * y.value(), rotated.X().value());
EXPECT_DOUBLE_EQ(s * x.value() + c * y.value(), rotated.Y().value());
EXPECT_DOUBLE_EQ(0.0, rotated.Z().value());
EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().X().value());
EXPECT_DOUBLE_EQ(0.0, rotated.Rotation().Y().value());
EXPECT_DOUBLE_EQ(initial.Rotation().Z().value() + rotation.Z().value(),
rotated.Rotation().Z().value());
}
TEST(Pose3dTest, TestTransformByRotations) {
const double kEpsilon = 1E-9;