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] Increase constexpr support in geometry data types (#4231)

Browse Source 
This uses std::is_constant_evaluated() to conditionally use the gcem library for constexpr calculations.
This commit is contained in:
David K Turner
2022-10-31 11:17:00 -05:00
committed by GitHub
parent 1c3c86e9f1
commit 3a5a376465
94 changed files with 6919 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -10,16 +10,6 @@
using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
: m_translation(x, y), m_rotation(std::move(rotation)) {}
Pose2d Pose2d::operator+(const Transform2d& other) const {
return TransformBy(other);
}
Transform2d Pose2d::operator-(const Pose2d& other) const {
const auto pose = this->RelativeTo(other);
return Transform2d{pose.Translation(), pose.Rotation()};
@@ -33,19 +23,6 @@ bool Pose2d::operator!=(const Pose2d& other) const {
return !operator==(other);
}
Pose2d Pose2d::operator*(double scalar) const {
return Pose2d{m_translation * scalar, m_rotation * scalar};
}
Pose2d Pose2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
Pose2d Pose2d::TransformBy(const Transform2d& other) const {
return {m_translation + (other.Translation().RotateBy(m_rotation)),
m_rotation + other.Rotation()};
}
Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
const Transform2d transform{other, *this};
return {transform.Translation(), transform.Rotation()};

55
wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
View File

@@ -12,61 +12,6 @@
using namespace frc;
Rotation2d::Rotation2d(units::radian_t value)
: m_value(value),
m_cos(units::math::cos(value)),
m_sin(units::math::sin(value)) {}
Rotation2d::Rotation2d(units::degree_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);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = units::radian_t{std::atan2(m_sin, m_cos)};
}
Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
return RotateBy(other);
}
Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
return *this + -other;
}
Rotation2d Rotation2d::operator-() const {
return Rotation2d{-m_value};
}
Rotation2d Rotation2d::operator*(double scalar) const {
return Rotation2d{m_value * scalar};
}
Rotation2d Rotation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Rotation2d::operator==(const Rotation2d& other) const {
return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
}
bool Rotation2d::operator!=(const Rotation2d& other) const {
return !operator==(other);
}
Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
json = wpi::json{{"radians", rotation.Radians().value()}};
}

10
wpimath/src/main/native/cpp/geometry/Transform2d.cpp
View File

@@ -18,16 +18,6 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
m_rotation = final.Rotation() - initial.Rotation();
}
Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
: m_translation(std::move(translation)), m_rotation(std::move(rotation)) {}
Transform2d Transform2d::Inverse() const {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
}
Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}

35
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -10,12 +10,6 @@
using namespace frc;
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
: m_x(x), m_y(y) {}
Translation2d::Translation2d(units::meter_t distance, const Rotation2d& angle)
: m_x(distance * angle.Cos()), m_y(distance * angle.Sin()) {}
units::meter_t Translation2d::Distance(const Translation2d& other) const {
return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
}
@@ -24,35 +18,6 @@ units::meter_t Translation2d::Norm() const {
return units::math::hypot(m_x, m_y);
}
Rotation2d Translation2d::Angle() const {
return Rotation2d{m_x.value(), m_y.value()};
}
Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
return {m_x * other.Cos() - m_y * other.Sin(),
m_x * other.Sin() + m_y * other.Cos()};
}
Translation2d Translation2d::operator+(const Translation2d& other) const {
return {X() + other.X(), Y() + other.Y()};
}
Translation2d Translation2d::operator-(const Translation2d& other) const {
return *this + -other;
}
Translation2d Translation2d::operator-() const {
return {-m_x, -m_y};
}
Translation2d Translation2d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y};
}
Translation2d Translation2d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation2d::operator==(const Translation2d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m;

30
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -4,14 +4,8 @@
#include "frc/geometry/Translation3d.h"
#include "units/math.h"
using namespace frc;
Translation3d::Translation3d(units::meter_t x, units::meter_t y,
units::meter_t z)
: m_x(x), m_y(y), m_z(z) {}
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle);
m_x = rectangular.X();
@@ -36,30 +30,6 @@ Translation3d Translation3d::RotateBy(const Rotation3d& other) const {
units::meter_t{qprime.Z()}};
}
Translation2d Translation3d::ToTranslation2d() const {
return Translation2d{m_x, m_y};
}
Translation3d Translation3d::operator+(const Translation3d& other) const {
return {X() + other.X(), Y() + other.Y(), Z() + other.Z()};
}
Translation3d Translation3d::operator-(const Translation3d& other) const {
return *this + -other;
}
Translation3d Translation3d::operator-() const {
return {-m_x, -m_y, -m_z};
}
Translation3d Translation3d::operator*(double scalar) const {
return {scalar * m_x, scalar * m_y, scalar * m_z};
}
Translation3d Translation3d::operator/(double scalar) const {
return *this * (1.0 / scalar);
}
bool Translation3d::operator==(const Translation3d& other) const {
return units::math::abs(m_x - other.m_x) < 1E-9_m &&
units::math::abs(m_y - other.m_y) < 1E-9_m &&