diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index f999de8c79..3b27418e1c 100644 --- a/wpimath/algorithms.md +++ b/wpimath/algorithms.md @@ -351,3 +351,109 @@ When calculating a\_z: ``` Note that this reuses the cos(a\_y) cos(a\_z) and cos(a\_y) sin(a\_z) terms needed to calculate a\_z. + +## Quaternion Exponential + +We will take it as given that a quaternion has scalar and vector components `𝑞 = s + đ‘Ŗ⃗`, with vector component đ‘Ŗ⃗ consisting of a unit vector and magnitude `đ‘Ŗ⃗ = θ * vĖ‚`. + +``` +𝑞 = s + đ‘Ŗ⃗ + +đ‘Ŗ⃗ = θ * vĖ‚ + +exp(𝑞) = exp(s + đ‘Ŗ⃗) +exp(𝑞) = exp(s) * exp(đ‘Ŗ⃗) +exp(𝑞) = exp(s) * exp(θ * vĖ‚) +``` + +Applying euler's identity: + +``` +exp(θ * vĖ‚) = cos(θ) + sin(θ) * vĖ‚ +``` + +Gives us: +``` +exp(𝑞) = exp(s) * [cos(θ) + sin(θ) * vĖ‚] +``` + +Rearranging `đ‘Ŗ⃗ = θ * vĖ‚` we can solve for vĖ‚: `vĖ‚ = đ‘Ŗ⃗ / θ` + +``` +exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * đ‘Ŗ⃗] +``` + +## Quaternion Logarithm + +We will take it as a given that for a given quaternion of the form `𝑞 = s + đ‘Ŗ⃗`, we can calculate the exponential: `exp(𝑞) = exp(s) * [cos(θ) + sin(θ) / θ * đ‘Ŗ⃗]` where `θ = ||đ‘Ŗ⃗||`. + +Additionally, `exp(log(𝑞)) = q` for a given value of `log(𝑞)`. There are multiple solutions to `log(𝑞)` caused by the imaginary axes in đ‘Ŗ⃗, discussed here: https://en.wikipedia.org/wiki/Complex_logarithm + +We will demonstrate the principal solution of `log(𝑞)` satisfying `exp(log(𝑞)) = q`. +This being `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * đ‘Ŗ⃗`, is the principal solution to `log(𝑞)` because the function `atan2(θ, s)` returns the principal value corresponding to its arguments. + +Proof: `log(𝑞) = log(||𝑞||) + atan2(θ, s) / θ * đ‘Ŗ⃗` satisfies `exp(log(𝑞)) = q`. + +``` +exp(log(𝑞)) = exp(log(||𝑞||) + atan2(θ, s) / θ * đ‘Ŗ⃗) + + +exp(log(𝑞)) = exp(log(||𝑞||)) * exp(atan2(θ, s) / θ * đ‘Ŗ⃗) + +Substitutions: +đ‘Ŗ⃗ = θ * vĖ‚: +exp(log(||𝑞||)) = ||𝑞|| +exp(log(𝑞)) = ||𝑞|| * exp(atan2(θ, s) * vĖ‚) + +exp(log(𝑞)) = ||𝑞|| * [cos(atan2(θ, s)) + sin(atan2(θ, s)) * vĖ‚] + +Substitutions: +cos(atan2(θ, s)) = s / √(θ² + s²) +sin(atan2(θ, s)) = θ / √(θ² + s²) + +exp(log(𝑞)) = ||𝑞|| * [s / √(θ² + s²) + θ / √(θ² + s²) * vĖ‚] + +√(θ² + s²) = ||𝑞|| + +exp(log(𝑞)) = ||𝑞|| * [s / ||𝑞|| + θ / ||𝑞|| * vĖ‚] +exp(log(𝑞)) = s + θ * vĖ‚ + +exp(log(𝑞)) = s + đ‘Ŗ⃗ + +exp(log(𝑞)) = 𝑞 +``` + +## Unit Quaternion in SO(3) from Rotation Vector in 𝖘𝖔(3) + +We will take it as a given that members of 𝖘𝖔(3) take the form `đ‘Ŗ⃗ = θ * vĖ‚`, representing a rotation θ around a unit axis vĖ‚. + +We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = cos(θ / 2) + sin(θ / 2) * vĖ‚`, representing a rotation of θ around unit axis vĖ‚. + +``` +θ = ||đ‘Ŗ⃗|| +vĖ‚ = đ‘Ŗ⃗ / θ + +𝑞 = cos(θ / 2) + sin(θ / 2) * vĖ‚ +𝑞 = cos(||đ‘Ŗ⃗|| / 2) + sin(||đ‘Ŗ⃗|| / 2) / ||đ‘Ŗ⃗|| * đ‘Ŗ⃗ +``` + +## Rotation vector in 𝖘𝖔(3) from Unit Quaternion in SO(3) + +We will take it as a given that members of 𝖘𝖔(3) take the form `𝑟⃗ = θ * rĖ‚`, representing a rotation θ around a unit axis rĖ‚. + +We additionally take it as a given that quaternions in SO(3) are of the form `𝑞 = s + đ‘Ŗ⃗ = cos(θ / 2) + sin(θ / 2) * vĖ‚`, representing a rotation of θ around unit axis vĖ‚. + +``` +s + đ‘Ŗ⃗ = cos(θ / 2) + sin(θ / 2) * vĖ‚ +s = cos(θ / 2) +đ‘Ŗ⃗ = sin(θ / 2) * vĖ‚ +||đ‘Ŗ⃗|| = sin(θ / 2) + +θ / 2 = atan2(||đ‘Ŗ⃗||, s) +θ = 2 * atan2(||đ‘Ŗ⃗||, s) + +rĖ‚ = đ‘Ŗ⃗ / ||đ‘Ŗ⃗|| + +𝑟⃗ = θ * rĖ‚ +𝑟⃗ = 2 * atan2(||đ‘Ŗ⃗||, s) / ||đ‘Ŗ⃗|| * đ‘Ŗ⃗ +``` 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 53cc90c5a5..b3ec5eaa86 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 @@ -52,6 +52,48 @@ public class Quaternion { m_z = z; } + /** + * Adds another quaternion to this quaternion entrywise. + * + * @param other The other quaternion. + * @return The quaternion sum. + */ + public Quaternion plus(Quaternion other) { + return new Quaternion( + getW() + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ()); + } + + /** + * Subtracts another quaternion from this quaternion entrywise. + * + * @param other The other quaternion. + * @return The quaternion difference. + */ + public Quaternion minus(Quaternion other) { + return new Quaternion( + getW() - other.getW(), getX() - other.getX(), getY() - other.getY(), getZ() - other.getZ()); + } + + /** + * Divides by a scalar. + * + * @param scalar The value to scale each component by. + * @return The scaled quaternion. + */ + public Quaternion divide(double scalar) { + return new Quaternion(getW() / scalar, getX() / scalar, getY() / scalar, getZ() / scalar); + } + + /** + * Multiplies with a scalar. + * + * @param scalar The value to scale each component by. + * @return The scaled quaternion. + */ + public Quaternion times(double scalar) { + return new Quaternion(getW() * scalar, getX() * scalar, getY() * scalar, getZ() * scalar); + } + /** * Multiply with another quaternion. * @@ -96,12 +138,8 @@ public class Quaternion { if (obj instanceof Quaternion) { var other = (Quaternion) obj; - return Math.abs( - getW() * other.getW() - + getX() * other.getX() - + getY() * other.getY() - + getZ() * other.getZ()) - > 1.0 - 1E-9; + return Math.abs(dot(other) - norm() * other.norm()) < 1e-9 + && Math.abs(norm() - other.norm()) < 1e-9; } return false; } @@ -111,13 +149,45 @@ public class Quaternion { return Objects.hash(m_w, m_x, m_y, m_z); } + /** + * Returns the conjugate of the quaternion. + * + * @return The conjugate quaternion. + */ + public Quaternion conjugate() { + return new Quaternion(getW(), -getX(), -getY(), -getZ()); + } + + /** + * Returns the elementwise product of two quaternions. + * + * @param other The other quaternion. + * @return The dot product of two quaternions. + */ + public double dot(final Quaternion other) { + return getW() * other.getW() + + getX() * other.getX() + + getY() * other.getY() + + getZ() * other.getZ(); + } + /** * Returns the inverse of the quaternion. * * @return The inverse quaternion. */ public Quaternion inverse() { - return new Quaternion(getW(), -getX(), -getY(), -getZ()); + var norm = norm(); + return conjugate().divide(norm * norm); + } + + /** + * Calculates the L2 norm of the quaternion. + * + * @return The L2 norm. + */ + public double norm() { + return Math.sqrt(dot(this)); } /** @@ -126,7 +196,7 @@ public class Quaternion { * @return The normalized quaternion. */ public Quaternion normalize() { - double norm = Math.sqrt(getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ()); + double norm = norm(); if (norm == 0.0) { return new Quaternion(); } else { @@ -134,6 +204,104 @@ public class Quaternion { } } + /** + * Rational power of a quaternion. + * + * @param t the power to raise this quaternion to. + * @return The quaternion power + */ + public Quaternion pow(double t) { + // q^t = e^(ln(q^t)) = e^(t * ln(q)) + return this.log().times(t).exp(); + } + + /** + * Matrix exponential of a quaternion. + * + * @param adjustment the "Twist" that will be applied to this quaternion. + * @return The quaternion product of exp(adjustment) * this + */ + public Quaternion exp(Quaternion adjustment) { + return adjustment.exp().times(this); + } + + /** + * Matrix exponential of a quaternion. + * + *

