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

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

@@ -9,51 +9,47 @@
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Pose2dTest, TransformBy) {
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, Rotation2d{45_deg}};
const Transform2d transform{Translation2d{5_m, 0_m}, Rotation2d{5_deg}};
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
EXPECT_DOUBLE_EQ(1.0 + 5.0 / std::sqrt(2.0), transformed.X().value());
EXPECT_DOUBLE_EQ(2.0 + 5.0 / std::sqrt(2.0), transformed.Y().value());
EXPECT_DOUBLE_EQ(50.0, transformed.Rotation().Degrees().value());
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45.0_deg}};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
kEpsilon);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value());
EXPECT_NEAR(0.0, finalRelativeToInitial.Y().value(), 1e-9);
EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Degrees().value());
}
TEST(Pose2dTest, Equality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_m, Rotation2d{43_deg}};
EXPECT_TRUE(a == b);
}
TEST(Pose2dTest, Inequality) {
const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
const Pose2d a{0_m, 5_m, Rotation2d{43_deg}};
const Pose2d b{0_m, 5_ft, Rotation2d{43_deg}};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const Pose2d initial{0_m, 0_m, Rotation2d{45_deg}};
const Pose2d final{5_m, 5_m, Rotation2d{45_deg}};
const auto transform = final - initial;
EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value());
EXPECT_NEAR(0.0, transform.Y().value(), 1e-9);
EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Degrees().value());
}