From b9e2c2bac490686b59b1d79914df3d6f3c595c0b Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 2 Jun 2024 17:05:03 -0700 Subject: [PATCH 1/2] [wpimath] Make geometry classes constexpr --- .../wpi/first/math/geometry/Quaternion.java | 4 +- .../wpi/first/math/geometry/Rotation3d.java | 56 ++- .../native/cpp/geometry/CoordinateAxis.cpp | 41 -- .../native/cpp/geometry/CoordinateSystem.cpp | 76 ---- .../main/native/cpp/geometry/Ellipse2d.cpp | 4 - .../src/main/native/cpp/geometry/Pose2d.cpp | 93 ----- .../src/main/native/cpp/geometry/Pose3d.cpp | 173 -------- .../main/native/cpp/geometry/Quaternion.cpp | 213 ---------- .../main/native/cpp/geometry/Rotation2d.cpp | 6 - .../main/native/cpp/geometry/Rotation3d.cpp | 249 ------------ .../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 - .../src/main/native/include/frc/ct_matrix.h | 372 ++++++++++++++++++ .../include/frc/geometry/CoordinateAxis.h | 18 +- .../include/frc/geometry/CoordinateSystem.h | 74 +++- .../native/include/frc/geometry/Ellipse2d.h | 4 +- .../main/native/include/frc/geometry/Pose2d.h | 122 +++++- .../main/native/include/frc/geometry/Pose3d.h | 244 +++++++++++- .../native/include/frc/geometry/Quaternion.h | 196 +++++++-- .../native/include/frc/geometry/Rotation3d.h | 259 +++++++++++- .../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 +- .../test/native/cpp/geometry/Pose3dTest.cpp | 4 +- .../native/cpp/geometry/Rotation3dTest.cpp | 4 +- 30 files changed, 1325 insertions(+), 1115 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 create mode 100644 wpimath/src/main/native/include/frc/ct_matrix.h diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 47acce270ba..6044e90737c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -295,7 +295,9 @@ public Quaternion log() { double v_scalar; if (v_norm < 1e-9) { - // Taylor series expansion of atan2(y / x) / y around y = 0 => 1/x - y²/3*x³ + O(y⁴) + // Taylor series expansion of atan2(y/x)/y at y = 0: + // + // 1/x - 1/3 y²/x³ + O(y⁴) v_scalar = 1.0 / getW() - 1.0 / 3.0 * v_norm * v_norm / (getW() * getW() * getW()); } else { v_scalar = Math.atan2(v_norm, getW()) / v_norm; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 123badb6ca0..6d12bbd1856 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -24,7 +24,6 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; import java.util.Objects; -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; /** A rotation in a 3D coordinate frame represented by a quaternion. */ @JsonIgnoreProperties(ignoreUnknown = true) @@ -230,27 +229,42 @@ public Rotation3d(Vector initial, Vector last) { // there's no rotation. The default initialization of m_q will work. m_q = new Quaternion(); } else if (dotNorm < -1.0 + 1E-9) { - // If the dot product is -1, the two vectors point in opposite directions - // so a 180 degree rotation is required. Any orthogonal vector can be used - // for it. Q in the QR decomposition is an orthonormal basis, so it - // contains orthogonal unit vectors. - var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0)); - final var qr = DecompositionFactory_DDRM.qr(3, 1); - qr.decompose(X.getStorage().getMatrix()); - final var Q = qr.getQ(null, false); - - // w = cos(θ/2) = cos(90°) = 0 - // - // 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 = new Quaternion(0.0, Q.get(0, 1), Q.get(1, 1), Q.get(2, 1)); + // If the dot product is -1, the two vectors are antiparallel, so a 180° + // rotation is required. Any other vector can be used to generate an + // orthogonal one. + + double x = Math.abs(initial.get(0, 0)); + double y = Math.abs(initial.get(1, 0)); + double z = Math.abs(initial.get(2, 0)); + + // Find vector that is most orthogonal to initial vector + Vector other; + if (x < y) { + if (x < z) { + // Use x-axis + other = VecBuilder.fill(1, 0, 0); + } else { + // Use z-axis + other = VecBuilder.fill(0, 0, 1); + } + } else { + if (y < z) { + // Use y-axis + other = VecBuilder.fill(0, 1, 0); + } else { + // Use z-axis + other = VecBuilder.fill(0, 0, 1); + } + } + + var axis = Vector.cross(initial, other); + + double axisNorm = axis.norm(); + m_q = + new Quaternion( + 0.0, axis.get(0, 0) / axisNorm, axis.get(1, 0) / axisNorm, axis.get(2, 0) / axisNorm); } else { - // initial x last - var axis = - VecBuilder.fill( - initial.get(1, 0) * last.get(2, 0) - last.get(1, 0) * initial.get(2, 0), - last.get(0, 0) * initial.get(2, 0) - initial.get(0, 0) * last.get(2, 0), - initial.get(0, 0) * last.get(1, 0) - last.get(0, 0) * initial.get(1, 0)); + var axis = Vector.cross(initial, last); // https://stackoverflow.com/a/11741520 m_q = 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/Ellipse2d.cpp b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp index 6f8afa636a2..eef320cd128 100644 --- a/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp @@ -8,10 +8,6 @@ using namespace frc; -units::meter_t Ellipse2d::Distance(const Translation2d& point) const { - return FindNearestPoint(point).Distance(point); -} - Translation2d Ellipse2d::FindNearestPoint(const Translation2d& point) const { // Check if already in ellipse if (Contains(point)) { 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..8b21131fe19 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -4,181 +4,8 @@ #include "frc/geometry/Pose3d.h" -#include -#include - -#include #include -using namespace frc; - -namespace { - -/** - * Applies the hat operator to a rotation vector. - * - * It takes a rotation vector and returns the corresponding matrix - * representation of the Lie algebra element (a 3x3 rotation matrix). - * - * @param rotation The rotation vector. - * @return The rotation vector as a 3x3 rotation matrix. - */ -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}}; -} -} // 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 thetaSq = theta * theta; - - double A; - double B; - double C; - if (std::abs(theta) < 1E-7) { - // Taylor Expansions around θ = 0 - // A = 1/1! - θ²/3! + θ⁴/5! - // B = 1/2! - θ²/4! + θ⁴/6! - // C = 1/3! - θ²/5! + θ⁴/7! - // sources: - // A: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 - // B: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 - // C: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 - A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120; - B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720; - C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; - } else { - // A = std::sin(θ)/θ - // B = (1 - std::cos(θ)) / θ² - // C = (1 - A) / θ² - A = std::sin(theta) / theta; - B = (1 - std::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; - const Transform3d transform{ - Translation3d{units::meter_t{translation_component(0)}, - units::meter_t{translation_component(1)}, - units::meter_t{translation_component(2)}}, - Rotation3d{R}}; - - return *this + transform; -} - -Twist3d Pose3d::Log(const Pose3d& end) const { - // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf - const auto transform = end.RelativeTo(*this); - - Eigen::Vector3d u{transform.X().value(), transform.Y().value(), - transform.Z().value()}; - Eigen::Vector3d rvec = - transform.Rotation().GetQuaternion().ToRotationVector(); - - Eigen::Matrix3d omega = RotationVectorToMatrix(rvec); - Eigen::Matrix3d omegaSq = omega * omega; - double theta = rvec.norm(); - double thetaSq = theta * theta; - - double C; - if (std::abs(theta) < 1E-7) { - // Taylor Expansions around θ = 0 - // A = 1/1! - θ²/3! + θ⁴/5! - // B = 1/2! - θ²/4! + θ⁴/6! - // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!) - // sources: - // A: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 - // B: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 - // C: - // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 - C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240; - } else { - // 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; - C = (1 - A / (2 * B)) / thetaSq; - } - - Eigen::Matrix3d V_inv = - Eigen::Matrix3d::Identity() - 0.5 * omega + C * omegaSq; - - 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()}; -} - void frc::to_json(wpi::json& json, const Pose3d& pose) { json = wpi::json{{"translation", pose.Translation()}, {"rotation", pose.Rotation()}}; 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..453a42df3df 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -4,257 +4,8 @@ #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 dotNorm = dot / normProduct; - - if (dotNorm > 1.0 - 1E-9) { - // If the dot product is 1, the two vectors point in the same direction so - // there's no rotation. The default initialization of m_q will work. - return; - } else if (dotNorm < -1.0 + 1E-9) { - // If the dot product is -1, the two vectors point in opposite directions so - // a 180 degree rotation is required. Any orthogonal vector can be used for - // it. Q in the QR decomposition is an orthonormal basis, so it contains - // orthogonal unit vectors. - Eigen::Matrix X = initial; - Eigen::HouseholderQR qr{X}; - Eigen::Matrix Q = qr.householderQ(); - - // w = std::cos(θ/2) = std::cos(90°) = 0 - // - // 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)}; - } 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)}; - - // 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)}; - } -} - -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/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h new file mode 100644 index 00000000000..ecc9f28a608 --- /dev/null +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -0,0 +1,372 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace frc { + +template +concept EigenMatrixLike = std::derived_from>; + +/** + * Compile-time wrapper for Eigen::Matrix. + * + * @tparam Rows Rows of matrix. + * @tparam Cols Columns of matrix. + */ +template +class ct_matrix { + public: + constexpr ct_matrix() = default; + + /** + * Constructs a scalar VariableMatrix from a nested list of Variables. + * + * @param list The nested list of Variables. + */ + constexpr ct_matrix( // NOLINT (runtime/explicit) + std::initializer_list> list) + : m_storage{list} {} + + template + requires std::derived_from> + // NOLINTNEXTLINE (runtime/explicit) + constexpr ct_matrix(const Derived& mat) : m_storage{mat} {} + + /** + * Returns number of rows. + * + * @return Number of rows. + */ + constexpr int rows() const { return m_storage.rows(); } + + /** + * Returns number of columns. + * + * @return Number of columns. + */ + constexpr int cols() const { return m_storage.cols(); } + + /** + * Returns reference to matrix element. + * + * @param row Row index. + * @param col Column index. + */ + constexpr const Scalar& operator()(int row, int col) const { + return m_storage.coeff(row, col); + } + + /** + * Returns reference to matrix element. + * + * @param row Row index. + * @param col Column index. + */ + constexpr Scalar& operator()(int row, int col) { + return m_storage.coeffRef(row, col); + } + + /** + * Returns reference to matrix element. + * + * @param index Index. + */ + constexpr const Scalar& operator()(int index) const + requires(Rows == 1 || Cols == 1) + { + return m_storage.coeff(index); + } + + /** + * Returns reference to matrix element. + * + * @param index Index. + */ + constexpr Scalar& operator()(int index) + requires(Rows == 1 || Cols == 1) + { + return m_storage.coeffRef(index); + } + + /** + * Constexpr version of Eigen's scalar multiplication operator. + * + * @param lhs LHS scalar. + * @param rhs RHS matrix. + * @return Result of multiplication. + */ + friend constexpr ct_matrix operator*( + Scalar lhs, const ct_matrix& rhs) { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int i = 0; i < rhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + result(i, j) = lhs * rhs(i, j); + } + } + + return result; + } else { + return lhs * rhs.m_storage; + } + } + + /** + * Constexpr version of Eigen's matrix multiplication operator. + * + * @tparam Cols2 Columns of RHS matrix. + * @param lhs LHS matrix. + * @param rhs RHS matrix. + * @return Result of multiplication. + */ + template + friend constexpr ct_matrix operator*( + const ct_matrix& lhs, + const ct_matrix& rhs) { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int i = 0; i < lhs.rows(); ++i) { + for (int j = 0; j < rhs.cols(); ++j) { + Scalar sum = 0.0; + for (int k = 0; k < lhs.cols(); ++k) { + sum += lhs(i, k) * rhs(k, j); + } + result(i, j) = sum; + } + } + + return result; + } else { + return lhs.m_storage * rhs.storage(); + } + } + + /** + * Constexpr version of Eigen's matrix addition operator. + * + * @param lhs LHS matrix. + * @param rhs RHS matrix. + * @return Result of addition. + */ + friend constexpr ct_matrix operator+( + const ct_matrix& lhs, + const ct_matrix& rhs) { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + result(row, col) = lhs(row, col) + rhs(row, col); + } + } + + return result; + } else { + return lhs.m_storage + rhs.m_storage; + } + } + + /** + * Constexpr version of Eigen's matrix subtraction operator. + * + * @param lhs LHS matrix. + * @param rhs RHS matrix. + * @return Result of subtraction. + */ + friend constexpr ct_matrix operator-( + const ct_matrix& lhs, + const ct_matrix& rhs) { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + result(row, col) = lhs(row, col) - rhs(row, col); + } + } + + return result; + } else { + return lhs.m_storage - rhs.m_storage; + } + } + + /** + * Constexpr version of Eigen's transpose member function. + * + * @return Transpose of matrix. + */ + constexpr ct_matrix transpose() const { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { + result(col, row) = (*this)(row, col); + } + } + + return result; + } else { + return m_storage.transpose().eval(); + } + } + + /** + * Constexpr version of Eigen's identity function. + * + * @return Identity matrix of the specified size. + */ + static constexpr ct_matrix Identity() + requires(Rows != Eigen::Dynamic && Cols != Eigen::Dynamic) + { + if (std::is_constant_evaluated()) { + ct_matrix result; + + for (int row = 0; row < Rows; ++row) { + for (int col = 0; col < Cols; ++col) { + if (row == col) { + result(row, row) = 1.0; + } else { + result(row, col) = 0.0; + } + } + } + + return result; + } else { + return Eigen::Matrix::Identity(); + } + } + + /** + * Constexpr version of Eigen's vector dot member function. + * + * @tparam RhsRows Rows of RHS vector. + * @tparam RhsCols Columns of RHS vector. + * @param rhs RHS vector. + * @return Dot product of two vectors. + */ + template + requires(Rows == 1 || Cols == 1) && (RhsRows == 1 || RhsCols == 1) && + (Rows * Cols == RhsRows * RhsCols) + constexpr Scalar dot(const ct_matrix& rhs) const { + if (std::is_constant_evaluated()) { + Scalar sum = 0.0; + + for (int index = 0; index < rows() * rhs.cols(); ++index) { + sum += (*this)(index)*rhs(index); + } + + return sum; + } else { + return m_storage.dot(rhs.storage()); + } + } + + /** + * Constexpr version of Eigen's norm member function. + * + * @return Norm of matrix. + */ + constexpr Scalar norm() const { + if (std::is_constant_evaluated()) { + Scalar sum = 0.0; + + for (int row = 0; row < Rows; ++row) { + for (int col = 0; col < Cols; ++col) { + sum += (*this)(row, col) * (*this)(row, col); + } + } + + return gcem::sqrt(sum); + } else { + return m_storage.norm(); + } + } + + /** + * Constexpr version of Eigen's 3D vector cross member function. + * + * @param rhs RHS vector. + * @return Cross product of two vectors. + */ + constexpr ct_matrix cross(const ct_matrix& rhs) + requires(Rows == 3 && Cols == 1) + { + return Eigen::Vector3d{{(*this)(1) * rhs(2) - rhs(1) * (*this)(2), + rhs(0) * (*this)(2) - (*this)(0) * rhs(2), + (*this)(0) * rhs(1) - rhs(0) * (*this)(1)}}; + } + + /** + * Constexpr version of Eigen's 3x3 matrix determinant member function. + * + * @return Determinant of matrix. + */ + constexpr Scalar determinant() const + requires(Rows == 3 && Cols == 3) + { + // |a b c| + // |d e f| = aei + bfg + cgh - ceg - bdi - afh + // |g h i| + Scalar a = (*this)(0, 0); + Scalar b = (*this)(0, 1); + Scalar c = (*this)(0, 2); + Scalar d = (*this)(1, 0); + Scalar e = (*this)(1, 1); + Scalar f = (*this)(1, 2); + Scalar g = (*this)(2, 0); + Scalar h = (*this)(2, 1); + Scalar i = (*this)(2, 2); + return a * e * i + b * f * g + c * g * h - c * e * g - b * d * i - + a * f * h; + } + + /** + * Returns the internal Eigen matrix. + * + * @return The internal Eigen matrix. + */ + constexpr const Eigen::Matrix& storage() const { + return m_storage; + } + + /** + * Implicit cast to an Eigen matrix. + */ + constexpr operator Eigen::Matrix() const { // NOLINT + return m_storage; + } + + private: + Eigen::Matrix m_storage; +}; + +template + requires std::derived_from> +ct_matrix(const Derived&) + -> ct_matrix; + +template +using ct_vector = ct_matrix; + +template +using ct_row_vector = ct_matrix; + +using ct_matrix3d = ct_matrix; +using ct_vector3d = ct_vector; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h index 2b471b7d5df..be292194036 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::hypot(x, y, 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/Ellipse2d.h b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h index e1731105ebb..4957b15914e 100644 --- a/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Ellipse2d.h @@ -153,7 +153,9 @@ class WPILIB_DLLEXPORT Ellipse2d { * @param point The point to check. * @return The distance (0, if the point is contained by the ellipse) */ - units::meter_t Distance(const Translation2d& point) const; + units::meter_t Distance(const Translation2d& point) const { + return FindNearestPoint(point).Distance(point); + } /** * Returns the nearest point that is contained within the ellipse. 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..bffe9967d65 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,17 +4,23 @@ #pragma once +#include +#include + +#include #include #include +#include "frc/ct_matrix.h" #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 +37,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 +50,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 +73,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 +83,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 +132,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 +143,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 +155,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 +169,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 +183,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. @@ -185,7 +204,7 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The new pose of the robot. */ - Pose3d Exp(const Twist3d& twist) const; + constexpr Pose3d Exp(const Twist3d& twist) const; /** * Returns a Twist3d that maps this pose to the end pose. If c is the output @@ -195,12 +214,14 @@ class WPILIB_DLLEXPORT Pose3d { * * @return The twist that maps this to end. */ - Twist3d Log(const Pose3d& end) const; + constexpr Twist3d Log(const Pose3d& end) const; /** * 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 +240,184 @@ 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 { + +namespace detail { + +/** + * Applies the hat operator to a rotation vector. + * + * It takes a rotation vector and returns the corresponding matrix + * representation of the Lie algebra element (a 3x3 rotation matrix). + * + * @param rotation The rotation vector. + * @return The rotation vector as a 3x3 rotation matrix. + */ +constexpr ct_matrix3d RotationVectorToMatrix(const ct_vector3d& rotation) { + // Given a rotation vector , + // [ 0 -c b] + // Omega = [ c 0 -a] + // [-b a 0] + return ct_matrix3d{{0.0, -rotation(2), rotation(1)}, + {rotation(2), 0.0, -rotation(0)}, + {-rotation(1), rotation(0), 0.0}}; +} + +/** + * Applies the hat operator to a rotation vector. + * + * It takes a rotation vector and returns the corresponding matrix + * representation of the Lie algebra element (a 3x3 rotation matrix). + * + * @param rotation The rotation vector. + * @return The rotation vector as a 3x3 rotation matrix. + */ +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.coeff(2), rotation.coeff(1)}, + {rotation.coeff(2), 0.0, -rotation.coeff(0)}, + {-rotation.coeff(1), rotation.coeff(0), 0.0}}; +} + +} // namespace detail + +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()}; +} + +constexpr Pose3d Pose3d::Exp(const Twist3d& twist) const { + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + + auto impl = [this]( + const Twist3d& twist) -> Pose3d { + Vector3d u{{twist.dx.value(), twist.dy.value(), twist.dz.value()}}; + Vector3d rvec{{twist.rx.value(), twist.ry.value(), twist.rz.value()}}; + Matrix3d omega = detail::RotationVectorToMatrix(rvec); + Matrix3d omegaSq = omega * omega; + double theta = rvec.norm(); + double thetaSq = theta * theta; + + double A; + double B; + double C; + if (gcem::abs(theta) < 1E-7) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/3! - θ²/5! + θ⁴/7! + // sources: + // A: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 + // B: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + // C: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + A = 1 - thetaSq / 6 + thetaSq * thetaSq / 120; + B = 1 / 2.0 - thetaSq / 24 + thetaSq * thetaSq / 720; + C = 1 / 6.0 - thetaSq / 120 + thetaSq * thetaSq / 5040; + } else { + // A = std::sin(θ)/θ + // B = (1 - std::cos(θ)) / θ² + // C = (1 - A) / θ² + A = gcem::sin(theta) / theta; + B = (1 - gcem::cos(theta)) / thetaSq; + C = (1 - A) / thetaSq; + } + + Matrix3d R = Matrix3d::Identity() + A * omega + B * omegaSq; + Matrix3d V = Matrix3d::Identity() + B * omega + C * omegaSq; + + 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)}}, + Rotation3d{R}}; + + return *this + transform; + }; + + if (std::is_constant_evaluated()) { + return impl.template operator()(twist); + } else { + return impl.template operator()(twist); + } +} + +constexpr Twist3d Pose3d::Log(const Pose3d& end) const { + // Implementation from Section 3.2 of https://ethaneade.org/lie.pdf + + auto impl = [this]( + const Pose3d& end) -> Twist3d { + const auto transform = end.RelativeTo(*this); + + Vector3d u{ + {transform.X().value(), transform.Y().value(), transform.Z().value()}}; + Vector3d rvec = transform.Rotation().GetQuaternion().ToRotationVector(); + Matrix3d omega = detail::RotationVectorToMatrix(rvec); + Matrix3d omegaSq = omega * omega; + double theta = rvec.norm(); + double thetaSq = theta * theta; + + double C; + if (gcem::abs(theta) < 1E-7) { + // Taylor Expansions around θ = 0 + // A = 1/1! - θ²/3! + θ⁴/5! + // B = 1/2! - θ²/4! + θ⁴/6! + // C = 1/6 * (1/2 + θ²/5! + θ⁴/7!) + // sources: + // A: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D+at+x%3D0 + // B: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + // C: + // https://www.wolframalpha.com/input?i2d=true&i=series+expansion+of+Divide%5B1-Divide%5BDivide%5Bsin%5C%2840%29x%5C%2841%29%2Cx%5D%2C2Divide%5B1-cos%5C%2840%29x%5C%2841%29%2CPower%5Bx%2C2%5D%5D%5D%2CPower%5Bx%2C2%5D%5D+at+x%3D0 + C = 1 / 12.0 + thetaSq / 720 + thetaSq * thetaSq / 30240; + } else { + // A = std::sin(θ)/θ + // B = (1 - std::cos(θ)) / θ² + // C = (1 - A/(2*B)) / θ² + double A = gcem::sin(theta) / theta; + double B = (1 - gcem::cos(theta)) / thetaSq; + C = (1 - A / (2 * B)) / thetaSq; + } + + Matrix3d V_inv = Matrix3d::Identity() - 0.5 * omega + C * omegaSq; + + 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)}}; + }; + + if (std::is_constant_evaluated()) { + return impl.template operator()(end); + } else { + return impl.template operator()(end); + } +} + +} // 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..5226917a3f9 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 at y = 0: + // + // 1/x - 1/3 y²/x³ + O(y⁴) + 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 = gcem::hypot(m_x, m_y, 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..380730d3101 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -4,13 +4,23 @@ #pragma once +#include +#include + #include +#include +#include +#include #include #include +#include "frc/ct_matrix.h" +#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 +32,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 +55,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 +78,19 @@ 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 = ct_matrix{axis}.norm(); + 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 +99,8 @@ 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{ct_matrix{rvec}.norm()}} {} /** * Constructs a Rotation3d from a rotation matrix. @@ -71,7 +108,63 @@ 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) { + // Require that the rotation matrix is special orthogonal. This is true if + // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + auto impl = [](const Matrix3d& R) -> Quaternion { + if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) { + throw std::domain_error("Rotation matrix isn't orthogonal"); + } + if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + throw std::domain_error( + "Rotation matrix is orthogonal but not special orthogonal"); + } + + // 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 / gcem::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 * gcem::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 * gcem::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 * gcem::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; + } + } + + return Quaternion{w, x, y, z}; + }; + + if (std::is_constant_evaluated()) { + m_q = impl(ct_matrix3d{rotationMatrix}); + } else { + m_q = impl(rotationMatrix); + } + } /** * Constructs a Rotation3d that rotates the initial vector onto the final @@ -83,7 +176,58 @@ class WPILIB_DLLEXPORT Rotation3d { * @param initial The initial vector. * @param final The final vector. */ - Rotation3d(const Eigen::Vector3d& initial, const Eigen::Vector3d& final); + constexpr Rotation3d(const Eigen::Vector3d& initial, + const Eigen::Vector3d& final) { + double dot = ct_matrix{initial}.dot(ct_matrix{final}); + double normProduct = ct_matrix{initial}.norm() * ct_matrix{final}.norm(); + double dotNorm = dot / normProduct; + + if (dotNorm > 1.0 - 1E-9) { + // If the dot product is 1, the two vectors point in the same direction so + // there's no rotation. The default initialization of m_q will work. + return; + } else if (dotNorm < -1.0 + 1E-9) { + // If the dot product is -1, the two vectors are antiparallel, so a 180° + // rotation is required. Any other vector can be used to generate an + // orthogonal one. + + double x = gcem::abs(initial.coeff(0)); + double y = gcem::abs(initial.coeff(1)); + double z = gcem::abs(initial.coeff(2)); + + // Find vector that is most orthogonal to initial vector + Eigen::Vector3d other; + if (x < y) { + if (x < z) { + // Use x-axis + other = Eigen::Vector3d{{1, 0, 0}}; + } else { + // Use z-axis + other = Eigen::Vector3d{{0, 0, 1}}; + } + } else { + if (y < z) { + // Use y-axis + other = Eigen::Vector3d{{0, 1, 0}}; + } else { + // Use z-axis + other = Eigen::Vector3d{{0, 0, 1}}; + } + } + + auto axis = ct_matrix{initial}.cross(ct_matrix{other}); + + double axisNorm = axis.norm(); + m_q = Quaternion{0.0, axis(0) / axisNorm, axis(1) / axisNorm, + axis(2) / axisNorm}; + } else { + auto axis = ct_matrix{initial}.cross(final); + + // https://stackoverflow.com/a/11741520 + m_q = + Quaternion{normProduct + dot, axis(0), axis(1), axis(2)}.Normalize(); + } + } /** * Adds two rotations together. @@ -92,7 +236,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 +248,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 +266,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 +284,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 +307,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 diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index a9490fbdcc9..88acb1fb897 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -80,7 +80,7 @@ TEST(Pose3dTest, RelativeTo) { EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), finalRelativeToInitial.X().value()); EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Y().value()); - EXPECT_DOUBLE_EQ(0.0, finalRelativeToInitial.Rotation().Z().value()); + EXPECT_NEAR(0.0, finalRelativeToInitial.Rotation().Z().value(), 1e-9); } TEST(Pose3dTest, Equality) { @@ -109,7 +109,7 @@ TEST(Pose3dTest, Minus) { EXPECT_DOUBLE_EQ(5.0 * std::sqrt(2.0), transform.X().value()); EXPECT_DOUBLE_EQ(0.0, transform.Y().value()); - EXPECT_DOUBLE_EQ(0.0, transform.Rotation().Z().value()); + EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9); } TEST(Pose3dTest, ToPose2d) { diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 01b37e5b10e..16c033723d1 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -23,7 +23,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) { units::radian_t{std::numbers::pi / 2}}; EXPECT_EQ(expected1, result1); EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result1.X() + result1.Z()).value()); - EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, result1.Y().value()); + EXPECT_NEAR(-std::numbers::pi / 2, result1.Y().value(), 1e-7); rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}; rot2 = Rotation3d{units::radian_t{-std::numbers::pi}, 0_rad, 0_rad}; @@ -34,7 +34,7 @@ TEST(Rotation3dTest, GimbalLockAccuracy) { units::radian_t{std::numbers::pi / 2}}; EXPECT_EQ(expected2, result2); EXPECT_DOUBLE_EQ(std::numbers::pi / 2, (result2.Z() - result2.X()).value()); - EXPECT_DOUBLE_EQ(std::numbers::pi / 2, result2.Y().value()); + EXPECT_NEAR(std::numbers::pi / 2, result2.Y().value(), 1e-7); rot1 = Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}; rot2 = Rotation3d{0_rad, units::radian_t{std::numbers::pi / 3}, 0_rad}; From 0c576856ba57aabfe740c2699151ccd5d65fa9db Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 17 Oct 2024 16:19:29 -0700 Subject: [PATCH 2/2] Update wpimath/src/main/native/include/frc/geometry/Rotation3d.h Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- wpimath/src/main/native/include/frc/geometry/Rotation3d.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 380730d3101..0defa21f1a2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -109,9 +109,9 @@ class WPILIB_DLLEXPORT Rotation3d { * @throws std::domain_error if the rotation matrix isn't special orthogonal. */ constexpr explicit Rotation3d(const Eigen::Matrix3d& rotationMatrix) { - // Require that the rotation matrix is special orthogonal. This is true if - // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). auto impl = [](const Matrix3d& R) -> Quaternion { + // 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() - Matrix3d::Identity()).norm() > 1e-9) { throw std::domain_error("Rotation matrix isn't orthogonal"); }