source: wpimath/algorithms.md + * + *

If this quaternion is in 𝖘𝖔(3) and you are looking for an element of SO(3), use {@link + * fromRotationVector} + * + * @return The Matrix exponential of this quaternion. + */ + public Quaternion exp() { + var scalar = Math.exp(getW()); + + var axial_magnitude = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ()); + var cosine = Math.cos(axial_magnitude); + + double axial_scalar; + + if (axial_magnitude < 1e-9) { + // Taylor series of sin(θ) / θ near θ = 0: 1 − θ²/6 + θ⁴/120 + O(nâļ) + var axial_magnitude_sq = axial_magnitude * axial_magnitude; + var 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 = Math.sin(axial_magnitude) / axial_magnitude; + } + + return new Quaternion( + cosine * scalar, + getX() * axial_scalar * scalar, + getY() * axial_scalar * scalar, + getZ() * axial_scalar * scalar); + } + + /** + * Log operator of a quaternion. + * + * @param end The quaternion to map this quaternion onto. + * @return The "Twist" that maps this quaternion to the argument. + */ + public Quaternion log(Quaternion end) { + return end.times(this.inverse()).log(); + } + + /** + * The Log operator of a general quaternion. + * + *

source: wpimath/algorithms.md + * + *

If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), use {@link + * toRotationVector} + * + * @return The logarithm of this quaternion. + */ + public Quaternion log() { + var scalar = Math.log(norm()); + + var v_norm = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ()); + + var s_norm = getW() / norm(); + + if (Math.abs(s_norm + 1) < 1e-9) { + return new Quaternion(scalar, -Math.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²/3*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; + } + + return new Quaternion(scalar, v_scalar * getX(), v_scalar * getY(), v_scalar * getZ()); + } + /** * Returns W component of the quaternion. * @@ -174,6 +342,37 @@ public class Quaternion { return m_z; } + /** + * Returns the quaternion representation of this rotation vector. + * + *

This is also the exp operator of 𝖘𝖔(3). + * + *

source: wpimath/algorithms.md + * + * @param rvec The rotation vector. + * @return The quaternion representation of this rotation vector. + */ + public static Quaternion fromRotationVector(Vector rvec) { + double theta = rvec.norm(); + + double cos = Math.cos(theta / 2); + + double axial_scalar; + + if (theta < 1e-9) { + // taylor series expansion of sin(θ/2) / θ = 1/2 - θ²/48 + O(θ⁴) + axial_scalar = 1.0 / 2.0 - theta * theta / 48.0; + } else { + axial_scalar = Math.sin(theta / 2) / theta; + } + + return new Quaternion( + cos, + axial_scalar * rvec.get(0, 0), + axial_scalar * rvec.get(1, 0), + axial_scalar * rvec.get(2, 0)); + } + /** * Returns the rotation vector representation of this quaternion. * 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 1ad2f2aff0..467ec1ca24 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 @@ -420,7 +420,7 @@ public class Rotation3d implements Interpolatable { public boolean equals(Object obj) { if (obj instanceof Rotation3d) { var other = (Rotation3d) obj; - return m_q.equals(other.m_q); + return Math.abs(Math.abs(m_q.dot(other.m_q)) - m_q.norm() * other.m_q.norm()) < 1e-9; } return false; } diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 71c3a812d6..ea9b179ca8 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -4,6 +4,8 @@ #include "frc/geometry/Quaternion.h" +#include + #include using namespace frc; @@ -11,6 +13,42 @@ 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; @@ -33,22 +71,95 @@ Quaternion Quaternion::operator*(const Quaternion& other) const { } bool Quaternion::operator==(const Quaternion& other) const { - return std::abs(W() * other.W() + m_v.dot(other.m_v)) > 1.0 - 1E-9; + return std::abs(Dot(other) - Norm() * other.Norm()) < 1e-9 && + std::abs(Norm() - other.Norm()) < 1e-9; } -Quaternion Quaternion::Inverse() const { +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 = std::sqrt(W() * W() + X() * X() + Y() * Y() + Z() * Z()); + double norm = Norm(); if (norm == 0.0) { return Quaternion{}; } else { - return Quaternion{W() / norm, X() / norm, Y() / norm, Z() / norm}; + 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 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; } @@ -83,6 +194,30 @@ Eigen::Vector3d Quaternion::ToRotationVector() const { } } +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/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index d1f18464a1..b4dce35a5d 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -174,6 +174,11 @@ 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}; } diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 13ecbab705..874928135c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -27,6 +27,34 @@ class WPILIB_DLLEXPORT Quaternion { */ Quaternion(double w, double x, double y, double z); + /** + * Adds with another quaternion. + * + * @param other the other quaternion + */ + Quaternion operator+(const Quaternion& other) const; + + /** + * Subtracts another quaternion. + * + * @param other the other quaternion + */ + Quaternion operator-(const Quaternion& other) const; + + /** + * Multiples with a scalar value. + * + * @param other the scalar value + */ + Quaternion operator*(const double other) const; + + /** + * Divides by a scalar value. + * + * @param other the scalar value + */ + Quaternion operator/(const double other) const; + /** * Multiply with another quaternion. * @@ -42,6 +70,16 @@ class WPILIB_DLLEXPORT Quaternion { */ bool operator==(const Quaternion& other) const; + /** + * Returns the elementwise product of two quaternions. + */ + double Dot(const Quaternion& other) const; + + /** + * Returns the conjugate of the quaternion. + */ + Quaternion Conjugate() const; + /** * Returns the inverse of the quaternion. */ @@ -52,6 +90,52 @@ class WPILIB_DLLEXPORT Quaternion { */ Quaternion Normalize() const; + /** + * Calculates the L2 norm of the quaternion. + */ + double Norm() const; + + /** + * Calculates this quaternion raised to a power. + * + * @param t the power to raise this quaternion to. + */ + Quaternion Pow(const double t) const; + + /** + * Matrix exponential of a quaternion. + * + * @param other the "Twist" that will be applied to this quaternion. + */ + Quaternion Exp(const Quaternion& other) const; + + /** + * Matrix exponential of a quaternion. + * + * source: wpimath/algorithms.md + * + * If this quaternion is in 𝖘𝖔(3) and you are looking for an element of + * SO(3), use FromRotationVector + */ + Quaternion Exp() const; + + /** + * Log operator of a quaternion. + * + * @param other The quaternion to map this quaternion onto + */ + Quaternion Log(const Quaternion& other) const; + + /** + * Log operator of a quaternion. + * + * source: wpimath/algorithms.md + * + * If this quaternion is in SO(3) and you are looking for an element of 𝖘𝖔(3), + * use ToRotationVector + */ + Quaternion Log() const; + /** * Returns W component of the quaternion. */ @@ -79,6 +163,15 @@ class WPILIB_DLLEXPORT Quaternion { */ Eigen::Vector3d ToRotationVector() const; + /** + * Returns the quaternion representation of this rotation vector. + * + * This is also the exp operator of 𝖘𝖔(3). + * + * source: wpimath/algorithms.md + */ + static Quaternion FromRotationVector(const Eigen::Vector3d& rvec); + private: // Scalar r in versor form double m_r = 1.0; diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 607fd56a25..6755205572 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -132,7 +132,7 @@ class WPILIB_DLLEXPORT Rotation3d { /** * Checks equality between this Rotation3d and another object. */ - bool operator==(const Rotation3d&) const = default; + bool operator==(const Rotation3d&) const; /** * Adds the new rotation to the current rotation. The other rotation is diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java index 7c7e10301f..458b14dbd3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/QuaternionTest.java @@ -4,7 +4,9 @@ package edu.wpi.first.math.geometry; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; @@ -14,37 +16,91 @@ class QuaternionTest { void testInit() { // Identity var q1 = new Quaternion(); - assertEquals(1.0, q1.getW()); - assertEquals(0.0, q1.getX()); - assertEquals(0.0, q1.getY()); - assertEquals(0.0, q1.getZ()); + assertAll( + () -> assertEquals(1.0, q1.getW()), + () -> assertEquals(0.0, q1.getX()), + () -> assertEquals(0.0, q1.getY()), + () -> assertEquals(0.0, q1.getZ())); // Normalized var q2 = new Quaternion(0.5, 0.5, 0.5, 0.5); - assertEquals(0.5, q2.getW()); - assertEquals(0.5, q2.getX()); - assertEquals(0.5, q2.getY()); - assertEquals(0.5, q2.getZ()); + assertAll( + () -> assertEquals(0.5, q2.getW()), + () -> assertEquals(0.5, q2.getX()), + () -> assertEquals(0.5, q2.getY()), + () -> assertEquals(0.5, q2.getZ())); // Unnormalized var q3 = new Quaternion(0.75, 0.3, 0.4, 0.5); - assertEquals(0.75, q3.getW()); - assertEquals(0.3, q3.getX()); - assertEquals(0.4, q3.getY()); - assertEquals(0.5, q3.getZ()); + assertAll( + () -> assertEquals(0.75, q3.getW()), + () -> assertEquals(0.3, q3.getX()), + () -> assertEquals(0.4, q3.getY()), + () -> assertEquals(0.5, q3.getZ())); - q3 = q3.normalize(); + var q3_norm = q3.normalize(); double norm = Math.sqrt(0.75 * 0.75 + 0.3 * 0.3 + 0.4 * 0.4 + 0.5 * 0.5); - assertEquals(0.75 / norm, q3.getW()); - assertEquals(0.3 / norm, q3.getX()); - assertEquals(0.4 / norm, q3.getY()); - assertEquals(0.5 / norm, q3.getZ()); - assertEquals( - 1.0, - q3.getW() * q3.getW() - + q3.getX() * q3.getX() - + q3.getY() * q3.getY() - + q3.getZ() * q3.getZ()); + assertAll( + () -> assertEquals(0.75 / norm, q3_norm.getW()), + () -> assertEquals(0.3 / norm, q3_norm.getX()), + () -> assertEquals(0.4 / norm, q3_norm.getY()), + () -> assertEquals(0.5 / norm, q3_norm.getZ()), + () -> assertEquals(1.0, q3_norm.dot(q3_norm))); + } + + @Test + void testAddition() { + var q = new Quaternion(0.1, 0.2, 0.3, 0.4); + var p = new Quaternion(0.5, 0.6, 0.7, 0.8); + + var sum = q.plus(p); + assertAll( + () -> assertEquals(q.getW() + p.getW(), sum.getW()), + () -> assertEquals(q.getX() + p.getX(), sum.getX()), + () -> assertEquals(q.getY() + p.getY(), sum.getY()), + () -> assertEquals(q.getZ() + p.getZ(), sum.getZ())); + } + + @Test + void testSubtraction() { + var q = new Quaternion(0.1, 0.2, 0.3, 0.4); + var p = new Quaternion(0.5, 0.6, 0.7, 0.8); + + var difference = q.minus(p); + + assertAll( + () -> assertEquals(q.getW() - p.getW(), difference.getW()), + () -> assertEquals(q.getX() - p.getX(), difference.getX()), + () -> assertEquals(q.getY() - p.getY(), difference.getY()), + () -> assertEquals(q.getZ() - p.getZ(), difference.getZ())); + } + + @Test + void testScalarMultiplication() { + var q = new Quaternion(0.1, 0.2, 0.3, 0.4); + var scalar = 2; + + var product = q.times(scalar); + + assertAll( + () -> assertEquals(q.getW() * scalar, product.getW()), + () -> assertEquals(q.getX() * scalar, product.getX()), + () -> assertEquals(q.getY() * scalar, product.getY()), + () -> assertEquals(q.getZ() * scalar, product.getZ())); + } + + @Test + void testScalarDivision() { + var q = new Quaternion(0.1, 0.2, 0.3, 0.4); + var scalar = 2; + + var product = q.divide(scalar); + + assertAll( + () -> assertEquals(q.getW() / scalar, product.getW()), + () -> assertEquals(q.getX() / scalar, product.getX()), + () -> assertEquals(q.getY() / scalar, product.getY()), + () -> assertEquals(q.getZ() / scalar, product.getZ())); } @Test @@ -59,31 +115,131 @@ class QuaternionTest { // 90° CCW X rotation, 90° CCW Y rotation, and 90° CCW Z rotation should // produce a 90° CCW Y rotation var expected = yRot; - var actual = zRot.times(yRot).times(xRot); - assertEquals(expected.getW(), actual.getW(), 1e-9); - assertEquals(expected.getX(), actual.getX(), 1e-9); - assertEquals(expected.getY(), actual.getY(), 1e-9); - assertEquals(expected.getZ(), actual.getZ(), 1e-9); + final var actual = zRot.times(yRot).times(xRot); + assertAll( + () -> assertEquals(expected.getW(), actual.getW(), 1e-9), + () -> assertEquals(expected.getX(), actual.getX(), 1e-9), + () -> assertEquals(expected.getY(), actual.getY(), 1e-9), + () -> assertEquals(expected.getZ(), actual.getZ(), 1e-9)); // Identity var q = new Quaternion( 0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594); - actual = q.times(q.inverse()); - assertEquals(1.0, actual.getW()); - assertEquals(0.0, actual.getX()); - assertEquals(0.0, actual.getY()); - assertEquals(0.0, actual.getZ()); + final var actual2 = q.times(q.inverse()); + assertAll( + () -> assertEquals(1.0, actual2.getW()), + () -> assertEquals(0.0, actual2.getX()), + () -> assertEquals(0.0, actual2.getY()), + () -> assertEquals(0.0, actual2.getZ())); + } + + @Test + void testConjugate() { + var q = new Quaternion(0.75, 0.3, 0.4, 0.5); + var inv = q.conjugate(); + + assertAll( + () -> assertEquals(q.getW(), inv.getW()), + () -> assertEquals(-q.getX(), inv.getX()), + () -> assertEquals(-q.getY(), inv.getY()), + () -> assertEquals(-q.getZ(), inv.getZ())); } @Test void testInverse() { var q = new Quaternion(0.75, 0.3, 0.4, 0.5); var inv = q.inverse(); + var norm = q.norm(); - assertEquals(q.getW(), inv.getW()); - assertEquals(-q.getX(), inv.getX()); - assertEquals(-q.getY(), inv.getY()); - assertEquals(-q.getZ(), inv.getZ()); + assertAll( + () -> assertEquals(q.getW() / (norm * norm), inv.getW(), 1e-10), + () -> assertEquals(-q.getX() / (norm * norm), inv.getX(), 1e-10), + () -> assertEquals(-q.getY() / (norm * norm), inv.getY(), 1e-10), + () -> assertEquals(-q.getZ() / (norm * norm), inv.getZ(), 1e-10)); + } + + @Test + void testNorm() { + var q = new Quaternion(3, 4, 12, 84); + + // pythagorean triples (3, 4, 5), (5, 12, 13), (13, 84, 85) + assertEquals(q.norm(), 85, 1e-10); + } + + @Test + void testExponential() { + var q = new Quaternion(1.1, 2.2, 3.3, 4.4); + var q_exp = + new Quaternion( + 2.81211398529184, -0.392521193481878, -0.588781790222817, -0.785042386963756); + + assertEquals(q_exp, q.exp()); + } + + @Test + void testLogarithm() { + var q = new Quaternion(1.1, 2.2, 3.3, 4.4); + var q_log = + new Quaternion(1.7959088706354, 0.515190292664085, 0.772785438996128, 1.03038058532817); + + assertEquals(q_log, q.log()); + + var zero = new Quaternion(0, 0, 0, 0); + var one = new Quaternion(); + + assertEquals(zero, one.log()); + + var i = new Quaternion(0, 1, 0, 0); + assertEquals(i.times(Math.PI / 2), i.log()); + + var j = new Quaternion(0, 0, 1, 0); + assertEquals(j.times(Math.PI / 2), j.log()); + + var k = new Quaternion(0, 0, 0, 1); + assertEquals(k.times(Math.PI / 2), k.log()); + assertEquals(i.times(-Math.PI), one.times(-1).log()); + + var ln_half = Math.log(0.5); + assertEquals(new Quaternion(ln_half, -Math.PI, 0, 0), one.times(-0.5).log()); + } + + @Test + void testLogarithmIsInverseOfExponential() { + var q = new Quaternion(1.1, 2.2, 3.3, 4.4); + + // These operations are order-dependent: ln(exp(q)) is congruent + // but not necessarily equal to exp(ln(q)) due to the multi-valued nature of the complex + // logarithm. + + var q_log_exp = q.log().exp(); + + assertEquals(q, q_log_exp); + + var start = new Quaternion(1, 2, 3, 4); + var expect = new Quaternion(5, 6, 7, 8); + + var twist = start.log(expect); + var actual = start.exp(twist); + + assertEquals(expect, actual); + } + + @Test + void testDotProduct() { + var q = new Quaternion(1.1, 2.2, 3.3, 4.4); + var p = new Quaternion(5.5, 6.6, 7.7, 8.8); + + assertEquals( + q.getW() * p.getW() + q.getX() * p.getX() + q.getY() * p.getY() + q.getZ() * p.getZ(), + q.dot(p)); + } + + @Test + void testDotProductAsEquality() { + var q = new Quaternion(1.1, 2.2, 3.3, 4.4); + var q_conj = q.conjugate(); + + assertAll(() -> assertEquals(q, q), () -> assertNotEquals(q, q_conj)); } } diff --git a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp index bf9d24a4b6..ef0e95ea5a 100644 --- a/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/QuaternionTest.cpp @@ -44,6 +44,54 @@ TEST(QuaternionTest, Init) { q3.Z() * q3.Z()); } +TEST(QuaternionTest, Addition) { + Quaternion q{0.1, 0.2, 0.3, 0.4}; + Quaternion p{0.5, 0.6, 0.7, 0.8}; + + auto sum = q + p; + + EXPECT_DOUBLE_EQ(q.W() + p.W(), sum.W()); + EXPECT_DOUBLE_EQ(q.X() + p.X(), sum.X()); + EXPECT_DOUBLE_EQ(q.Y() + p.Y(), sum.Y()); + EXPECT_DOUBLE_EQ(q.Z() + p.Z(), sum.Z()); +} + +TEST(QuaternionTest, Subtraction) { + Quaternion q{0.1, 0.2, 0.3, 0.4}; + Quaternion p{0.5, 0.6, 0.7, 0.8}; + + auto difference = q - p; + + EXPECT_DOUBLE_EQ(q.W() - p.W(), difference.W()); + EXPECT_DOUBLE_EQ(q.X() - p.X(), difference.X()); + EXPECT_DOUBLE_EQ(q.Y() - p.Y(), difference.Y()); + EXPECT_DOUBLE_EQ(q.Z() - p.Z(), difference.Z()); +} + +TEST(QuaternionTest, ScalarMultiplication) { + Quaternion q{0.1, 0.2, 0.3, 0.4}; + auto scalar = 2; + + auto product = q * scalar; + + EXPECT_DOUBLE_EQ(q.W() * scalar, product.W()); + EXPECT_DOUBLE_EQ(q.X() * scalar, product.X()); + EXPECT_DOUBLE_EQ(q.Y() * scalar, product.Y()); + EXPECT_DOUBLE_EQ(q.Z() * scalar, product.Z()); +} + +TEST(QuaternionTest, ScalarDivision) { + Quaternion q{0.1, 0.2, 0.3, 0.4}; + auto scalar = 2; + + auto product = q / scalar; + + EXPECT_DOUBLE_EQ(q.W() / scalar, product.W()); + EXPECT_DOUBLE_EQ(q.X() / scalar, product.X()); + EXPECT_DOUBLE_EQ(q.Y() / scalar, product.Y()); + EXPECT_DOUBLE_EQ(q.Z() / scalar, product.Z()); +} + TEST(QuaternionTest, Multiply) { // 90° CCW rotations around each axis double c = units::math::cos(90_deg / 2.0); @@ -71,13 +119,104 @@ TEST(QuaternionTest, Multiply) { EXPECT_NEAR(0.0, actual.Z(), 1e-9); } +TEST(QuaternionTest, Conjugate) { + Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276, + 0.48507125007266594}; + auto conj = q.Conjugate(); + + EXPECT_DOUBLE_EQ(q.W(), conj.W()); + EXPECT_DOUBLE_EQ(-q.X(), conj.X()); + EXPECT_DOUBLE_EQ(-q.Y(), conj.Y()); + EXPECT_DOUBLE_EQ(-q.Z(), conj.Z()); +} + TEST(QuaternionTest, Inverse) { Quaternion q{0.72760687510899891, 0.29104275004359953, 0.38805700005813276, 0.48507125007266594}; + auto norm = q.Norm(); + auto inv = q.Inverse(); - EXPECT_DOUBLE_EQ(q.W(), inv.W()); - EXPECT_DOUBLE_EQ(-q.X(), inv.X()); - EXPECT_DOUBLE_EQ(-q.Y(), inv.Y()); - EXPECT_DOUBLE_EQ(-q.Z(), inv.Z()); + EXPECT_DOUBLE_EQ(q.W() / (norm * norm), inv.W()); + EXPECT_DOUBLE_EQ(-q.X() / (norm * norm), inv.X()); + EXPECT_DOUBLE_EQ(-q.Y() / (norm * norm), inv.Y()); + EXPECT_DOUBLE_EQ(-q.Z() / (norm * norm), inv.Z()); +} + +TEST(QuaternionTest, Norm) { + Quaternion q{3, 4, 12, 84}; + auto norm = q.Norm(); + + EXPECT_NEAR(85, norm, 1e-9); +} + +TEST(QuaternionTest, Exponential) { + Quaternion q{1.1, 2.2, 3.3, 4.4}; + Quaternion expect{2.81211398529184, -0.392521193481878, -0.588781790222817, + -0.785042386963756}; + + auto q_exp = q.Exp(); + + EXPECT_EQ(expect, q_exp); +} + +TEST(QuaternionTest, Logarithm) { + Quaternion q{1.1, 2.2, 3.3, 4.4}; + Quaternion expect{1.7959088706354, 0.515190292664085, 0.772785438996128, + 1.03038058532817}; + + auto q_log = q.Log(); + + EXPECT_EQ(expect, q_log); + + Quaternion zero{0, 0, 0, 0}; + Quaternion one{1, 0, 0, 0}; + Quaternion i{0, 1, 0, 0}; + Quaternion j{0, 0, 1, 0}; + Quaternion k{0, 0, 0, 1}; + Quaternion ln_half{std::log(0.5), -std::numbers::pi, 0, 0}; + + EXPECT_EQ(zero, one.Log()); + EXPECT_EQ(i * std::numbers::pi / 2, i.Log()); + EXPECT_EQ(j * std::numbers::pi / 2, j.Log()); + EXPECT_EQ(k * std::numbers::pi / 2, k.Log()); + + EXPECT_EQ(i * -std::numbers::pi, (one * -1).Log()); + EXPECT_EQ(ln_half, (one * -0.5).Log()); +} + +TEST(QuaternionTest, LogarithmAndExponentialInverse) { + Quaternion q{1.1, 2.2, 3.3, 4.4}; + + // These operations are order-dependent: ln(exp(q)) is congruent but not + // necessarily equal to exp(ln(q)) due to the multi-valued nature of the + // complex logarithm. + + auto q_log_exp = q.Log().Exp(); + + EXPECT_EQ(q, q_log_exp); + + Quaternion start{1, 2, 3, 4}; + Quaternion expect{5, 6, 7, 8}; + + auto twist = start.Log(expect); + auto actual = start.Exp(twist); + + EXPECT_EQ(expect, actual); +} + +TEST(QuaternionTest, DotProduct) { + Quaternion q{1.1, 2.2, 3.3, 4.4}; + Quaternion p{5.5, 6.6, 7.7, 8.8}; + + EXPECT_NEAR(q.W() * p.W() + q.X() * p.X() + q.Y() * p.Y() + q.Z() * p.Z(), + q.Dot(p), 1e-9); +} + +TEST(QuaternionTest, DotProductAsEquality) { + Quaternion q{1.1, 2.2, 3.3, 4.4}; + auto q_conj = q.Conjugate(); + + EXPECT_NEAR(q.Dot(q), q.Norm() * q.Norm(), 1e-9); + EXPECT_GT(std::abs(q.Dot(q_conj) - q.Norm() * q_conj.Norm()), 1e-9); }