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

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(),