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 3D geometry classes (#4175)

Browse Source 
Also clean up 2D geometry documentation.
This commit is contained in:
Tyler Veness
2022-05-06 08:41:23 -07:00
committed by GitHub
parent 708a4bc3bc
commit f20a20f3f1
48 changed files with 4299 additions and 255 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -11,55 +11,57 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const Twist2d straight{5_m, 0_m, 0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0, straightPose.X().value());
EXPECT_DOUBLE_EQ(0.0, straightPose.Y().value());
EXPECT_DOUBLE_EQ(0.0, straightPose.Rotation().Radians().value());
}
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
units::radian_t(wpi::numbers::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
const Twist2d quarterCircle{5_m / 2.0 * wpi::numbers::pi, 0_m,
units::radian_t{wpi::numbers::pi / 2.0}};
const auto quarterCirclePose = Pose2d{}.Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 90.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.X().value());
EXPECT_DOUBLE_EQ(5.0, quarterCirclePose.Y().value());
EXPECT_DOUBLE_EQ(90.0, quarterCirclePose.Rotation().Degrees().value());
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
const Twist2d diagonal{2_m, 2_m, 0_deg};
const auto diagonalPose = Pose2d{}.Exp(diagonal);
EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(2.0, diagonalPose.X().value());
EXPECT_DOUBLE_EQ(2.0, diagonalPose.Y().value());
EXPECT_DOUBLE_EQ(0.0, diagonalPose.Rotation().Degrees().value());
}
TEST(Twist2dTest, Equality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
const Twist2d one{5_m, 1_m, 3_rad};
const Twist2d two{5_m, 1_m, 3_rad};
EXPECT_TRUE(one == two);
}
TEST(Twist2dTest, Inequality) {
const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
const Twist2d one{5_m, 1_m, 3_rad};
const Twist2d two{5_m, 1.2_m, 3_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist2dTest, Pose2dLog) {
const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
const Pose2d start{};
const Pose2d end{5_m, 5_m, Rotation2d{90_deg}};
const Pose2d start;
const auto twist = start.Log(end);
EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
Twist2d expected{units::meter_t{5.0 / 2.0 * wpi::numbers::pi}, 0_m,
units::radian_t{wpi::numbers::pi / 2.0}};
EXPECT_EQ(expected, twist);
// Make sure computed twist gives back original end pose
const auto reapplied = start.Exp(twist);
EXPECT_EQ(end, reapplied);
}