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

25
wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -15,25 +15,28 @@ using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Pose2dTest, TransformBy) {
const Pose2d initial{1.0, 2.0, Rotation2d::FromDegrees(45.0)};
const Transform2d transform{Translation2d{5.0, 0.0},
Rotation2d::FromDegrees(5.0)};
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 auto transformed = initial + transform;
EXPECT_NEAR(transformed.Translation().X(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Translation().Y(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees(), 50.0, kEpsilon);
EXPECT_NEAR(transformed.Translation().X().to<double>(),
1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Translation().Y().to<double>(),
2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0.0, 0.0, Rotation2d::FromDegrees(45.0)};
const Pose2d final{5.0, 5.0, Rotation2d::FromDegrees(45.0)};
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.Translation().X(), 5.0 * std::sqrt(2.0),
EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
5.0 * std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Translation().Y(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees(), 0.0, kEpsilon);
}

34
wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -17,39 +17,39 @@ using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Rotation2dTest, RadiansToDegrees) {
const Rotation2d one{wpi::math::pi / 3};
const Rotation2d two{wpi::math::pi / 4};
const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
EXPECT_NEAR(one.Degrees(), 60.0, kEpsilon);
EXPECT_NEAR(two.Degrees(), 45.0, kEpsilon);
EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
}
TEST(Rotation2dTest, DegreesToRadians) {
const auto one = Rotation2d::FromDegrees(45.0);
const auto two = Rotation2d::FromDegrees(30.0);
const auto one = Rotation2d(45.0_deg);
const auto two = Rotation2d(30.0_deg);
EXPECT_NEAR(one.Radians(), wpi::math::pi / 4.0, kEpsilon);
EXPECT_NEAR(two.Radians(), wpi::math::pi / 6.0, kEpsilon);
EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByFromZero) {
const Rotation2d zero;
auto sum = zero + Rotation2d::FromDegrees(90.0);
auto sum = zero + Rotation2d(90.0_deg);
EXPECT_NEAR(sum.Radians(), wpi::math::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees(), 90.0, kEpsilon);
EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
}
TEST(Rotation2dTest, RotateByNonZero) {
auto rot = Rotation2d::FromDegrees(90.0);
rot += Rotation2d::FromDegrees(30.0);
auto rot = Rotation2d(90.0_deg);
rot += Rotation2d(30.0_deg);
EXPECT_NEAR(rot.Degrees(), 120.0, kEpsilon);
EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
}
TEST(Rotation2dTest, Minus) {
const auto one = Rotation2d::FromDegrees(70.0);
const auto two = Rotation2d::FromDegrees(30.0);
const auto one = Rotation2d(70.0_deg);
const auto two = Rotation2d(30.0_deg);
EXPECT_NEAR((one - two).Degrees(), 40.0, kEpsilon);
EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
}

52
wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
View File

@@ -15,64 +15,64 @@ using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Translation2dTest, Sum) {
const Translation2d one{1.0, 3.0};
const Translation2d two{2.0, 5.0};
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const auto sum = one + two;
EXPECT_NEAR(sum.X(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y(), 8.0, kEpsilon);
EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
}
TEST(Translation2dTest, Difference) {
const Translation2d one{1.0, 3.0};
const Translation2d two{2.0, 5.0};
const Translation2d one{1.0_m, 3.0_m};
const Translation2d two{2.0_m, 5.0_m};
const auto difference = one - two;
EXPECT_NEAR(difference.X(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y(), -2.0, kEpsilon);
EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
}
TEST(Translation2dTest, RotateBy) {
const Translation2d another{3.0, 0.0};
const auto rotated = another.RotateBy(Rotation2d::FromDegrees(90.0));
const Translation2d another{3.0_m, 0.0_m};
const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
EXPECT_NEAR(rotated.X(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y(), 3.0, kEpsilon);
EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
}
TEST(Translation2dTest, Multiplication) {
const Translation2d original{3.0, 5.0};
const Translation2d original{3.0_m, 5.0_m};
const auto mult = original * 3;
EXPECT_NEAR(mult.X(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y(), 15.0, kEpsilon);
EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
}
TEST(Translation2d, Division) {
const Translation2d original{3.0, 5.0};
const Translation2d original{3.0_m, 5.0_m};
const auto div = original / 2;
EXPECT_NEAR(div.X(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y(), 2.5, kEpsilon);
EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
}
TEST(Translation2dTest, Norm) {
const Translation2d one{3.0, 5.0};
EXPECT_NEAR(one.Norm(), std::hypot(3, 5), kEpsilon);
const Translation2d one{3.0_m, 5.0_m};
EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
}
TEST(Translation2dTest, Distance) {
const Translation2d one{1, 1};
const Translation2d two{6, 6};
EXPECT_NEAR(one.Distance(two), 5 * std::sqrt(2), kEpsilon);
const Translation2d one{1_m, 1_m};
const Translation2d two{6_m, 6_m};
EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
}
TEST(Translation2dTest, UnaryMinus) {
const Translation2d original{-4.5, 7};
const Translation2d original{-4.5_m, 7_m};
const auto inverted = -original;
EXPECT_NEAR(inverted.X(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y(), -7, kEpsilon);
EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
}

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);
}