Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make geometry classes constexpr (#7222)

Browse Source
This commit is contained in:
Tyler Veness
2024-10-18 16:08:41 -07:00
committed by GitHub
parent 2054d0f57e
commit 95b9bd880b
30 changed files with 1324 additions and 1114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
wpimath/src/main/native/include/frc/geometry/Quaternion.h
View File

@@ -4,7 +4,10 @@
#pragma once
#include <numbers>
#include <Eigen/Core>
#include <gcem.hpp>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
@@ -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