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

wpilibc: Add unit-safety to C++ geometry classes (#1811)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-08-17 01:00:33 -04:00
committed by Peter Johnson
parent c07ac23532
commit 8f386f6bb3
12 changed files with 108 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -7,6 +7,8 @@
#include <cmath>
#include <wpi/math>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
@@ -15,29 +17,30 @@ using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0, 0.0, 0.0};
const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.Translation().X(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Translation().Y(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
}
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5.0 / 2.0 * 3.14159265358979323846, 0,
3.14159265358979323846 / 2.0};
const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
units::radian_t(wpi::math::pi / 2.0)};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.Translation().X(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Translation().Y(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees(), 90.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
kEpsilon);
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0, 2.0, 0.0};
const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
const auto diagonalPose = Pose2d().Exp(diagonal);
EXPECT_NEAR(diagonalPose.Translation().X(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Translation().Y(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees(), 0.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}