From f1ea3ccc16ebf3ce253677589a843061fc352b0b Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 17:05:03 -0700 Subject: [PATCH] [wpimath] Make geometry classes constexpr --- .../native/cpp/geometry/CoordinateAxis.cpp | 41 --- .../native/cpp/geometry/CoordinateSystem.cpp | 76 ----- .../src/main/native/cpp/geometry/Pose2d.cpp | 93 ----- .../src/main/native/cpp/geometry/Pose3d.cpp | 94 ++--- .../main/native/cpp/geometry/Quaternion.cpp | 213 ------------ .../main/native/cpp/geometry/Rotation2d.cpp | 6 - .../main/native/cpp/geometry/Rotation3d.cpp | 232 +------------ .../main/native/cpp/geometry/Transform2d.cpp | 23 -- .../main/native/cpp/geometry/Transform3d.cpp | 39 --- .../native/cpp/geometry/Translation2d.cpp | 29 -- .../native/cpp/geometry/Translation3d.cpp | 2 - .../include/frc/geometry/CoordinateAxis.h | 18 +- .../include/frc/geometry/CoordinateSystem.h | 74 ++-- .../main/native/include/frc/geometry/Pose2d.h | 122 ++++++- .../main/native/include/frc/geometry/Pose3d.h | 77 +++-- .../native/include/frc/geometry/Quaternion.h | 196 +++++++++-- .../native/include/frc/geometry/Rotation3d.h | 320 ++++++++++++++++-- .../native/include/frc/geometry/Transform2d.h | 29 +- .../native/include/frc/geometry/Transform3d.h | 63 +++- .../include/frc/geometry/Translation2d.h | 24 +- .../include/frc/geometry/Translation3d.h | 10 +- .../native/include/frc/geometry/Twist2d.h | 4 +- .../native/include/frc/geometry/Twist3d.h | 5 +- 23 files changed, 842 insertions(+), 948 deletions(-) delete mode 100644 wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp delete mode 100644 wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp delete mode 100644 wpimath/src/main/native/cpp/geometry/Transform2d.cpp delete mode 100644 wpimath/src/main/native/cpp/geometry/Transform3d.cpp diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp deleted file mode 100644 index 1d9c42a2a4d..00000000000 --- a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/geometry/CoordinateAxis.h" - -using namespace frc; - -CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} { - m_axis /= m_axis.norm(); -} - -const CoordinateAxis& CoordinateAxis::N() { - static const CoordinateAxis instance{1.0, 0.0, 0.0}; - return instance; -} - -const CoordinateAxis& CoordinateAxis::S() { - static const CoordinateAxis instance{-1.0, 0.0, 0.0}; - return instance; -} - -const CoordinateAxis& CoordinateAxis::E() { - static const CoordinateAxis instance{0.0, -1.0, 0.0}; - return instance; -} - -const CoordinateAxis& CoordinateAxis::W() { - static const CoordinateAxis instance{0.0, 1.0, 0.0}; - return instance; -} - -const CoordinateAxis& CoordinateAxis::U() { - static const CoordinateAxis instance{0.0, 0.0, 1.0}; - return instance; -} - -const CoordinateAxis& CoordinateAxis::D() { - static const CoordinateAxis instance{0.0, 0.0, -1.0}; - return instance; -} diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp deleted file mode 100644 index 2a8c9db1544..00000000000 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/geometry/CoordinateSystem.h" - -#include -#include -#include - -#include -#include - -using namespace frc; - -CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX, - const CoordinateAxis& positiveY, - const CoordinateAxis& positiveZ) { - // Construct a change of basis matrix from the source coordinate system to the - // NWU coordinate system. Each column vector in the change of basis matrix is - // one of the old basis vectors mapped to its representation in the new basis. - Eigen::Matrix3d R; - R.block<3, 1>(0, 0) = positiveX.m_axis; - R.block<3, 1>(0, 1) = positiveY.m_axis; - R.block<3, 1>(0, 2) = positiveZ.m_axis; - - // The change of basis matrix should be a pure rotation. The Rotation3d - // constructor will verify this by checking for special orthogonality. - m_rotation = Rotation3d{R}; -} - -const CoordinateSystem& CoordinateSystem::NWU() { - static const CoordinateSystem instance{ - CoordinateAxis::N(), CoordinateAxis::W(), CoordinateAxis::U()}; - return instance; -} - -const CoordinateSystem& CoordinateSystem::EDN() { - static const CoordinateSystem instance{ - CoordinateAxis::E(), CoordinateAxis::D(), CoordinateAxis::N()}; - return instance; -} - -const CoordinateSystem& CoordinateSystem::NED() { - static const CoordinateSystem instance{ - CoordinateAxis::N(), CoordinateAxis::E(), CoordinateAxis::D()}; - return instance; -} - -Translation3d CoordinateSystem::Convert(const Translation3d& translation, - const CoordinateSystem& from, - const CoordinateSystem& to) { - return translation.RotateBy(from.m_rotation - to.m_rotation); -} - -Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation, - const CoordinateSystem& from, - const CoordinateSystem& to) { - return rotation.RotateBy(from.m_rotation - to.m_rotation); -} - -Pose3d CoordinateSystem::Convert(const Pose3d& pose, - const CoordinateSystem& from, - const CoordinateSystem& to) { - return Pose3d{Convert(pose.Translation(), from, to), - Convert(pose.Rotation(), from, to)}; -} - -Transform3d CoordinateSystem::Convert(const Transform3d& transform, - const CoordinateSystem& from, - const CoordinateSystem& to) { - const auto coordRot = from.m_rotation - to.m_rotation; - return Transform3d{ - Convert(transform.Translation(), from, to), - (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; -} diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp index 53779d05c83..2a9dc8392f6 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp @@ -4,101 +4,8 @@ #include "frc/geometry/Pose2d.h" -#include - #include -#include "frc/MathUtil.h" - -using namespace frc; - -Transform2d Pose2d::operator-(const Pose2d& other) const { - const auto pose = this->RelativeTo(other); - return Transform2d{pose.Translation(), pose.Rotation()}; -} - -Pose2d Pose2d::RelativeTo(const Pose2d& other) const { - const Transform2d transform{other, *this}; - return {transform.Translation(), transform.Rotation()}; -} - -Pose2d Pose2d::Exp(const Twist2d& twist) const { - const auto dx = twist.dx; - const auto dy = twist.dy; - const auto dtheta = twist.dtheta.value(); - - const auto sinTheta = std::sin(dtheta); - const auto cosTheta = std::cos(dtheta); - - double s, c; - if (std::abs(dtheta) < 1E-9) { - s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; - c = 0.5 * dtheta; - } else { - s = sinTheta / dtheta; - c = (1 - cosTheta) / dtheta; - } - - const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s}, - Rotation2d{cosTheta, sinTheta}}; - - return *this + transform; -} - -Twist2d Pose2d::Log(const Pose2d& end) const { - const auto transform = end.RelativeTo(*this); - const auto dtheta = transform.Rotation().Radians().value(); - const auto halfDtheta = dtheta / 2.0; - - const auto cosMinusOne = transform.Rotation().Cos() - 1; - - double halfThetaByTanOfHalfDtheta; - - if (std::abs(cosMinusOne) < 1E-9) { - halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else { - halfThetaByTanOfHalfDtheta = - -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne; - } - - const Translation2d translationPart = - transform.Translation().RotateBy( - {halfThetaByTanOfHalfDtheta, -halfDtheta}) * - std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta); - - return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}}; -} - -Pose2d Pose2d::Nearest(std::span poses) const { - return *std::min_element( - poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { - auto aDistance = this->Translation().Distance(a.Translation()); - auto bDistance = this->Translation().Distance(b.Translation()); - - // If the distances are equal sort by difference in rotation - if (aDistance == bDistance) { - return std::abs((this->Rotation() - a.Rotation()).Radians().value()) < - std::abs((this->Rotation() - b.Rotation()).Radians().value()); - } - return aDistance < bDistance; - }); -} - -Pose2d Pose2d::Nearest(std::initializer_list poses) const { - return *std::min_element( - poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { - auto aDistance = this->Translation().Distance(a.Translation()); - auto bDistance = this->Translation().Distance(b.Translation()); - - // If the distances are equal sort by difference in rotation - if (aDistance == bDistance) { - return std::abs((this->Rotation() - a.Rotation()).Radians().value()) < - std::abs((this->Rotation() - b.Rotation()).Radians().value()); - } - return aDistance < bDistance; - }); -} - void frc::to_json(wpi::json& json, const Pose2d& pose) { json = wpi::json{{"translation", pose.Translation()}, {"rotation", pose.Rotation()}}; diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 66c5bab910b..80c715f5c97 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -4,9 +4,6 @@ #include "frc/geometry/Pose3d.h" -#include -#include - #include #include @@ -23,72 +20,31 @@ namespace { * @param rotation The rotation vector. * @return The rotation vector as a 3x3 rotation matrix. */ -Eigen::Matrix3d RotationVectorToMatrix(const Eigen::Vector3d& rotation) { +constexpr Eigen::Matrix3d RotationVectorToMatrix( + const Eigen::Vector3d& rotation) { // Given a rotation vector , // [ 0 -c b] // Omega = [ c 0 -a] // [-b a 0] - return Eigen::Matrix3d{{0.0, -rotation(2), rotation(1)}, - {rotation(2), 0.0, -rotation(0)}, - {-rotation(1), rotation(0), 0.0}}; + return Eigen::Matrix3d{{0.0, -rotation.coeff(2), rotation.coeff(1)}, + {rotation.coeff(2), 0.0, -rotation.coeff(0)}, + {-rotation.coeff(1), rotation.coeff(0), 0.0}}; } } // namespace -Pose3d::Pose3d(Translation3d translation, Rotation3d rotation) - : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} - -Pose3d::Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, - Rotation3d rotation) - : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} - -Pose3d::Pose3d(const Pose2d& pose) - : m_translation{pose.X(), pose.Y(), 0_m}, - m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {} - -Pose3d Pose3d::operator+(const Transform3d& other) const { - return TransformBy(other); -} - -Transform3d Pose3d::operator-(const Pose3d& other) const { - const auto pose = this->RelativeTo(other); - return Transform3d{pose.Translation(), pose.Rotation()}; -} - -Pose3d Pose3d::operator*(double scalar) const { - return Pose3d{m_translation * scalar, m_rotation * scalar}; -} - -Pose3d Pose3d::operator/(double scalar) const { - return *this * (1.0 / scalar); -} - -Pose3d Pose3d::RotateBy(const Rotation3d& other) const { - return {m_translation.RotateBy(other), m_rotation.RotateBy(other)}; -} - -Pose3d Pose3d::TransformBy(const Transform3d& other) const { - return {m_translation + (other.Translation().RotateBy(m_rotation)), - other.Rotation() + m_rotation}; -} - -Pose3d Pose3d::RelativeTo(const Pose3d& other) const { - const Transform3d transform{other, *this}; - return {transform.Translation(), transform.Rotation()}; -} - Pose3d Pose3d::Exp(const Twist3d& twist) const { // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf Eigen::Vector3d u{twist.dx.value(), twist.dy.value(), twist.dz.value()}; Eigen::Vector3d rvec{twist.rx.value(), twist.ry.value(), twist.rz.value()}; Eigen::Matrix3d omega = RotationVectorToMatrix(rvec); Eigen::Matrix3d omegaSq = omega * omega; - double theta = rvec.norm(); + double theta = gcem::hypot(rvec.coeff(0), rvec.coeff(1), rvec.coeff(2)); double thetaSq = theta * theta; double A; double B; double C; - if (std::abs(theta) < 1E-7) { + if (gcem::abs(theta) < 1E-7) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! @@ -107,19 +63,19 @@ Pose3d Pose3d::Exp(const Twist3d& twist) const { // A = std::sin(θ)/θ // B = (1 - std::cos(θ)) / θ² // C = (1 - A) / θ² - A = std::sin(theta) / theta; - B = (1 - std::cos(theta)) / thetaSq; + A = gcem::sin(theta) / theta; + B = (1 - gcem::cos(theta)) / thetaSq; C = (1 - A) / thetaSq; } Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + A * omega + B * omegaSq; Eigen::Matrix3d V = Eigen::Matrix3d::Identity() + B * omega + C * omegaSq; - auto translation_component = V * u; + Eigen::Vector3d translation_component = V * u; const Transform3d transform{ - Translation3d{units::meter_t{translation_component(0)}, - units::meter_t{translation_component(1)}, - units::meter_t{translation_component(2)}}, + Translation3d{units::meter_t{translation_component.coeff(0)}, + units::meter_t{translation_component.coeff(1)}, + units::meter_t{translation_component.coeff(2)}}, Rotation3d{R}}; return *this + transform; @@ -136,11 +92,11 @@ Twist3d Pose3d::Log(const Pose3d& end) const { Eigen::Matrix3d omega = RotationVectorToMatrix(rvec); Eigen::Matrix3d omegaSq = omega * omega; - double theta = rvec.norm(); + double theta = gcem::hypot(rvec.coeff(0), rvec.coeff(1), rvec.coeff(2)); double thetaSq = theta * theta; double C; - if (std::abs(theta) < 1E-7) { + if (gcem::abs(theta) < 1E-7) { // Taylor Expansions around θ = 0 // A = 1/1! - θ²/3! + θ⁴/5! // B = 1/2! - θ²/4! + θ⁴/6! @@ -157,8 +113,8 @@ Twist3d Pose3d::Log(const Pose3d& end) const { // A = std::sin(θ)/θ // B = (1 - std::cos(θ)) / θ² // C = (1 - A/(2*B)) / θ² - double A = std::sin(theta) / theta; - double B = (1 - std::cos(theta)) / thetaSq; + double A = gcem::sin(theta) / theta; + double B = (1 - gcem::cos(theta)) / thetaSq; C = (1 - A / (2 * B)) / thetaSq; } @@ -167,16 +123,12 @@ Twist3d Pose3d::Log(const Pose3d& end) const { Eigen::Vector3d translation_component = V_inv * u; - return Twist3d{units::meter_t{translation_component(0)}, - units::meter_t{translation_component(1)}, - units::meter_t{translation_component(2)}, - units::radian_t{rvec(0)}, - units::radian_t{rvec(1)}, - units::radian_t{rvec(2)}}; -} - -Pose2d Pose3d::ToPose2d() const { - return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()}; + return Twist3d{units::meter_t{translation_component.coeff(0)}, + units::meter_t{translation_component.coeff(1)}, + units::meter_t{translation_component.coeff(2)}, + units::radian_t{rvec.coeff(0)}, + units::radian_t{rvec.coeff(1)}, + units::radian_t{rvec.coeff(2)}}; } void frc::to_json(wpi::json& json, const Pose3d& pose) { diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 136f48db0c6..709f226022d 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -4,221 +4,8 @@ #include "frc/geometry/Quaternion.h" -#include - #include -using namespace frc; - -Quaternion::Quaternion(double w, double x, double y, double z) - : m_r{w}, m_v{x, y, z} {} - -Quaternion Quaternion::operator+(const Quaternion& other) const { - return Quaternion{ - m_r + other.m_r, - m_v(0) + other.m_v(0), - m_v(1) + other.m_v(1), - m_v(2) + other.m_v(2), - }; -} - -Quaternion Quaternion::operator-(const Quaternion& other) const { - return Quaternion{ - m_r - other.m_r, - m_v(0) - other.m_v(0), - m_v(1) - other.m_v(1), - m_v(2) - other.m_v(2), - }; -} - -Quaternion Quaternion::operator*(const double other) const { - return Quaternion{ - m_r * other, - m_v(0) * other, - m_v(1) * other, - m_v(2) * other, - }; -} - -Quaternion Quaternion::operator/(const double other) const { - return Quaternion{ - m_r / other, - m_v(0) / other, - m_v(1) / other, - m_v(2) / other, - }; -} - -Quaternion Quaternion::operator*(const Quaternion& other) const { - // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts - const auto& r1 = m_r; - const auto& v1 = m_v; - const auto& r2 = other.m_r; - const auto& v2 = other.m_v; - - // v₁ x v₂ - Eigen::Vector3d cross{v1(1) * v2(2) - v2(1) * v1(2), - v2(0) * v1(2) - v1(0) * v2(2), - v1(0) * v2(1) - v2(0) * v1(1)}; - - // v = r₁v₂ + r₂v₁ + v₁ x v₂ - Eigen::Vector3d v = r1 * v2 + r2 * v1 + cross; - - return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂ - r1 * r2 - v1.dot(v2), - // v = r₁v₂ + r₂v₁ + v₁ x v₂ - v(0), v(1), v(2)}; -} - -bool Quaternion::operator==(const Quaternion& other) const { - return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 && - std::abs(Norm() - other.Norm()) < 1e-9; -} - -Quaternion Quaternion::Conjugate() const { - return Quaternion{W(), -X(), -Y(), -Z()}; -} - -double Quaternion::Dot(const Quaternion& other) const { - return W() * other.W() + m_v.dot(other.m_v); -} - -Quaternion Quaternion::Inverse() const { - double norm = Norm(); - return Conjugate() / (norm * norm); -} - -double Quaternion::Norm() const { - return std::sqrt(Dot(*this)); -} - -Quaternion Quaternion::Normalize() const { - double norm = Norm(); - if (norm == 0.0) { - return Quaternion{}; - } else { - return Quaternion{W(), X(), Y(), Z()} / norm; - } -} - -Quaternion Quaternion::Pow(const double other) const { - return (Log() * other).Exp(); -} - -Quaternion Quaternion::Exp(const Quaternion& other) const { - return other.Exp() * *this; -} - -Quaternion Quaternion::Exp() const { - double scalar = std::exp(m_r); - - double axial_magnitude = m_v.norm(); - double cosine = std::cos(axial_magnitude); - - double axial_scalar; - - if (axial_magnitude < 1e-9) { - // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶) - double axial_magnitude_sq = axial_magnitude * axial_magnitude; - double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq; - axial_scalar = - 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0; - } else { - axial_scalar = std::sin(axial_magnitude) / axial_magnitude; - } - - return Quaternion(cosine * scalar, X() * axial_scalar * scalar, - Y() * axial_scalar * scalar, Z() * axial_scalar * scalar); -} - -Quaternion Quaternion::Log(const Quaternion& other) const { - return (other * Inverse()).Log(); -} - -Quaternion Quaternion::Log() const { - double norm = Norm(); - double scalar = std::log(norm); - - double v_norm = m_v.norm(); - - double s_norm = W() / norm; - - if (std::abs(s_norm + 1) < 1e-9) { - return Quaternion{scalar, -std::numbers::pi, 0, 0}; - } - - double v_scalar; - - if (v_norm < 1e-9) { - // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x - - // y^2/3*x^3 + O(y^4) - v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W()); - } else { - v_scalar = std::atan2(v_norm, W()) / v_norm; - } - - return Quaternion{scalar, v_scalar * m_v(0), v_scalar * m_v(1), - v_scalar * m_v(2)}; -} - -double Quaternion::W() const { - return m_r; -} - -double Quaternion::X() const { - return m_v(0); -} - -double Quaternion::Y() const { - return m_v(1); -} - -double Quaternion::Z() const { - return m_v(2); -} - -Eigen::Vector3d Quaternion::ToRotationVector() const { - // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with - // Sound State Representation through Encapsulation of Manifolds" - // - // https://arxiv.org/pdf/1107.1119.pdf - double norm = m_v.norm(); - - if (norm < 1e-9) { - return (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())) * m_v; - } else { - if (W() < 0.0) { - return 2.0 * std::atan2(-norm, -W()) / norm * m_v; - } else { - return 2.0 * std::atan2(norm, W()) / norm * m_v; - } - } -} - -Quaternion Quaternion::FromRotationVector(const Eigen::Vector3d& rvec) { - // 𝑣⃗ = θ * v̂ - // v̂ = 𝑣⃗ / θ - - // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂ - // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗ - - double theta = rvec.norm(); - double cos = std::cos(theta / 2); - - double axial_scalar; - - if (theta < 1e-9) { - // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 + - // O(θ⁴) - axial_scalar = 1.0 / 2.0 - theta * theta / 48.0; - } else { - axial_scalar = std::sin(theta / 2) / theta; - } - - return Quaternion{cos, axial_scalar * rvec(0), axial_scalar * rvec(1), - axial_scalar * rvec(2)}; -} - void frc::to_json(wpi::json& json, const Quaternion& quaternion) { json = wpi::json{{"W", quaternion.W()}, {"X", quaternion.X()}, diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp index 921e1f81a09..71c64b6cd83 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp @@ -4,14 +4,8 @@ #include "frc/geometry/Rotation2d.h" -#include - #include -#include "units/math.h" - -using namespace frc; - void frc::to_json(wpi::json& json, const Rotation2d& rotation) { json = wpi::json{{"radians", rotation.Radians().value()}}; } diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index b8c4063eb73..f1deb70c62d 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -4,119 +4,19 @@ #include "frc/geometry/Rotation3d.h" -#include -#include -#include - -#include -#include #include #include -#include "frc/fmt/Eigen.h" -#include "units/math.h" -#include "wpimath/MathShared.h" - using namespace frc; -Rotation3d::Rotation3d(const Quaternion& q) { - m_q = q.Normalize(); -} - -Rotation3d::Rotation3d(units::radian_t roll, units::radian_t pitch, - units::radian_t yaw) { - // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion - double cr = units::math::cos(roll * 0.5); - double sr = units::math::sin(roll * 0.5); - - double cp = units::math::cos(pitch * 0.5); - double sp = units::math::sin(pitch * 0.5); - - double cy = units::math::cos(yaw * 0.5); - double sy = units::math::sin(yaw * 0.5); - - m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, - cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy}; -} - -Rotation3d::Rotation3d(const Eigen::Vector3d& rvec) - : Rotation3d{rvec, units::radian_t{rvec.norm()}} {} - -Rotation3d::Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) { - double norm = axis.norm(); - if (norm == 0.0) { - return; - } - - // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition - Eigen::Vector3d v = axis / norm * units::math::sin(angle / 2.0); - m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)}; -} - -Rotation3d::Rotation3d(const Eigen::Matrix3d& rotationMatrix) { - const auto& R = rotationMatrix; - - // Require that the rotation matrix is special orthogonal. This is true if the - // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). - if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) { - std::string msg = - fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R); - - wpi::math::MathSharedStore::ReportError(msg); - throw std::domain_error(msg); - } - if (std::abs(R.determinant() - 1.0) > 1e-9) { - std::string msg = fmt::format( - "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n", - R); - - wpi::math::MathSharedStore::ReportError(msg); - throw std::domain_error(msg); - } - - // Turn rotation matrix into a quaternion - // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ - double trace = R(0, 0) + R(1, 1) + R(2, 2); - double w; - double x; - double y; - double z; - - if (trace > 0.0) { - double s = 0.5 / std::sqrt(trace + 1.0); - w = 0.25 / s; - x = (R(2, 1) - R(1, 2)) * s; - y = (R(0, 2) - R(2, 0)) * s; - z = (R(1, 0) - R(0, 1)) * s; - } else { - if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) { - double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)); - w = (R(2, 1) - R(1, 2)) / s; - x = 0.25 * s; - y = (R(0, 1) + R(1, 0)) / s; - z = (R(0, 2) + R(2, 0)) / s; - } else if (R(1, 1) > R(2, 2)) { - double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)); - w = (R(0, 2) - R(2, 0)) / s; - x = (R(0, 1) + R(1, 0)) / s; - y = 0.25 * s; - z = (R(1, 2) + R(2, 1)) / s; - } else { - double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)); - w = (R(1, 0) - R(0, 1)) / s; - x = (R(0, 2) + R(2, 0)) / s; - y = (R(1, 2) + R(2, 1)) / s; - z = 0.25 * s; - } - } - - m_q = Quaternion{w, x, y, z}; -} - Rotation3d::Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final) { - double dot = initial.dot(final); - double normProduct = initial.norm() * final.norm(); + double dot = initial.coeff(0) * final.coeff(0) + + initial.coeff(1) * final.coeff(1) + + initial.coeff(2) * final.coeff(2); + double normProduct = + gcem::hypot(initial.coeff(0), initial.coeff(1), initial.coeff(2)) * + gcem::hypot(final.coeff(0), final.coeff(1), final.coeff(2)); double dotNorm = dot / normProduct; if (dotNorm > 1.0 - 1E-9) { @@ -136,125 +36,21 @@ Rotation3d::Rotation3d(const Eigen::Vector3d& initial, // // For x, y, and z, we use the second column of Q because the first is // parallel instead of orthogonal. The third column would also work. - m_q = Quaternion{0.0, Q(0, 1), Q(1, 1), Q(2, 1)}; + m_q = Quaternion{0.0, Q.coeff(0, 1), Q.coeff(1, 1), Q.coeff(2, 1)}; } else { // initial x final - Eigen::Vector3d axis{initial(1) * final(2) - final(1) * initial(2), - final(0) * initial(2) - initial(0) * final(2), - initial(0) * final(1) - final(0) * initial(1)}; + Eigen::Vector3d axis{ + initial.coeff(1) * final.coeff(2) - final.coeff(1) * initial.coeff(2), + final.coeff(0) * initial.coeff(2) - initial.coeff(0) * final.coeff(2), + initial.coeff(0) * final.coeff(1) - final.coeff(0) * initial.coeff(1)}; // https://stackoverflow.com/a/11741520 - m_q = Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize(); - } -} - -Rotation3d Rotation3d::operator+(const Rotation3d& other) const { - return RotateBy(other); -} - -Rotation3d Rotation3d::operator-(const Rotation3d& other) const { - return *this + -other; -} - -Rotation3d Rotation3d::operator-() const { - return Rotation3d{m_q.Inverse()}; -} - -Rotation3d Rotation3d::operator*(double scalar) const { - // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp - if (m_q.W() >= 0.0) { - return Rotation3d{{m_q.X(), m_q.Y(), m_q.Z()}, - 2.0 * units::radian_t{scalar * std::acos(m_q.W())}}; - } else { - return Rotation3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}, - 2.0 * units::radian_t{scalar * std::acos(-m_q.W())}}; - } -} - -Rotation3d Rotation3d::operator/(double scalar) const { - return *this * (1.0 / scalar); -} - -bool Rotation3d::operator==(const Rotation3d& other) const { - return std::abs(std::abs(m_q.Dot(other.m_q)) - - m_q.Norm() * other.m_q.Norm()) < 1e-9; -} - -Rotation3d Rotation3d::RotateBy(const Rotation3d& other) const { - return Rotation3d{other.m_q * m_q}; -} - -const Quaternion& Rotation3d::GetQuaternion() const { - return m_q; -} - -units::radian_t Rotation3d::X() const { - double w = m_q.W(); - double x = m_q.X(); - double y = m_q.Y(); - double z = m_q.Z(); - - // wpimath/algorithms.md - double cxcy = 1.0 - 2.0 * (x * x + y * y); - double sxcy = 2.0 * (w * x + y * z); - double cy_sq = cxcy * cxcy + sxcy * sxcy; - if (cy_sq > 1e-20) { - return units::radian_t{std::atan2(sxcy, cxcy)}; - } else { - return 0_rad; - } -} - -units::radian_t Rotation3d::Y() const { - double w = m_q.W(); - double x = m_q.X(); - double y = m_q.Y(); - double z = m_q.Z(); - - // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion - double ratio = 2.0 * (w * y - z * x); - if (std::abs(ratio) >= 1.0) { - return units::radian_t{std::copysign(std::numbers::pi / 2.0, ratio)}; - } else { - return units::radian_t{std::asin(ratio)}; - } -} - -units::radian_t Rotation3d::Z() const { - double w = m_q.W(); - double x = m_q.X(); - double y = m_q.Y(); - double z = m_q.Z(); - - // wpimath/algorithms.md - double cycz = 1.0 - 2.0 * (y * y + z * z); - double cysz = 2.0 * (w * z + x * y); - double cy_sq = cycz * cycz + cysz * cysz; - if (cy_sq > 1e-20) { - return units::radian_t{std::atan2(cysz, cycz)}; - } else { - return units::radian_t{std::atan2(2.0 * w * z, w * w - z * z)}; + m_q = Quaternion{normProduct + dot, axis.coeff(0), axis.coeff(1), + axis.coeff(2)} + .Normalize(); } } -Eigen::Vector3d Rotation3d::Axis() const { - double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z()); - if (norm == 0.0) { - return {0.0, 0.0, 0.0}; - } else { - return {m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}; - } -} - -units::radian_t Rotation3d::Angle() const { - double norm = std::hypot(m_q.X(), m_q.Y(), m_q.Z()); - return units::radian_t{2.0 * std::atan2(norm, m_q.W())}; -} - -Rotation2d Rotation3d::ToRotation2d() const { - return Rotation2d{Z()}; -} - void frc::to_json(wpi::json& json, const Rotation3d& rotation) { json = wpi::json{{"quaternion", rotation.GetQuaternion()}}; } diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp deleted file mode 100644 index 25b05907ed9..00000000000 --- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/geometry/Transform2d.h" - -#include "frc/geometry/Pose2d.h" - -using namespace frc; - -Transform2d::Transform2d(Pose2d initial, Pose2d final) { - // 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). - m_translation = (final.Translation() - initial.Translation()) - .RotateBy(-initial.Rotation()); - - m_rotation = final.Rotation() - initial.Rotation(); -} - -Transform2d Transform2d::operator+(const Transform2d& other) const { - return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; -} diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp deleted file mode 100644 index 34dad68f277..00000000000 --- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "frc/geometry/Transform3d.h" - -#include - -#include "frc/geometry/Pose3d.h" - -using namespace frc; - -Transform3d::Transform3d(Pose3d initial, Pose3d final) { - // 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). - m_translation = (final.Translation() - initial.Translation()) - .RotateBy(-initial.Rotation()); - - m_rotation = final.Rotation() - initial.Rotation(); -} - -Transform3d::Transform3d(Translation3d translation, Rotation3d rotation) - : m_translation{std::move(translation)}, m_rotation{std::move(rotation)} {} - -Transform3d::Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, - Rotation3d rotation) - : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} - -Transform3d Transform3d::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 Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; -} - -Transform3d Transform3d::operator+(const Transform3d& other) const { - return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; -} diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp index 3d7d5d7d0bb..70ba92cbe83 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp @@ -4,37 +4,8 @@ #include "frc/geometry/Translation2d.h" -#include - #include -#include "units/math.h" - -using namespace frc; - -Translation2d::Translation2d(const Eigen::Vector2d& vector) - : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} - -units::meter_t Translation2d::Norm() const { - return units::math::hypot(m_x, m_y); -} - -Translation2d Translation2d::Nearest( - std::span translations) const { - return *std::min_element(translations.begin(), translations.end(), - [this](Translation2d a, Translation2d b) { - return this->Distance(a) < this->Distance(b); - }); -} - -Translation2d Translation2d::Nearest( - std::initializer_list translations) const { - return *std::min_element(translations.begin(), translations.end(), - [this](Translation2d a, Translation2d b) { - return this->Distance(a) < this->Distance(b); - }); -} - void frc::to_json(wpi::json& json, const Translation2d& translation) { json = wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}}; diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 1609f434e8b..3169528475b 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -6,8 +6,6 @@ #include -using namespace frc; - void frc::to_json(wpi::json& json, const Translation3d& translation) { json = wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}, diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h index 2b471b7d5df..0b15080706b 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include "frc/geometry/Pose3d.h" @@ -26,7 +27,10 @@ class WPILIB_DLLEXPORT CoordinateAxis { * @param y The y component. * @param z The z component. */ - CoordinateAxis(double x, double y, double z); + constexpr CoordinateAxis(double x, double y, double z) { + double norm = gcem::sqrt(x * x + y * y + z * z); + m_axis = Eigen::Vector3d{{x / norm, y / norm, z / norm}}; + } CoordinateAxis(const CoordinateAxis&) = default; CoordinateAxis& operator=(const CoordinateAxis&) = default; @@ -37,32 +41,32 @@ class WPILIB_DLLEXPORT CoordinateAxis { /** * Returns a coordinate axis corresponding to +X in the NWU coordinate system. */ - static const CoordinateAxis& N(); + static constexpr CoordinateAxis N() { return CoordinateAxis{1.0, 0.0, 0.0}; } /** * Returns a coordinate axis corresponding to -X in the NWU coordinate system. */ - static const CoordinateAxis& S(); + static constexpr CoordinateAxis S() { return CoordinateAxis{-1.0, 0.0, 0.0}; } /** * Returns a coordinate axis corresponding to -Y in the NWU coordinate system. */ - static const CoordinateAxis& E(); + static constexpr CoordinateAxis E() { return CoordinateAxis{0.0, -1.0, 0.0}; } /** * Returns a coordinate axis corresponding to +Y in the NWU coordinate system. */ - static const CoordinateAxis& W(); + static constexpr CoordinateAxis W() { return CoordinateAxis{0.0, 1.0, 0.0}; } /** * Returns a coordinate axis corresponding to +Z in the NWU coordinate system. */ - static const CoordinateAxis& U(); + static constexpr CoordinateAxis U() { return CoordinateAxis{0.0, 0.0, 1.0}; } /** * Returns a coordinate axis corresponding to -Z in the NWU coordinate system. */ - static const CoordinateAxis& D(); + static constexpr CoordinateAxis D() { return CoordinateAxis{0.0, 0.0, -1.0}; } private: friend class CoordinateSystem; diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h index f5e71f2b287..90a6a363d0d 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h @@ -28,35 +28,54 @@ class WPILIB_DLLEXPORT CoordinateSystem { * @param positiveZ The cardinal direction of the positive z-axis. * @throws std::domain_error if the coordinate system isn't special orthogonal */ - CoordinateSystem(const CoordinateAxis& positiveX, - const CoordinateAxis& positiveY, - const CoordinateAxis& positiveZ); + constexpr CoordinateSystem(const CoordinateAxis& positiveX, + const CoordinateAxis& positiveY, + const CoordinateAxis& positiveZ) { + // Construct a change of basis matrix from the source coordinate system to + // the NWU coordinate system. Each column vector in the change of basis + // matrix is one of the old basis vectors mapped to its representation in + // the new basis. + Eigen::Matrix3d R{{positiveX.m_axis.coeff(0), positiveY.m_axis.coeff(0), + positiveZ.m_axis.coeff(0)}, + {positiveX.m_axis.coeff(1), positiveY.m_axis.coeff(1), + positiveZ.m_axis.coeff(1)}, + {positiveX.m_axis.coeff(2), positiveY.m_axis.coeff(2), + positiveZ.m_axis.coeff(2)}}; - CoordinateSystem(const CoordinateSystem&) = default; - CoordinateSystem& operator=(const CoordinateSystem&) = default; - CoordinateSystem(CoordinateSystem&&) = default; - CoordinateSystem& operator=(CoordinateSystem&&) = default; + // The change of basis matrix should be a pure rotation. The Rotation3d + // constructor will verify this by checking for special orthogonality. + m_rotation = Rotation3d{R}; + } /** * Returns an instance of the North-West-Up (NWU) coordinate system. * * The +X axis is north, the +Y axis is west, and the +Z axis is up. */ - static const CoordinateSystem& NWU(); + constexpr static CoordinateSystem NWU() { + return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(), + CoordinateAxis::U()}; + } /** * Returns an instance of the East-Down-North (EDN) coordinate system. * * The +X axis is east, the +Y axis is down, and the +Z axis is north. */ - static const CoordinateSystem& EDN(); + constexpr static CoordinateSystem EDN() { + return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(), + CoordinateAxis::N()}; + } /** * Returns an instance of the NED coordinate system. * * The +X axis is north, the +Y axis is east, and the +Z axis is down. */ - static const CoordinateSystem& NED(); + constexpr static CoordinateSystem NED() { + return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(), + CoordinateAxis::D()}; + } /** * Converts the given translation from one coordinate system to another. @@ -66,9 +85,11 @@ class WPILIB_DLLEXPORT CoordinateSystem { * @param to The coordinate system to which to convert. * @return The given translation in the desired coordinate system. */ - static Translation3d Convert(const Translation3d& translation, - const CoordinateSystem& from, - const CoordinateSystem& to); + constexpr static Translation3d Convert(const Translation3d& translation, + const CoordinateSystem& from, + const CoordinateSystem& to) { + return translation.RotateBy(from.m_rotation - to.m_rotation); + } /** * Converts the given rotation from one coordinate system to another. @@ -78,9 +99,11 @@ class WPILIB_DLLEXPORT CoordinateSystem { * @param to The coordinate system to which to convert. * @return The given rotation in the desired coordinate system. */ - static Rotation3d Convert(const Rotation3d& rotation, - const CoordinateSystem& from, - const CoordinateSystem& to); + constexpr static Rotation3d Convert(const Rotation3d& rotation, + const CoordinateSystem& from, + const CoordinateSystem& to) { + return rotation.RotateBy(from.m_rotation - to.m_rotation); + } /** * Converts the given pose from one coordinate system to another. @@ -90,8 +113,12 @@ class WPILIB_DLLEXPORT CoordinateSystem { * @param to The coordinate system to which to convert. * @return The given pose in the desired coordinate system. */ - static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from, - const CoordinateSystem& to); + constexpr static Pose3d Convert(const Pose3d& pose, + const CoordinateSystem& from, + const CoordinateSystem& to) { + return Pose3d{Convert(pose.Translation(), from, to), + Convert(pose.Rotation(), from, to)}; + } /** * Converts the given transform from one coordinate system to another. @@ -101,9 +128,14 @@ class WPILIB_DLLEXPORT CoordinateSystem { * @param to The coordinate system to which to convert. * @return The given transform in the desired coordinate system. */ - static Transform3d Convert(const Transform3d& transform, - const CoordinateSystem& from, - const CoordinateSystem& to); + constexpr static Transform3d Convert(const Transform3d& transform, + const CoordinateSystem& from, + const CoordinateSystem& to) { + const auto coordRot = from.m_rotation - to.m_rotation; + return Transform3d{ + Convert(transform.Translation(), from, to), + (-coordRot).RotateBy(transform.Rotation().RotateBy(coordRot))}; + } private: // Rotation from this coordinate system to NWU coordinate system diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 3a267e8cf08..e105495fca9 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -4,21 +4,24 @@ #pragma once +#include #include #include #include +#include #include #include #include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Transform2d.h" #include "frc/geometry/Translation2d.h" #include "frc/geometry/Twist2d.h" #include "units/length.h" namespace frc { +class Transform2d; + /** * Represents a 2D pose containing translational and rotational elements. */ @@ -74,12 +77,12 @@ class WPILIB_DLLEXPORT Pose2d { * @param other The initial pose of the transformation. * @return The transform that maps the other pose to the current pose. */ - Transform2d operator-(const Pose2d& other) const; + constexpr Transform2d operator-(const Pose2d& other) const; /** * Checks equality between this Pose2d and another object. */ - bool operator==(const Pose2d&) const = default; + constexpr bool operator==(const Pose2d&) const = default; /** * Returns the underlying translation. @@ -150,10 +153,7 @@ class WPILIB_DLLEXPORT Pose2d { * * @return The transformed pose. */ - constexpr Pose2d TransformBy(const Transform2d& other) const { - return {m_translation + (other.Translation().RotateBy(m_rotation)), - other.Rotation() + m_rotation}; - } + constexpr Pose2d TransformBy(const Transform2d& other) const; /** * Returns the current pose relative to the given pose. @@ -167,7 +167,7 @@ class WPILIB_DLLEXPORT Pose2d { * * @return The current pose relative to the new origin pose. */ - Pose2d RelativeTo(const Pose2d& other) const; + constexpr Pose2d RelativeTo(const Pose2d& other) const; /** * Obtain a new Pose2d from a (constant curvature) velocity. @@ -190,7 +190,7 @@ class WPILIB_DLLEXPORT Pose2d { * * @return The new pose of the robot. */ - Pose2d Exp(const Twist2d& twist) const; + constexpr Pose2d Exp(const Twist2d& twist) const; /** * Returns a Twist2d that maps this pose to the end pose. If c is the output @@ -200,21 +200,51 @@ class WPILIB_DLLEXPORT Pose2d { * * @return The twist that maps this to end. */ - Twist2d Log(const Pose2d& end) const; + constexpr Twist2d Log(const Pose2d& end) const; /** * Returns the nearest Pose2d from a collection of poses * @param poses The collection of poses. * @return The nearest Pose2d from the collection. */ - Pose2d Nearest(std::span poses) const; + constexpr Pose2d Nearest(std::span poses) const { + return *std::min_element( + poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { + auto aDistance = this->Translation().Distance(a.Translation()); + auto bDistance = this->Translation().Distance(b.Translation()); + + // If the distances are equal sort by difference in rotation + if (aDistance == bDistance) { + return gcem::abs( + (this->Rotation() - a.Rotation()).Radians().value()) < + gcem::abs( + (this->Rotation() - b.Rotation()).Radians().value()); + } + return aDistance < bDistance; + }); + } /** * Returns the nearest Pose2d from a collection of poses * @param poses The collection of poses. * @return The nearest Pose2d from the collection. */ - Pose2d Nearest(std::initializer_list poses) const; + constexpr Pose2d Nearest(std::initializer_list poses) const { + return *std::min_element( + poses.begin(), poses.end(), [this](const Pose2d& a, const Pose2d& b) { + auto aDistance = this->Translation().Distance(a.Translation()); + auto bDistance = this->Translation().Distance(b.Translation()); + + // If the distances are equal sort by difference in rotation + if (aDistance == bDistance) { + return gcem::abs( + (this->Rotation() - a.Rotation()).Radians().value()) < + gcem::abs( + (this->Rotation() - b.Rotation()).Radians().value()); + } + return aDistance < bDistance; + }); + } private: Translation2d m_translation; @@ -233,3 +263,71 @@ void from_json(const wpi::json& json, Pose2d& pose); #include "frc/geometry/proto/Pose2dProto.h" #endif #include "frc/geometry/struct/Pose2dStruct.h" + +#include "frc/geometry/Transform2d.h" + +namespace frc { + +constexpr Transform2d Pose2d::operator-(const Pose2d& other) const { + const auto pose = this->RelativeTo(other); + return Transform2d{pose.Translation(), pose.Rotation()}; +} + +constexpr Pose2d Pose2d::TransformBy(const frc::Transform2d& other) const { + return {m_translation + (other.Translation().RotateBy(m_rotation)), + other.Rotation() + m_rotation}; +} + +constexpr Pose2d Pose2d::RelativeTo(const Pose2d& other) const { + const Transform2d transform{other, *this}; + return {transform.Translation(), transform.Rotation()}; +} + +constexpr Pose2d Pose2d::Exp(const Twist2d& twist) const { + const auto dx = twist.dx; + const auto dy = twist.dy; + const auto dtheta = twist.dtheta.value(); + + const auto sinTheta = gcem::sin(dtheta); + const auto cosTheta = gcem::cos(dtheta); + + double s, c; + if (gcem::abs(dtheta) < 1E-9) { + s = 1.0 - 1.0 / 6.0 * dtheta * dtheta; + c = 0.5 * dtheta; + } else { + s = sinTheta / dtheta; + c = (1 - cosTheta) / dtheta; + } + + const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s}, + Rotation2d{cosTheta, sinTheta}}; + + return *this + transform; +} + +constexpr Twist2d Pose2d::Log(const Pose2d& end) const { + const auto transform = end.RelativeTo(*this); + const auto dtheta = transform.Rotation().Radians().value(); + const auto halfDtheta = dtheta / 2.0; + + const auto cosMinusOne = transform.Rotation().Cos() - 1; + + double halfThetaByTanOfHalfDtheta; + + if (gcem::abs(cosMinusOne) < 1E-9) { + halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halfThetaByTanOfHalfDtheta = + -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne; + } + + const Translation2d translationPart = + transform.Translation().RotateBy( + {halfThetaByTanOfHalfDtheta, -halfDtheta}) * + gcem::hypot(halfThetaByTanOfHalfDtheta, halfDtheta); + + return {translationPart.X(), translationPart.Y(), units::radian_t{dtheta}}; +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index f077a610109..69ea36d709a 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,17 +4,20 @@ #pragma once +#include + #include #include #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation3d.h" -#include "frc/geometry/Transform3d.h" #include "frc/geometry/Translation3d.h" #include "frc/geometry/Twist3d.h" namespace frc { +class Transform3d; + /** * Represents a 3D pose containing translational and rotational elements. */ @@ -31,7 +34,9 @@ class WPILIB_DLLEXPORT Pose3d { * @param translation The translational component of the pose. * @param rotation The rotational component of the pose. */ - Pose3d(Translation3d translation, Rotation3d rotation); + constexpr Pose3d(Translation3d translation, Rotation3d rotation) + : m_translation{std::move(translation)}, + m_rotation{std::move(rotation)} {} /** * Constructs a pose with x, y, and z translations instead of a separate @@ -42,15 +47,18 @@ class WPILIB_DLLEXPORT Pose3d { * @param z The z component of the translational component of the pose. * @param rotation The rotational component of the pose. */ - Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, - Rotation3d rotation); + constexpr Pose3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation) + : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * * @param pose The 2D pose. */ - explicit Pose3d(const Pose2d& pose); + constexpr explicit Pose3d(const Pose2d& pose) + : m_translation{pose.X(), pose.Y(), 0_m}, + m_rotation{0_rad, 0_rad, pose.Rotation().Radians()} {} /** * Transforms the pose by the given transformation and returns the new @@ -62,7 +70,9 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The transformed pose. */ - Pose3d operator+(const Transform3d& other) const; + constexpr Pose3d operator+(const Transform3d& other) const { + return TransformBy(other); + } /** * Returns the Transform3d that maps the one pose to another. @@ -70,47 +80,47 @@ class WPILIB_DLLEXPORT Pose3d { * @param other The initial pose of the transformation. * @return The transform that maps the other pose to the current pose. */ - Transform3d operator-(const Pose3d& other) const; + constexpr Transform3d operator-(const Pose3d& other) const; /** * Checks equality between this Pose3d and another object. */ - bool operator==(const Pose3d&) const = default; + constexpr bool operator==(const Pose3d&) const = default; /** * Returns the underlying translation. * * @return Reference to the translational component of the pose. */ - const Translation3d& Translation() const { return m_translation; } + constexpr const Translation3d& Translation() const { return m_translation; } /** * Returns the X component of the pose's translation. * * @return The x component of the pose's translation. */ - units::meter_t X() const { return m_translation.X(); } + constexpr units::meter_t X() const { return m_translation.X(); } /** * Returns the Y component of the pose's translation. * * @return The y component of the pose's translation. */ - units::meter_t Y() const { return m_translation.Y(); } + constexpr units::meter_t Y() const { return m_translation.Y(); } /** * Returns the Z component of the pose's translation. * * @return The z component of the pose's translation. */ - units::meter_t Z() const { return m_translation.Z(); } + constexpr units::meter_t Z() const { return m_translation.Z(); } /** * Returns the underlying rotation. * * @return Reference to the rotational component of the pose. */ - const Rotation3d& Rotation() const { return m_rotation; } + constexpr const Rotation3d& Rotation() const { return m_rotation; } /** * Multiplies the current pose by a scalar. @@ -119,7 +129,9 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The new scaled Pose2d. */ - Pose3d operator*(double scalar) const; + constexpr Pose3d operator*(double scalar) const { + return Pose3d{m_translation * scalar, m_rotation * scalar}; + } /** * Divides the current pose by a scalar. @@ -128,7 +140,9 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The new scaled Pose2d. */ - Pose3d operator/(double scalar) const; + constexpr Pose3d operator/(double scalar) const { + return *this * (1.0 / scalar); + } /** * Rotates the pose around the origin and returns the new pose. @@ -138,7 +152,9 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The rotated pose. */ - Pose3d RotateBy(const Rotation3d& other) const; + constexpr Pose3d RotateBy(const Rotation3d& other) const { + return {m_translation.RotateBy(other), m_rotation.RotateBy(other)}; + } /** * Transforms the pose by the given transformation and returns the new @@ -150,7 +166,7 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The transformed pose. */ - Pose3d TransformBy(const Transform3d& other) const; + constexpr Pose3d TransformBy(const Transform3d& other) const; /** * Returns the current pose relative to the given pose. @@ -164,7 +180,7 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The current pose relative to the new origin pose. */ - Pose3d RelativeTo(const Pose3d& other) const; + constexpr Pose3d RelativeTo(const Pose3d& other) const; /** * Obtain a new Pose3d from a (constant curvature) velocity. @@ -200,7 +216,9 @@ class WPILIB_DLLEXPORT Pose3d { /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. */ - Pose2d ToPose2d() const; + constexpr Pose2d ToPose2d() const { + return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()}; + } private: Translation3d m_translation; @@ -219,3 +237,24 @@ void from_json(const wpi::json& json, Pose3d& pose); #include "frc/geometry/proto/Pose3dProto.h" #endif #include "frc/geometry/struct/Pose3dStruct.h" + +#include "frc/geometry/Transform3d.h" + +namespace frc { + +constexpr Transform3d Pose3d::operator-(const Pose3d& other) const { + const auto pose = this->RelativeTo(other); + return Transform3d{pose.Translation(), pose.Rotation()}; +} + +constexpr Pose3d Pose3d::TransformBy(const Transform3d& other) const { + return {m_translation + (other.Translation().RotateBy(m_rotation)), + other.Rotation() + m_rotation}; +} + +constexpr Pose3d Pose3d::RelativeTo(const Pose3d& other) const { + const Transform3d transform{other, *this}; + return {transform.Translation(), transform.Rotation()}; +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 4a4927632b8..e1a5cf4075f 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -4,7 +4,10 @@ #pragma once +#include + #include +#include #include #include @@ -18,7 +21,7 @@ class WPILIB_DLLEXPORT Quaternion { /** * Constructs a quaternion with a default angle of 0 degrees. */ - Quaternion() = default; + constexpr Quaternion() = default; /** * Constructs a quaternion with the given components. @@ -28,42 +31,72 @@ class WPILIB_DLLEXPORT Quaternion { * @param y Y component of the quaternion. * @param z Z component of the quaternion. */ - Quaternion(double w, double x, double y, double z); + constexpr Quaternion(double w, double x, double y, double z) + : m_w{w}, m_x{x}, m_y{y}, m_z{z} {} /** * Adds with another quaternion. * * @param other the other quaternion */ - Quaternion operator+(const Quaternion& other) const; + constexpr Quaternion operator+(const Quaternion& other) const { + return Quaternion{m_w + other.m_w, m_x + other.m_x, m_y + other.m_y, + m_z + other.m_z}; + } /** * Subtracts another quaternion. * * @param other the other quaternion */ - Quaternion operator-(const Quaternion& other) const; + constexpr Quaternion operator-(const Quaternion& other) const { + return Quaternion{m_w - other.m_w, m_x - other.m_x, m_y - other.m_y, + m_z - other.m_z}; + } /** * Multiples with a scalar value. * * @param other the scalar value */ - Quaternion operator*(const double other) const; + constexpr Quaternion operator*(double other) const { + return Quaternion{m_w * other, m_x * other, m_y * other, m_z * other}; + } /** * Divides by a scalar value. * * @param other the scalar value */ - Quaternion operator/(const double other) const; + constexpr Quaternion operator/(double other) const { + return Quaternion{m_w / other, m_x / other, m_y / other, m_z / other}; + } /** * Multiply with another quaternion. * * @param other The other quaternion. */ - Quaternion operator*(const Quaternion& other) const; + constexpr Quaternion operator*(const Quaternion& other) const { + // https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts + const auto& r1 = m_w; + const auto& r2 = other.m_w; + + // v₁ ⋅ v₂ + double dot = m_x * other.m_x + m_y * other.m_y + m_z * other.m_z; + + // v₁ x v₂ + double cross_x = m_y * other.m_z - other.m_y * m_z; + double cross_y = other.m_x * m_z - m_x * other.m_z; + double cross_z = m_x * other.m_y - other.m_x * m_y; + + return Quaternion{// r = r₁r₂ − v₁ ⋅ v₂ + r1 * r2 - dot, + // v = r₁v₂ + r₂v₁ + v₁ x v₂ + r1 * other.m_x + r2 * m_x + cross_x, + r1 * other.m_y + r2 * m_y + cross_y, + r1 * other.m_z + r2 * m_z + cross_z}; + } /** * Checks equality between this Quaternion and another object. @@ -71,46 +104,66 @@ class WPILIB_DLLEXPORT Quaternion { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const Quaternion& other) const; + constexpr bool operator==(const Quaternion& other) const { + return gcem::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 && + gcem::abs(Norm() - other.Norm()) < 1e-9; + } /** * Returns the elementwise product of two quaternions. */ - double Dot(const Quaternion& other) const; + constexpr double Dot(const Quaternion& other) const { + return W() * other.W() + X() * other.X() + Y() * other.Y() + + Z() * other.Z(); + } /** * Returns the conjugate of the quaternion. */ - Quaternion Conjugate() const; + constexpr Quaternion Conjugate() const { + return Quaternion{W(), -X(), -Y(), -Z()}; + } /** * Returns the inverse of the quaternion. */ - Quaternion Inverse() const; + constexpr Quaternion Inverse() const { + double norm = Norm(); + return Conjugate() / (norm * norm); + } /** * Normalizes the quaternion. */ - Quaternion Normalize() const; + constexpr Quaternion Normalize() const { + double norm = Norm(); + if (norm == 0.0) { + return Quaternion{}; + } else { + return Quaternion{W(), X(), Y(), Z()} / norm; + } + } /** * Calculates the L2 norm of the quaternion. */ - double Norm() const; + constexpr double Norm() const { return gcem::sqrt(Dot(*this)); } /** * Calculates this quaternion raised to a power. * * @param t the power to raise this quaternion to. */ - Quaternion Pow(const double t) const; + constexpr Quaternion Pow(double t) const { return (Log() * t).Exp(); } /** * Matrix exponential of a quaternion. * * @param other the "Twist" that will be applied to this quaternion. */ - Quaternion Exp(const Quaternion& other) const; + constexpr Quaternion Exp(const Quaternion& other) const { + return other.Exp() * *this; + } /** * Matrix exponential of a quaternion. @@ -120,14 +173,36 @@ class WPILIB_DLLEXPORT Quaternion { * If this quaternion is in 𝖘𝖔(3) and you are looking for an element of * SO(3), use FromRotationVector */ - Quaternion Exp() const; + constexpr Quaternion Exp() const { + double scalar = gcem::exp(m_w); + + double axial_magnitude = gcem::hypot(m_x, m_y, m_z); + double cosine = gcem::cos(axial_magnitude); + + double axial_scalar; + + if (axial_magnitude < 1e-9) { + // Taylor series of sin(x)/x near x=0: 1 − x²/6 + x⁴/120 + O(n⁶) + double axial_magnitude_sq = axial_magnitude * axial_magnitude; + double axial_magnitude_sq_sq = axial_magnitude_sq * axial_magnitude_sq; + axial_scalar = + 1.0 - axial_magnitude_sq / 6.0 + axial_magnitude_sq_sq / 120.0; + } else { + axial_scalar = gcem::sin(axial_magnitude) / axial_magnitude; + } + + return Quaternion(cosine * scalar, X() * axial_scalar * scalar, + Y() * axial_scalar * scalar, Z() * axial_scalar * scalar); + } /** * Log operator of a quaternion. * * @param other The quaternion to map this quaternion onto */ - Quaternion Log(const Quaternion& other) const; + constexpr Quaternion Log(const Quaternion& other) const { + return (other * Inverse()).Log(); + } /** * Log operator of a quaternion. @@ -137,34 +212,78 @@ class WPILIB_DLLEXPORT Quaternion { * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), * use ToRotationVector */ - Quaternion Log() const; + constexpr Quaternion Log() const { + double norm = Norm(); + double scalar = gcem::log(norm); + + double v_norm = gcem::hypot(m_x, m_y, m_z); + + double s_norm = W() / norm; + + if (gcem::abs(s_norm + 1) < 1e-9) { + return Quaternion{scalar, -std::numbers::pi, 0, 0}; + } + + double v_scalar; + + if (v_norm < 1e-9) { + // Taylor series expansion of atan2(y / x) / y around y = 0 = 1/x - + // y^2/3*x^3 + O(y^4) + v_scalar = 1.0 / W() - 1.0 / 3.0 * v_norm * v_norm / (W() * W() * W()); + } else { + v_scalar = gcem::atan2(v_norm, W()) / v_norm; + } + + return Quaternion{scalar, v_scalar * m_x, v_scalar * m_y, v_scalar * m_z}; + } /** * Returns W component of the quaternion. */ - double W() const; + constexpr double W() const { return m_w; } /** * Returns X component of the quaternion. */ - double X() const; + constexpr double X() const { return m_x; } /** * Returns Y component of the quaternion. */ - double Y() const; + constexpr double Y() const { return m_y; } /** * Returns Z component of the quaternion. */ - double Z() const; + constexpr double Z() const { return m_z; } /** * Returns the rotation vector representation of this quaternion. * * This is also the log operator of SO(3). */ - Eigen::Vector3d ToRotationVector() const; + constexpr Eigen::Vector3d ToRotationVector() const { + // See equation (31) in "Integrating Generic Sensor Fusion Algorithms with + // Sound State Representation through Encapsulation of Manifolds" + // + // https://arxiv.org/pdf/1107.1119.pdf + + // double norm = m_v.norm(); + double norm = gcem::sqrt(m_x * m_x + m_y * m_y + m_z * m_z); + + double scalar; + if (norm < 1e-9) { + scalar = (2.0 / W() - 2.0 / 3.0 * norm * norm / (W() * W() * W())); + } else { + if (W() < 0.0) { + scalar = 2.0 * gcem::atan2(-norm, -W()) / norm; + } else { + scalar = 2.0 * gcem::atan2(norm, W()) / norm; + } + } + + return Eigen::Vector3d{{scalar * m_x, scalar * m_y, scalar * m_z}}; + } /** * Returns the quaternion representation of this rotation vector. @@ -173,14 +292,39 @@ class WPILIB_DLLEXPORT Quaternion { * * source: wpimath/algorithms.md */ - static Quaternion FromRotationVector(const Eigen::Vector3d& rvec); + constexpr static Quaternion FromRotationVector(const Eigen::Vector3d& rvec) { + // 𝑣⃗ = θ * v̂ + // v̂ = 𝑣⃗ / θ + + // 𝑞 = std::cos(θ/2) + std::sin(θ/2) * v̂ + // 𝑞 = std::cos(θ/2) + std::sin(θ/2) / θ * 𝑣⃗ + + double theta = gcem::hypot(rvec.coeff(0), rvec.coeff(1), rvec.coeff(2)); + double cos = gcem::cos(theta / 2); + + double axial_scalar; + + if (theta < 1e-9) { + // taylor series expansion of sin(θ/2) / θ around θ = 0 = 1/2 - θ²/48 + + // O(θ⁴) + axial_scalar = 1.0 / 2.0 - theta * theta / 48.0; + } else { + axial_scalar = gcem::sin(theta / 2) / theta; + } + + return Quaternion{cos, axial_scalar * rvec.coeff(0), + axial_scalar * rvec.coeff(1), + axial_scalar * rvec.coeff(2)}; + } private: // Scalar r in versor form - double m_r = 1.0; + double m_w = 1.0; // Vector v in versor form - Eigen::Vector3d m_v{0.0, 0.0, 0.0}; + double m_x = 0.0; + double m_y = 0.0; + double m_z = 0.0; }; WPILIB_DLLEXPORT diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 632f2ba29f9..13a331d7eb8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -4,13 +4,22 @@ #pragma once +#include +#include + #include +#include +#include +#include #include #include +#include "frc/fmt/Eigen.h" #include "frc/geometry/Quaternion.h" #include "frc/geometry/Rotation2d.h" #include "units/angle.h" +#include "units/math.h" +#include "wpimath/MathShared.h" namespace frc { @@ -22,14 +31,14 @@ class WPILIB_DLLEXPORT Rotation3d { /** * Constructs a Rotation3d representing no rotation. */ - Rotation3d() = default; + constexpr Rotation3d() = default; /** * Constructs a Rotation3d from a quaternion. * * @param q The quaternion. */ - explicit Rotation3d(const Quaternion& q); + constexpr explicit Rotation3d(const Quaternion& q) { m_q = q.Normalize(); } /** * Constructs a Rotation3d from extrinsic roll, pitch, and yaw. @@ -45,7 +54,21 @@ class WPILIB_DLLEXPORT Rotation3d { * @param pitch The counterclockwise rotation angle around the Y axis (pitch). * @param yaw The counterclockwise rotation angle around the Z axis (yaw). */ - Rotation3d(units::radian_t roll, units::radian_t pitch, units::radian_t yaw); + constexpr Rotation3d(units::radian_t roll, units::radian_t pitch, + units::radian_t yaw) { + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion + double cr = units::math::cos(roll * 0.5); + double sr = units::math::sin(roll * 0.5); + + double cp = units::math::cos(pitch * 0.5); + double sp = units::math::sin(pitch * 0.5); + + double cy = units::math::cos(yaw * 0.5); + double sy = units::math::sin(yaw * 0.5); + + m_q = Quaternion{cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy}; + } /** * Constructs a Rotation3d with the given axis-angle representation. The axis @@ -54,7 +77,22 @@ class WPILIB_DLLEXPORT Rotation3d { * @param axis The rotation axis. * @param angle The rotation around the axis. */ - Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle); + constexpr Rotation3d(const Eigen::Vector3d& axis, units::radian_t angle) { + // double norm = axis.norm(); + double norm = gcem::sqrt(axis.coeff(0) * axis.coeff(0) + + axis.coeff(1) * axis.coeff(1) + + axis.coeff(2) * axis.coeff(2)); + if (norm == 0.0) { + return; + } + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Definition + Eigen::Vector3d v{{axis.coeff(0) / norm * units::math::sin(angle / 2.0), + axis.coeff(1) / norm * units::math::sin(angle / 2.0), + axis.coeff(2) / norm * units::math::sin(angle / 2.0)}}; + m_q = Quaternion{units::math::cos(angle / 2.0), v.coeff(0), v.coeff(1), + v.coeff(2)}; + } /** * Constructs a Rotation3d with the given rotation vector representation. This @@ -63,7 +101,12 @@ class WPILIB_DLLEXPORT Rotation3d { * * @param rvec The rotation vector. */ - explicit Rotation3d(const Eigen::Vector3d& rvec); + constexpr explicit Rotation3d(const Eigen::Vector3d& rvec) + // : Rotation3d{rvec, units::radian_t{rvec.norm()}} {} + : Rotation3d{ + rvec, units::radian_t{gcem::sqrt(rvec.coeff(0) * rvec.coeff(0) + + rvec.coeff(1) * rvec.coeff(1) + + rvec.coeff(2) * rvec.coeff(2))}} {} /** * Constructs a Rotation3d from a rotation matrix. @@ -71,7 +114,171 @@ class WPILIB_DLLEXPORT Rotation3d { * @param rotationMatrix The rotation matrix. * @throws std::domain_error if the rotation matrix isn't special orthogonal. */ - explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix); + constexpr explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix) { + const auto& R = rotationMatrix; + + // Require that the rotation matrix is special orthogonal. This is true if + // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + if (std::is_constant_evaluated()) { + auto transpose = + [](const Eigen::Matrix& A) + -> Eigen::Matrix { + Eigen::Matrix result; + + for (int row = 0; row < A.rows(); ++row) { + for (int col = 0; col < A.cols(); ++col) { + result.coeffRef(col, row) = A.coeff(row, col); + } + } + + return result; + }; + + auto mult = [](const Eigen::Matrix3d& lhs, + const Eigen::Matrix3d& rhs) -> Eigen::Matrix3d { + Eigen::Matrix3d result; + + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + double sum = 0.0; + for (int k = 0; k < lhs.cols(); ++k) { + sum += lhs(i, k) * rhs(k, j); + } + result.coeffRef(i, j) = sum; + } + } + + return result; + }; + + auto minus = []( + const Eigen::Matrix& A, + const Eigen::Matrix& B) + -> Eigen::Matrix { + Eigen::Matrix result; + + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + result.coeffRef(row, col) = A.coeff(row, col) - B.coeff(row, col); + } + } + + return result; + }; + + auto I = []() -> Eigen::Matrix { + Eigen::Matrix result; + + for (int row = 0; row < Rows; ++row) { + for (int col = 0; col < Cols; ++col) { + if (row == col) { + result.coeffRef(row, row) = 1.0; + } else { + result.coeffRef(row, col) = 0.0; + } + } + } + + return result; + }; + + auto norm = []( + const Eigen::Matrix& A) -> double { + double sum = 0.0; + + for (int row = 0; row < Rows; ++row) { + for (int col = 0; col < Cols; ++col) { + sum += A.coeff(row, col) * A.coeff(row, col); + } + } + + return gcem::sqrt(sum); + }; + + auto determinant = [](const Eigen::Matrix3d& A) -> double { + // |a b c| + // |d e f| = aei + bfg + cgh - ceg - bdi - afh + // |g h i| + double a = A.coeff(0, 0); + double b = A.coeff(0, 1); + double c = A.coeff(0, 2); + double d = A.coeff(1, 0); + double e = A.coeff(1, 1); + double f = A.coeff(1, 2); + double g = A.coeff(2, 0); + double h = A.coeff(2, 1); + double i = A.coeff(2, 2); + return a * e * i + b * f * g + c * g * h - c * e * g - b * d * i - + a * f * h; + }; + + if (norm(minus(mult(R, transpose(R)), I.operator()<3, 3>())) > 1e-9) { + throw std::domain_error("Rotation matrix isn't orthogonal"); + } + if (gcem::abs(determinant(R) - 1.0) > 1e-9) { + throw std::domain_error( + "Rotation matrix is orthogonal but not special orthogonal"); + } + } else { + if ((R * R.transpose() - Eigen::Matrix3d::Identity()).norm() > 1e-9) { + std::string msg = + fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R); + + wpi::math::MathSharedStore::ReportError(msg); + throw std::domain_error(msg); + } + if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + std::string msg = fmt::format( + "Rotation matrix is orthogonal but not special orthogonal\n\nR " + "=\n{}\n", + R); + + wpi::math::MathSharedStore::ReportError(msg); + throw std::domain_error(msg); + } + } + + // Turn rotation matrix into a quaternion + // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + double trace = R.coeff(0, 0) + R.coeff(1, 1) + R.coeff(2, 2); + double w; + double x; + double y; + double z; + + if (trace > 0.0) { + double s = 0.5 / gcem::sqrt(trace + 1.0); + w = 0.25 / s; + x = (R.coeff(2, 1) - R.coeff(1, 2)) * s; + y = (R.coeff(0, 2) - R.coeff(2, 0)) * s; + z = (R.coeff(1, 0) - R.coeff(0, 1)) * s; + } else { + if (R.coeff(0, 0) > R.coeff(1, 1) && R.coeff(0, 0) > R.coeff(2, 2)) { + double s = 2.0 * gcem::sqrt(1.0 + R.coeff(0, 0) - R.coeff(1, 1) - + R.coeff(2, 2)); + w = (R.coeff(2, 1) - R.coeff(1, 2)) / s; + x = 0.25 * s; + y = (R.coeff(0, 1) + R.coeff(1, 0)) / s; + z = (R.coeff(0, 2) + R.coeff(2, 0)) / s; + } else if (R.coeff(1, 1) > R.coeff(2, 2)) { + double s = 2.0 * gcem::sqrt(1.0 + R.coeff(1, 1) - R.coeff(0, 0) - + R.coeff(2, 2)); + w = (R.coeff(0, 2) - R.coeff(2, 0)) / s; + x = (R.coeff(0, 1) + R.coeff(1, 0)) / s; + y = 0.25 * s; + z = (R.coeff(1, 2) + R.coeff(2, 1)) / s; + } else { + double s = 2.0 * gcem::sqrt(1.0 + R.coeff(2, 2) - R.coeff(0, 0) - + R.coeff(1, 1)); + w = (R.coeff(1, 0) - R.coeff(0, 1)) / s; + x = (R.coeff(0, 2) + R.coeff(2, 0)) / s; + y = (R.coeff(1, 2) + R.coeff(2, 1)) / s; + z = 0.25 * s; + } + } + + m_q = Quaternion{w, x, y, z}; + } /** * Constructs a Rotation3d that rotates the initial vector onto the final @@ -92,7 +299,9 @@ class WPILIB_DLLEXPORT Rotation3d { * * @return The sum of the two rotations. */ - Rotation3d operator+(const Rotation3d& other) const; + constexpr Rotation3d operator+(const Rotation3d& other) const { + return RotateBy(other); + } /** * Subtracts the new rotation from the current rotation and returns the new @@ -102,14 +311,16 @@ class WPILIB_DLLEXPORT Rotation3d { * * @return The difference between the two rotations. */ - Rotation3d operator-(const Rotation3d& other) const; + constexpr Rotation3d operator-(const Rotation3d& other) const { + return *this + -other; + } /** * Takes the inverse of the current rotation. * * @return The inverse of the current rotation. */ - Rotation3d operator-() const; + constexpr Rotation3d operator-() const { return Rotation3d{m_q.Inverse()}; } /** * Multiplies the current rotation by a scalar. @@ -118,7 +329,16 @@ class WPILIB_DLLEXPORT Rotation3d { * * @return The new scaled Rotation3d. */ - Rotation3d operator*(double scalar) const; + constexpr Rotation3d operator*(double scalar) const { + // https://en.wikipedia.org/wiki/Slerp#Quaternion_Slerp + if (m_q.W() >= 0.0) { + return Rotation3d{Eigen::Vector3d{{m_q.X(), m_q.Y(), m_q.Z()}}, + 2.0 * units::radian_t{scalar * gcem::acos(m_q.W())}}; + } else { + return Rotation3d{Eigen::Vector3d{{-m_q.X(), -m_q.Y(), -m_q.Z()}}, + 2.0 * units::radian_t{scalar * gcem::acos(-m_q.W())}}; + } + } /** * Divides the current rotation by a scalar. @@ -127,12 +347,17 @@ class WPILIB_DLLEXPORT Rotation3d { * * @return The new scaled Rotation3d. */ - Rotation3d operator/(double scalar) const; + constexpr Rotation3d operator/(double scalar) const { + return *this * (1.0 / scalar); + } /** * Checks equality between this Rotation3d and another object. */ - bool operator==(const Rotation3d&) const; + constexpr bool operator==(const Rotation3d& other) const { + return gcem::abs(gcem::abs(m_q.Dot(other.m_q)) - + m_q.Norm() * other.m_q.Norm()) < 1e-9; + } /** * Adds the new rotation to the current rotation. The other rotation is @@ -145,43 +370,98 @@ class WPILIB_DLLEXPORT Rotation3d { * * @return The new rotated Rotation3d. */ - Rotation3d RotateBy(const Rotation3d& other) const; + constexpr Rotation3d RotateBy(const Rotation3d& other) const { + return Rotation3d{other.m_q * m_q}; + } /** * Returns the quaternion representation of the Rotation3d. */ - const Quaternion& GetQuaternion() const; + constexpr const Quaternion& GetQuaternion() const { return m_q; } /** * Returns the counterclockwise rotation angle around the X axis (roll). */ - units::radian_t X() const; + constexpr units::radian_t X() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // wpimath/algorithms.md + double cxcy = 1.0 - 2.0 * (x * x + y * y); + double sxcy = 2.0 * (w * x + y * z); + double cy_sq = cxcy * cxcy + sxcy * sxcy; + if (cy_sq > 1e-20) { + return units::radian_t{gcem::atan2(sxcy, cxcy)}; + } else { + return 0_rad; + } + } /** * Returns the counterclockwise rotation angle around the Y axis (pitch). */ - units::radian_t Y() const; + constexpr units::radian_t Y() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_(in_3-2-1_sequence)_conversion + double ratio = 2.0 * (w * y - z * x); + if (gcem::abs(ratio) >= 1.0) { + return units::radian_t{gcem::copysign(std::numbers::pi / 2.0, ratio)}; + } else { + return units::radian_t{gcem::asin(ratio)}; + } + } /** * Returns the counterclockwise rotation angle around the Z axis (yaw). */ - units::radian_t Z() const; + constexpr units::radian_t Z() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // wpimath/algorithms.md + double cycz = 1.0 - 2.0 * (y * y + z * z); + double cysz = 2.0 * (w * z + x * y); + double cy_sq = cycz * cycz + cysz * cysz; + if (cy_sq > 1e-20) { + return units::radian_t{gcem::atan2(cysz, cycz)}; + } else { + return units::radian_t{gcem::atan2(2.0 * w * z, w * w - z * z)}; + } + } /** * Returns the axis in the axis-angle representation of this rotation. */ - Eigen::Vector3d Axis() const; + constexpr Eigen::Vector3d Axis() const { + double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z()); + if (norm == 0.0) { + return Eigen::Vector3d{{0.0, 0.0, 0.0}}; + } else { + return Eigen::Vector3d{{m_q.X() / norm, m_q.Y() / norm, m_q.Z() / norm}}; + } + } /** * Returns the angle in the axis-angle representation of this rotation. */ - units::radian_t Angle() const; + constexpr units::radian_t Angle() const { + double norm = gcem::hypot(m_q.X(), m_q.Y(), m_q.Z()); + return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())}; + } /** * Returns a Rotation2d representing this Rotation3d projected into the X-Y * plane. */ - Rotation2d ToRotation2d() const; + constexpr Rotation2d ToRotation2d() const { return Rotation2d{Z()}; } private: Quaternion m_q; diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index 35271056edb..c8581414aa3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -13,7 +13,7 @@ namespace frc { -class WPILIB_DLLEXPORT Pose2d; +class Pose2d; /** * Represents a transformation for a Pose2d in the pose's frame. @@ -26,7 +26,7 @@ class WPILIB_DLLEXPORT Transform2d { * @param initial The initial pose for the transformation. * @param final The final pose for the transformation. */ - Transform2d(Pose2d initial, Pose2d final); + constexpr Transform2d(const Pose2d& initial, const Pose2d& final); /** * Constructs a transform with the given translation and rotation components. @@ -121,17 +121,38 @@ class WPILIB_DLLEXPORT Transform2d { * @param other The transform to compose with this one. * @return The composition of the two transformations. */ - Transform2d operator+(const Transform2d& other) const; + constexpr Transform2d operator+(const Transform2d& other) const; /** * Checks equality between this Transform2d and another object. */ - bool operator==(const Transform2d&) const = default; + constexpr bool operator==(const Transform2d&) const = default; private: Translation2d m_translation; Rotation2d m_rotation; }; + +} // namespace frc + +#include "frc/geometry/Pose2d.h" + +namespace frc { + +constexpr Transform2d::Transform2d(const Pose2d& initial, const Pose2d& final) { + // 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). + m_translation = (final.Translation() - initial.Translation()) + .RotateBy(-initial.Rotation()); + + m_rotation = final.Rotation() - initial.Rotation(); +} + +constexpr Transform2d Transform2d::operator+(const Transform2d& other) const { + return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)}; +} + } // namespace frc #ifndef NO_PROTOBUF diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 9f42d185812..5a46e8044d3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -4,13 +4,15 @@ #pragma once +#include + #include #include "frc/geometry/Translation3d.h" namespace frc { -class WPILIB_DLLEXPORT Pose3d; +class Pose3d; /** * Represents a transformation for a Pose3d in the pose's frame. @@ -23,7 +25,7 @@ class WPILIB_DLLEXPORT Transform3d { * @param initial The initial pose for the transformation. * @param final The final pose for the transformation. */ - Transform3d(Pose3d initial, Pose3d final); + constexpr Transform3d(const Pose3d& initial, const Pose3d& final); /** * Constructs a transform with the given translation and rotation components. @@ -31,7 +33,9 @@ class WPILIB_DLLEXPORT Transform3d { * @param translation Translational component of the transform. * @param rotation Rotational component of the transform. */ - Transform3d(Translation3d translation, Rotation3d rotation); + constexpr Transform3d(Translation3d translation, Rotation3d rotation) + : m_translation{std::move(translation)}, + m_rotation{std::move(rotation)} {} /** * Constructs a transform with x, y, and z translations instead of a separate @@ -42,8 +46,9 @@ class WPILIB_DLLEXPORT Transform3d { * @param z The z component of the translational component of the transform. * @param rotation The rotational component of the transform. */ - Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, - Rotation3d rotation); + constexpr Transform3d(units::meter_t x, units::meter_t y, units::meter_t z, + Rotation3d rotation) + : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} /** * Constructs the identity transform -- maps an initial pose to itself. @@ -55,42 +60,47 @@ class WPILIB_DLLEXPORT Transform3d { * * @return Reference to the translational component of the transform. */ - const Translation3d& Translation() const { return m_translation; } + constexpr const Translation3d& Translation() const { return m_translation; } /** * Returns the X component of the transformation's translation. * * @return The x component of the transformation's translation. */ - units::meter_t X() const { return m_translation.X(); } + constexpr units::meter_t X() const { return m_translation.X(); } /** * Returns the Y component of the transformation's translation. * * @return The y component of the transformation's translation. */ - units::meter_t Y() const { return m_translation.Y(); } + constexpr units::meter_t Y() const { return m_translation.Y(); } /** * Returns the Z component of the transformation's translation. * * @return The z component of the transformation's translation. */ - units::meter_t Z() const { return m_translation.Z(); } + constexpr units::meter_t Z() const { return m_translation.Z(); } /** * Returns the rotational component of the transformation. * * @return Reference to the rotational component of the transform. */ - const Rotation3d& Rotation() const { return m_rotation; } + constexpr const Rotation3d& Rotation() const { return m_rotation; } /** * Invert the transformation. This is useful for undoing a transformation. * * @return The inverted transformation. */ - Transform3d Inverse() const; + constexpr Transform3d 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 Transform3d{(-Translation()).RotateBy(-Rotation()), -Rotation()}; + } /** * Multiplies the transform by the scalar. @@ -98,7 +108,7 @@ class WPILIB_DLLEXPORT Transform3d { * @param scalar The scalar. * @return The scaled Transform3d. */ - Transform3d operator*(double scalar) const { + constexpr Transform3d operator*(double scalar) const { return Transform3d(m_translation * scalar, m_rotation * scalar); } @@ -108,7 +118,9 @@ class WPILIB_DLLEXPORT Transform3d { * @param scalar The scalar. * @return The scaled Transform3d. */ - Transform3d operator/(double scalar) const { return *this * (1.0 / scalar); } + constexpr Transform3d operator/(double scalar) const { + return *this * (1.0 / scalar); + } /** * Composes two transformations. The second transform is applied relative to @@ -117,17 +129,38 @@ class WPILIB_DLLEXPORT Transform3d { * @param other The transform to compose with this one. * @return The composition of the two transformations. */ - Transform3d operator+(const Transform3d& other) const; + constexpr Transform3d operator+(const Transform3d& other) const; /** * Checks equality between this Transform3d and another object. */ - bool operator==(const Transform3d&) const = default; + constexpr bool operator==(const Transform3d&) const = default; private: Translation3d m_translation; Rotation3d m_rotation; }; + +} // namespace frc + +#include "frc/geometry/Pose3d.h" + +namespace frc { + +constexpr Transform3d::Transform3d(const Pose3d& initial, const Pose3d& final) { + // 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). + m_translation = (final.Translation() - initial.Translation()) + .RotateBy(-initial.Rotation()); + + m_rotation = final.Rotation() - initial.Rotation(); +} + +constexpr Transform3d Transform3d::operator+(const Transform3d& other) const { + return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)}; +} + } // namespace frc #ifndef NO_PROTOBUF diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index 2a82a69c57f..697e474ac55 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -58,7 +59,9 @@ class WPILIB_DLLEXPORT Translation2d { * * @param vector The translation vector to represent. */ - explicit Translation2d(const Eigen::Vector2d& vector); + constexpr explicit Translation2d(const Eigen::Vector2d& vector) + : m_x{units::meter_t{vector.coeff(0)}}, + m_y{units::meter_t{vector.coeff(1)}} {} /** * Calculates the distance between two translations in 2D space. @@ -101,7 +104,7 @@ class WPILIB_DLLEXPORT Translation2d { * * @return The norm of the translation. */ - units::meter_t Norm() const; + constexpr units::meter_t Norm() const { return units::math::hypot(m_x, m_y); } /** * Returns the angle this translation forms with the positive X axis. @@ -234,15 +237,26 @@ class WPILIB_DLLEXPORT Translation2d { * @param translations The collection of translations. * @return The nearest Translation2d from the collection. */ - Translation2d Nearest(std::span translations) const; + constexpr Translation2d Nearest( + std::span translations) const { + return *std::min_element(translations.begin(), translations.end(), + [this](Translation2d a, Translation2d b) { + return this->Distance(a) < this->Distance(b); + }); + } /** * Returns the nearest Translation2d from a collection of translations * @param translations The collection of translations. * @return The nearest Translation2d from the collection. */ - Translation2d Nearest( - std::initializer_list translations) const; + constexpr Translation2d Nearest( + std::initializer_list translations) const { + return *std::min_element(translations.begin(), translations.end(), + [this](Translation2d a, Translation2d b) { + return this->Distance(a) < this->Distance(b); + }); + } private: units::meter_t m_x = 0_m; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 9aa3f61c31e..97d9b603090 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT Translation3d { * @param distance The distance from the origin to the end of the translation. * @param angle The angle between the x-axis and the translation vector. */ - Translation3d(units::meter_t distance, const Rotation3d& angle) { + constexpr Translation3d(units::meter_t distance, const Rotation3d& angle) { auto rectangular = Translation3d{distance, 0_m, 0_m}.RotateBy(angle); m_x = rectangular.X(); m_y = rectangular.Y(); @@ -61,7 +61,7 @@ class WPILIB_DLLEXPORT Translation3d { * * @param vector The translation vector to represent. */ - explicit Translation3d(const Eigen::Vector3d& vector) + constexpr explicit Translation3d(const Eigen::Vector3d& vector) : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}}, m_z{units::meter_t{vector.z()}} {} @@ -76,7 +76,7 @@ class WPILIB_DLLEXPORT Translation3d { * * @return The distance between the two translations. */ - units::meter_t Distance(const Translation3d& other) const { + constexpr units::meter_t Distance(const Translation3d& other) const { return units::math::sqrt(units::math::pow<2>(other.m_x - m_x) + units::math::pow<2>(other.m_y - m_y) + units::math::pow<2>(other.m_z - m_z)); @@ -117,7 +117,7 @@ class WPILIB_DLLEXPORT Translation3d { * * @return The norm of the translation. */ - units::meter_t Norm() const { + constexpr units::meter_t Norm() const { return units::math::sqrt(m_x * m_x + m_y * m_y + m_z * m_z); } @@ -131,7 +131,7 @@ class WPILIB_DLLEXPORT Translation3d { * * @return The new rotated translation. */ - Translation3d RotateBy(const Rotation3d& other) const { + constexpr Translation3d RotateBy(const Rotation3d& other) const { Quaternion p{0.0, m_x.value(), m_y.value(), m_z.value()}; auto qprime = other.GetQuaternion() * p * other.GetQuaternion().Inverse(); return Translation3d{units::meter_t{qprime.X()}, units::meter_t{qprime.Y()}, diff --git a/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/wpimath/src/main/native/include/frc/geometry/Twist2d.h index 4d61501d998..f8ae2eac63d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist2d.h @@ -11,6 +11,7 @@ #include "units/math.h" namespace frc { + /** * A change in distance along a 2D arc since the last pose update. We can use * ideas from differential calculus to create new Pose2ds from a Twist2d and @@ -40,7 +41,7 @@ struct WPILIB_DLLEXPORT Twist2d { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const Twist2d& other) const { + constexpr bool operator==(const Twist2d& other) const { return units::math::abs(dx - other.dx) < 1E-9_m && units::math::abs(dy - other.dy) < 1E-9_m && units::math::abs(dtheta - other.dtheta) < 1E-9_rad; @@ -56,6 +57,7 @@ struct WPILIB_DLLEXPORT Twist2d { return Twist2d{dx * factor, dy * factor, dtheta * factor}; } }; + } // namespace frc #ifndef NO_PROTOBUF diff --git a/wpimath/src/main/native/include/frc/geometry/Twist3d.h b/wpimath/src/main/native/include/frc/geometry/Twist3d.h index 2ce4a964e3f..65f26294fe7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Twist3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Twist3d.h @@ -6,12 +6,12 @@ #include -#include "frc/geometry/Rotation3d.h" #include "units/angle.h" #include "units/length.h" #include "units/math.h" namespace frc { + /** * A change in distance along a 3D arc since the last pose update. We can use * ideas from differential calculus to create new Pose3ds from a Twist3d and @@ -56,7 +56,7 @@ struct WPILIB_DLLEXPORT Twist3d { * @param other The other object. * @return Whether the two objects are equal. */ - bool operator==(const Twist3d& other) const { + constexpr bool operator==(const Twist3d& other) const { return units::math::abs(dx - other.dx) < 1E-9_m && units::math::abs(dy - other.dy) < 1E-9_m && units::math::abs(dz - other.dz) < 1E-9_m && @@ -76,6 +76,7 @@ struct WPILIB_DLLEXPORT Twist3d { rx * factor, ry * factor, rz * factor}; } }; + } // namespace frc #ifndef NO_PROTOBUF