Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

4
wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -14,7 +14,7 @@ using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(translation), m_rotation(rotation) {}
Pose2d::Pose2d(double x, double y, Rotation2d rotation)
Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation(x, y), m_rotation(rotation) {}
Pose2d Pose2d::operator+(const Transform2d& other) const {
@@ -40,7 +40,7 @@ Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
Pose2d Pose2d::Exp(const Twist2d& twist) const {
const auto dx = twist.dx;
const auto dy = twist.dy;
const auto dtheta = twist.dtheta;
const auto dtheta = twist.dtheta.to<double>();
const auto sinTheta = std::sin(dtheta);
const auto cosTheta = std::cos(dtheta);

14
wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -11,8 +11,10 @@
using namespace frc;
Rotation2d::Rotation2d(double value)
: m_value(value), m_cos(std::cos(value)), m_sin(std::sin(value)) {}
Rotation2d::Rotation2d(units::radian_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(double x, double y) {
const auto magnitude = std::hypot(x, y);
@@ -23,11 +25,7 @@ Rotation2d::Rotation2d(double x, double y) {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = std::atan2(m_sin, m_cos);
}
Rotation2d Rotation2d::FromDegrees(double degrees) {
return Rotation2d(Deg2Rad(degrees));
m_value = units::radian_t(std::atan2(m_sin, m_cos));
}
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
@@ -39,7 +37,7 @@ Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
double sin = Cos() * other.Sin() + Sin() * other.Cos();
m_cos = cos;
m_sin = sin;
m_value = std::atan2(m_sin, m_cos);
m_value = units::radian_t(std::atan2(m_sin, m_cos));
return *this;
}

13
wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -7,17 +7,18 @@
#include "frc/geometry/Translation2d.h"
#include <cmath>
using namespace frc;
Translation2d::Translation2d(double x, double y) : m_x(x), m_y(y) {}
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
double Translation2d::Distance(const Translation2d& other) const {
return std::hypot(other.m_x - m_x, other.m_y - m_y);
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
double Translation2d::Norm() const { return std::hypot(m_x, m_y); }
units::meter_t Translation2d::Norm() const {
return units::math::hypot(m_x, m_y);
}
Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),

2
wpilibc/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -40,7 +40,7 @@ class Pose2d {
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
Pose2d(double x, double y, Rotation2d rotation);
Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
/**
* Transforms the pose by the given transformation and returns the new

28
wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -7,6 +7,7 @@
#pragma once
#include <units/units.h>
#include <wpi/math>
namespace frc {
@@ -27,7 +28,7 @@ class Rotation2d {
*
* @param value The value of the angle in radians.
*/
explicit Rotation2d(double value);
explicit Rotation2d(units::radian_t value);
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -38,15 +39,6 @@ class Rotation2d {
*/
Rotation2d(double x, double y);
/**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
*
* @return The rotation object with the desired angle value.
*/
static Rotation2d FromDegrees(double degrees);
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
@@ -124,14 +116,14 @@ class Rotation2d {
*
* @return The radian value of the rotation.
*/
double Radians() const { return m_value; }
units::radian_t Radians() const { return m_value; }
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
double Degrees() const { return Rad2Deg(m_value); }
units::degree_t Degrees() const { return m_value; }
/**
* Returns the cosine of the rotation.
@@ -155,18 +147,8 @@ class Rotation2d {
double Tan() const { return m_sin / m_cos; }
private:
double m_value = 0;
units::radian_t m_value = 0_deg;
double m_cos = 1;
double m_sin = 0;
template <typename T>
static T Rad2Deg(const T& rad) {
return rad * 180.0 / wpi::math::pi;
}
template <typename T>
static T Deg2Rad(const T& deg) {
return deg * wpi::math::pi / 180.0;
}
};
} // namespace frc

16
wpilibc/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <units/units.h>
#include "Rotation2d.h"
namespace frc {
@@ -33,7 +35,7 @@ class Translation2d {
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
Translation2d(double x, double y);
Translation2d(units::meter_t x, units::meter_t y);
/**
* Calculates the distance between two translations in 2d space.
@@ -45,28 +47,28 @@ class Translation2d {
*
* @return The distance between the two translations.
*/
double Distance(const Translation2d& other) const;
units::meter_t Distance(const Translation2d& other) const;
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
double X() const { return m_x; }
units::meter_t X() const { return m_x; }
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
double Y() const { return m_y; }
units::meter_t Y() const { return m_y; }
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
double Norm() const;
units::meter_t Norm() const;
/**
* Applies a rotation to the translation in 2d space.
@@ -190,7 +192,7 @@ class Translation2d {
Translation2d& operator/=(double scalar);
private:
double m_x = 0;
double m_y = 0;
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
};
} // namespace frc

7
wpilibc/src/main/native/include/frc/geometry/Twist2d.h
View File

@@ -6,6 +6,7 @@
/*----------------------------------------------------------------------------*/
#pragma once
#include <units/units.h>
namespace frc {
/**
@@ -19,16 +20,16 @@ struct Twist2d {
/**
* Linear "dx" component
*/
double dx = 0;
units::meter_t dx = 0_m;
/**
* Linear "dy" component
*/
double dy = 0;
units::meter_t dy = 0_m;
/**
* Angular "dtheta" component (radians)
*/
double dtheta = 0;
units::radian_t dtheta = 0_rad;
};
} // namespace frc

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

1
wpiutil/src/main/native/include/units/units.h
View File

@@ -4850,4 +4850,5 @@ using namespace length;
using namespace time;
using namespace velocity;
using namespace acceleration;
using namespace angle;
} // namespace units