From 94710053538fc6004f2954bc3b20caf73beb8c86 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Sun, 26 Jul 2026 20:21:48 +0000 Subject: [PATCH 1/4] add quartenion --- README.md | 1 + ROADMAP.md | 1 - doc/math/Quaternion.md | 154 +++++++++++ doc/math/README.md | 9 + numerical/math/CMakeLists.txt | 2 + numerical/math/Quaternion.cpp | 6 + numerical/math/Quaternion.hpp | 296 ++++++++++++++++++++++ numerical/math/test/CMakeLists.txt | 1 + numerical/math/test/TestQuaternion.cpp | 150 +++++++++++ roadmap/math/Quaternion/explanation.md | 30 --- roadmap/math/Quaternion/implementation.md | 89 ------- roadmap/math/Quaternion/tests.md | 66 ----- 12 files changed, 619 insertions(+), 186 deletions(-) create mode 100644 doc/math/Quaternion.md create mode 100644 doc/math/README.md create mode 100644 numerical/math/Quaternion.cpp create mode 100644 numerical/math/Quaternion.hpp create mode 100644 numerical/math/test/TestQuaternion.cpp delete mode 100644 roadmap/math/Quaternion/explanation.md delete mode 100644 roadmap/math/Quaternion/implementation.md delete mode 100644 roadmap/math/Quaternion/tests.md diff --git a/README.md b/README.md index a0aba6ca..3c45b13d 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Neural Network](doc/neural_network/README.md) | Layers, activations, losses, model | | [Optimization](doc/optimization/README.md) | Gradient Descent | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | Quaternion | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD | diff --git a/ROADMAP.md b/ROADMAP.md index cd8631a1..caa65a4b 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -32,7 +32,6 @@ Difficulty legend: | 15 | Biquad / Second-Order-Section cascade | `filters/passive` | ★★★☆☆ | | 16 | Notch / comb filter | `filters/passive` | ★★★☆☆ | | 17 | Lead-lag compensator | `controllers` | ★★★☆☆ | -| 18 | Quaternion type | `math` | ★★★☆☆ | | 19 | Luenberger observer + pole placement (Ackermann) | `controllers` | ★★★☆☆ | | 20 | Integral / servo state feedback (LQI) | `controllers` | ★★★☆☆ | | 21 | LMS / NLMS adaptive filter | `estimators/online` | ★★★☆☆ | diff --git a/doc/math/Quaternion.md b/doc/math/Quaternion.md new file mode 100644 index 00000000..ae63119d --- /dev/null +++ b/doc/math/Quaternion.md @@ -0,0 +1,154 @@ +# Quaternion + +## Overview & Motivation + +Three-dimensional attitude representation is a fundamental requirement in robotics, aerospace, +and wearable sensing. Euler angles are intuitive but suffer from gimbal lock — a singularity +that collapses three degrees of freedom into two whenever one angle reaches ±90°. Rotation +matrices avoid this but carry nine words of state and require orthogonality re-enforcement. + +A unit quaternion encodes the same rotation in four words, composes orientations with sixteen +multiply-adds, and is free of singularities. Every modern AHRS filter — Madgwick, Mahony, +Extended Kalman — stores attitude as a unit quaternion precisely because of this combination +of compactness, numerical stability, and algebraic closure. + +## Mathematical Theory + +### Core Definitions + +A quaternion is a hypercomplex number of the form + +$$q = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}$$ + +where $w, x, y, z \in \mathbb{R}$ and the basis elements satisfy + +$$\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{ijk} = -1.$$ + +A **unit quaternion** ($\|q\| = 1$) encodes a rotation by angle $\theta$ about unit axis $\hat{n}$ as + +$$q = \left(\cos\frac{\theta}{2},\; \hat{n}\sin\frac{\theta}{2}\right).$$ + +### Hamilton Product + +Composition of two rotations $q_a$ then $q_b$ is + +$$q_a \otimes q_b = \begin{pmatrix} +w_a w_b - x_a x_b - y_a y_b - z_a z_b \\ +w_a x_b + x_a w_b + y_a z_b - z_a y_b \\ +w_a y_b - x_a z_b + y_a w_b + z_a x_b \\ +w_a z_b + x_a y_b - y_a x_b + z_a w_b +\end{pmatrix}.$$ + +This product is **non-commutative**: $q_a \otimes q_b \neq q_b \otimes q_a$ in general. + +### Vector Rotation + +A pure quaternion $p = (0, \mathbf{v})$ is rotated by + +$$\mathbf{v}' = q \otimes p \otimes q^{-1}.$$ + +For unit $q$ this simplifies (Rodrigues cross-product form) to + +$$\mathbf{v}' = \mathbf{v} + 2w\,(\mathbf{u} \times \mathbf{v}) + 2\,\mathbf{u} \times (\mathbf{u} \times \mathbf{v}),$$ + +where $\mathbf{u} = (x, y, z)$. This costs 15 multiply-adds vs 9 for a pre-built rotation +matrix, making it preferable when rotating one vector. + +### Conjugate and Inverse + +For any quaternion $q^* = (w, -x, -y, -z)$. For a unit quaternion $q^{-1} = q^*$. + +### Rotation Matrix + +$$R(q) = \begin{pmatrix} +1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy) \\ +2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx) \\ +2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) +\end{pmatrix}.$$ + +### Euler Angles (ZYX / 321 convention) + +Converting from unit quaternion to roll $\phi$, pitch $\theta$, yaw $\psi$: + +$$\phi = \operatorname{atan2}(2(wx+yz),\; 1-2(x^2+y^2))$$ +$$\theta = \arcsin(2(wy-zx))$$ +$$\psi = \operatorname{atan2}(2(wz+xy),\; 1-2(y^2+z^2))$$ + +At $\theta = \pm 90°$ the $\phi$ and $\psi$ axes align (gimbal lock); the formula still +returns a bounded value but the decomposition is no longer unique. + +### SLERP + +Spherical Linear Interpolation between unit quaternions $q_0$ and $q_1$ at fraction $t \in [0,1]$: + +$$\operatorname{Slerp}(q_0, q_1, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\,q_0 + \frac{\sin(t\Omega)}{\sin\Omega}\,q_1,$$ + +where $\cos\Omega = q_0 \cdot q_1$. When $\Omega \approx 0$ (nearly parallel quaternions) +the formula degenerates; a normalized linear interpolation (nlerp) is substituted. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------------------|--------|-------|----------------------------------------| +| Hamilton product | O(1) | O(1) | 16 multiply-adds, scalar only | +| Vector rotate | O(1) | O(1) | 15 multiply-adds via cross-product | +| To rotation matrix | O(1) | O(1) | 9 elements, 16 multiplications | +| From rotation matrix | O(1) | O(1) | Branch on largest diagonal | +| SLERP | O(1) | O(1) | 1 acos + 2 sin + scalar blends | +| Euler conversion | O(1) | O(1) | 2 atan2 + 1 asin | + +All operations are stack-only with no heap allocation. + +## Step-by-Step Walkthrough + +Rotating $\hat{x} = (1,0,0)$ by 90° about $\hat{z}$: + +1. Axis-angle: $q = (\cos 45°,\, 0,\, 0,\, \sin 45°) = (\tfrac{\sqrt{2}}{2},\, 0,\, 0,\, \tfrac{\sqrt{2}}{2})$. +2. $\mathbf{u} = (0, 0, \tfrac{\sqrt{2}}{2})$, $\mathbf{v} = (1, 0, 0)$. +3. $\mathbf{t} = 2\,\mathbf{u} \times \mathbf{v} = 2(0 \cdot 0 - \tfrac{\sqrt{2}}{2} \cdot 0,\; \tfrac{\sqrt{2}}{2} \cdot 1 - 0,\; 0) = (0,\, \sqrt{2},\, 0)$. +4. $\mathbf{u} \times \mathbf{t} = (0 \cdot 0 - \tfrac{\sqrt{2}}{2} \cdot \sqrt{2},\; \ldots) = (-1, 0, 0)$. +5. $\mathbf{v}' = (1,0,0) + \tfrac{\sqrt{2}}{2}(0,\sqrt{2},0) + (-1,0,0) = (0,1,0) = \hat{y}$. Correct. + +## Pitfalls & Edge Cases + +- **Drift from unit sphere** — repeated products accumulate floating-point error; renormalize + when $|\|q\|^2 - 1| > \varepsilon$ rather than every step. +- **Double cover** — $q$ and $-q$ represent the same rotation. SLERP flips the sign of $q_1$ + when $q_0 \cdot q_1 < 0$ to guarantee the short arc. +- **Near-parallel SLERP** — when $\cos\Omega > 0.9995$, $\sin\Omega \approx 0$ causes + division instability; nlerp is substituted with identical results to first order. +- **Gimbal lock in ToEulerZYX** — at $\theta = \pm 90°$ the formula clamps pitch and + returns an arbitrary roll/yaw decomposition; the rotation itself remains correct. +- **FromRotationMatrix** — branching on the largest diagonal avoids dividing by a near-zero + value when the rotation is close to 180° about a coordinate axis. + +## Variants & Generalizations + +- **Dual quaternions** — extend to rigid-body transforms (rotation + translation), used in + screw-motion interpolation. +- **Exponential map / log** — convert between the Lie algebra $\mathfrak{so}(3)$ and unit + quaternions, enabling unbiased averaging and covariance propagation. +- **nlerp** — normalized linear interpolation is faster than SLERP but does not maintain + constant angular velocity; acceptable for small arcs or high frame rates. + +## Applications + +- Attitude estimation (AHRS, IMU fusion) — the canonical state representation. +- 3D rigid-body simulation — compose joint rotations without gimbal lock. +- Animation blending — SLERP between keyframe orientations at constant angular speed. +- Computer vision — rotation parameterization in bundle adjustment and PnP solvers. + +## Connections to Other Algorithms + +- `Geometry3D` (`RotationAboutAxis`, `CrossProduct`) — provides the rotation matrix and + vector primitives reused by quaternion conversions. +- Madgwick / Mahony AHRS (item 33) — propagates attitude as a unit quaternion and calls + `operator*` / `Normalize` on every sample. +- CORDIC (item 23) — shift-add approximation of `acos`/`sin` for fixed-point axis-angle + conversions on cores without an FPU. + +## References & Further Reading + +- J. B. Kuipers, *Quaternions and Rotation Sequences*, Princeton University Press, 1999. +- K. Shoemake, "Animating rotation with quaternion curves," *ACM SIGGRAPH*, 1985. +- J. Diebel, "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors," Stanford Technical Report, 2006. diff --git a/doc/math/README.md b/doc/math/README.md new file mode 100644 index 00000000..48f0de8e --- /dev/null +++ b/doc/math/README.md @@ -0,0 +1,9 @@ +# Math + +Core mathematical primitives for numerical computation. + +## Algorithms + +| Algorithm | Description | +|---------------------------------|-----------------------------------------------------------------------------------------------| +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index 536e6719..1d9da261 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(numerical.math PRIVATE LinearTimeInvariant.hpp Matrix.hpp QNumber.hpp + Quaternion.hpp RecursiveBuffer.hpp SingleInstructionMultipleData.hpp Statistics.hpp @@ -30,6 +31,7 @@ numerical_add_coverage_sources(numerical.math LinearTimeInvariant.cpp Matrix.cpp QNumber.cpp + Quaternion.cpp ) add_subdirectory(test) diff --git a/numerical/math/Quaternion.cpp b/numerical/math/Quaternion.cpp new file mode 100644 index 00000000..faa2d4e5 --- /dev/null +++ b/numerical/math/Quaternion.cpp @@ -0,0 +1,6 @@ +#include "numerical/math/Quaternion.hpp" + +namespace math +{ + template class Quaternion; +} diff --git a/numerical/math/Quaternion.hpp b/numerical/math/Quaternion.hpp new file mode 100644 index 00000000..72378e3a --- /dev/null +++ b/numerical/math/Quaternion.hpp @@ -0,0 +1,296 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include +#include +#include + +namespace math +{ + template + class Quaternion + { + static_assert(std::is_floating_point_v, "Quaternion supports floating-point types only"); + + public: + Quaternion(); + Quaternion(T w, T x, T y, T z); + + static Quaternion Identity(); + static Quaternion FromAxisAngle(const Vector3& axis, T angle); + static Quaternion FromRotationMatrix(const Matrix3& r); + static Quaternion FromEulerZYX(T roll, T pitch, T yaw); + static Quaternion Slerp(const Quaternion& a, const Quaternion& b, T t); + + OPTIMIZE_FOR_SPEED Quaternion operator*(const Quaternion& rhs) const; + Quaternion operator-() const; + + Quaternion Conjugate() const; + Quaternion Inverse() const; + T Norm() const; + T SquaredNorm() const; + Quaternion& Normalize(); + Quaternion Normalized() const; + + OPTIMIZE_FOR_SPEED Vector3 Rotate(const Vector3& v) const; + + Matrix3 ToRotationMatrix() const; + Vector3 ToEulerZYX() const; + + T w; + T x; + T y; + T z; + }; + + template + Quaternion::Quaternion() + : w{ T(1) } + , x{ T(0) } + , y{ T(0) } + , z{ T(0) } + {} + + template + Quaternion::Quaternion(T w, T x, T y, T z) + : w{ w } + , x{ x } + , y{ y } + , z{ z } + {} + + template + Quaternion Quaternion::Identity() + { + return Quaternion{ T(1), T(0), T(0), T(0) }; + } + + template + Quaternion Quaternion::FromAxisAngle(const Vector3& axis, T angle) + { + T halfAngle = angle * T(0.5); + T s = std::sin(halfAngle); + T n = VectorNorm(axis); + T invN = (n > T(0)) ? T(1) / n : T(0); + return Quaternion{ std::cos(halfAngle), axis.at(0, 0) * invN * s, axis.at(1, 0) * invN * s, axis.at(2, 0) * invN * s }; + } + + template + Quaternion Quaternion::FromRotationMatrix(const Matrix3& r) + { + T trace = r.at(0, 0) + r.at(1, 1) + r.at(2, 2); + Quaternion q; + if (trace > T(0)) + { + T s = T(0.5) / std::sqrt(trace + T(1)); + q.w = T(0.25) / s; + q.x = (r.at(2, 1) - r.at(1, 2)) * s; + q.y = (r.at(0, 2) - r.at(2, 0)) * s; + q.z = (r.at(1, 0) - r.at(0, 1)) * s; + } + else if (r.at(0, 0) > r.at(1, 1) && r.at(0, 0) > r.at(2, 2)) + { + T s = T(2) * std::sqrt(T(1) + r.at(0, 0) - r.at(1, 1) - r.at(2, 2)); + q.w = (r.at(2, 1) - r.at(1, 2)) / s; + q.x = T(0.25) * s; + q.y = (r.at(0, 1) + r.at(1, 0)) / s; + q.z = (r.at(0, 2) + r.at(2, 0)) / s; + } + else if (r.at(1, 1) > r.at(2, 2)) + { + T s = T(2) * std::sqrt(T(1) + r.at(1, 1) - r.at(0, 0) - r.at(2, 2)); + q.w = (r.at(0, 2) - r.at(2, 0)) / s; + q.x = (r.at(0, 1) + r.at(1, 0)) / s; + q.y = T(0.25) * s; + q.z = (r.at(1, 2) + r.at(2, 1)) / s; + } + else + { + T s = T(2) * std::sqrt(T(1) + r.at(2, 2) - r.at(0, 0) - r.at(1, 1)); + q.w = (r.at(1, 0) - r.at(0, 1)) / s; + q.x = (r.at(0, 2) + r.at(2, 0)) / s; + q.y = (r.at(1, 2) + r.at(2, 1)) / s; + q.z = T(0.25) * s; + } + return q; + } + + template + Quaternion Quaternion::FromEulerZYX(T roll, T pitch, T yaw) + { + T cr = std::cos(roll * T(0.5)); + T sr = std::sin(roll * T(0.5)); + T cp = std::cos(pitch * T(0.5)); + T sp = std::sin(pitch * T(0.5)); + T cy = std::cos(yaw * T(0.5)); + T sy = std::sin(yaw * T(0.5)); + return 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 + }; + } + + template + OPTIMIZE_FOR_SPEED Quaternion Quaternion::operator*(const Quaternion& rhs) const + { + return Quaternion{ + w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z, + w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y, + w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x, + w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w + }; + } + + template + Quaternion Quaternion::operator-() const + { + return Quaternion{ -w, -x, -y, -z }; + } + + template + Quaternion Quaternion::Conjugate() const + { + return Quaternion{ w, -x, -y, -z }; + } + + template + Quaternion Quaternion::Inverse() const + { + T sqn = SquaredNorm(); + T invSqn = (sqn > T(0)) ? T(1) / sqn : T(0); + return Quaternion{ w * invSqn, -x * invSqn, -y * invSqn, -z * invSqn }; + } + + template + T Quaternion::Norm() const + { + return std::sqrt(w * w + x * x + y * y + z * z); + } + + template + T Quaternion::SquaredNorm() const + { + return w * w + x * x + y * y + z * z; + } + + template + Quaternion& Quaternion::Normalize() + { + T n = Norm(); + T invN = (n > T(0)) ? T(1) / n : T(0); + w *= invN; + x *= invN; + y *= invN; + z *= invN; + return *this; + } + + template + Quaternion Quaternion::Normalized() const + { + Quaternion copy{ *this }; + copy.Normalize(); + return copy; + } + + template + OPTIMIZE_FOR_SPEED Vector3 Quaternion::Rotate(const Vector3& v) const + { + Vector3 u{ { x }, { y }, { z } }; + Vector3 t = CrossProduct(u, v); + t.at(0, 0) *= T(2); + t.at(1, 0) *= T(2); + t.at(2, 0) *= T(2); + Vector3 ct = CrossProduct(u, t); + return Vector3{ + { v.at(0, 0) + w * t.at(0, 0) + ct.at(0, 0) }, + { v.at(1, 0) + w * t.at(1, 0) + ct.at(1, 0) }, + { v.at(2, 0) + w * t.at(2, 0) + ct.at(2, 0) } + }; + } + + template + Matrix3 Quaternion::ToRotationMatrix() const + { + T ww = w * w; + T xx = x * x; + T yy = y * y; + T zz = z * z; + T wx = w * x; + T wy = w * y; + T wz = w * z; + T xy = x * y; + T xz = x * z; + T yz = y * z; + return Matrix3{ + { ww + xx - yy - zz, T(2) * (xy - wz), T(2) * (xz + wy) }, + { T(2) * (xy + wz), ww - xx + yy - zz, T(2) * (yz - wx) }, + { T(2) * (xz - wy), T(2) * (yz + wx), ww - xx - yy + zz } + }; + } + + template + Vector3 Quaternion::ToEulerZYX() const + { + T sinR = T(2) * (w * x + y * z); + T cosR = T(1) - T(2) * (x * x + y * y); + T roll = std::atan2(sinR, cosR); + + T sinP = T(2) * (w * y - z * x); + T pitch; + if (std::abs(sinP) >= T(1)) + pitch = std::copysign(std::numbers::pi_v / T(2), sinP); + else + pitch = std::asin(sinP); + + T sinY = T(2) * (w * z + x * y); + T cosY = T(1) - T(2) * (y * y + z * z); + T yaw = std::atan2(sinY, cosY); + + return Vector3{ { roll }, { pitch }, { yaw } }; + } + + template + Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, T t) + { + T d = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z; + Quaternion bAdjusted = b; + if (d < T(0)) + { + bAdjusted = -b; + d = -d; + } + if (d > T(0.9995)) + { + Quaternion result{ + a.w + t * (bAdjusted.w - a.w), + a.x + t * (bAdjusted.x - a.x), + a.y + t * (bAdjusted.y - a.y), + a.z + t * (bAdjusted.z - a.z) + }; + result.Normalize(); + return result; + } + T theta = std::acos(d); + T sinTheta = std::sin(theta); + T s0 = std::sin((T(1) - t) * theta) / sinTheta; + T s1 = std::sin(t * theta) / sinTheta; + return Quaternion{ + s0 * a.w + s1 * bAdjusted.w, + s0 * a.x + s1 * bAdjusted.x, + s0 * a.y + s1 * bAdjusted.y, + s0 * a.z + s1 * bAdjusted.z + }; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class Quaternion; +#endif +} diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index cb983071..b6fd8392 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -12,6 +12,7 @@ target_sources(numerical.math_test PRIVATE TestLinearTimeInvariant.cpp TestQNumber.cpp TestMatrix.cpp + TestQuaternion.cpp TestRecursiveBuffer.cpp TestStatistics.cpp TestToeplitz.cpp diff --git a/numerical/math/test/TestQuaternion.cpp b/numerical/math/test/TestQuaternion.cpp new file mode 100644 index 00000000..6cb7af32 --- /dev/null +++ b/numerical/math/test/TestQuaternion.cpp @@ -0,0 +1,150 @@ +#include "numerical/math/Quaternion.hpp" +#include "numerical/math/Tolerance.hpp" +#include +#include +#include + +namespace +{ + class TestQuaternion + : public ::testing::Test + { + protected: + math::Quaternion identity{ math::Quaternion::Identity() }; + }; +} + +TEST_F(TestQuaternion, IdentityIsNeutralProduct) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 0.5f); + auto r = q * identity; + EXPECT_NEAR(r.w, q.w, math::Tolerance()); + EXPECT_NEAR(r.x, q.x, math::Tolerance()); + EXPECT_NEAR(r.y, q.y, math::Tolerance()); + EXPECT_NEAR(r.z, q.z, math::Tolerance()); +} + +TEST_F(TestQuaternion, HamiltonProductNoncommutative) +{ + auto qx = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, + std::numbers::pi_v / 2.0f); + auto qy = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, + std::numbers::pi_v / 2.0f); + auto qxy = qx * qy; + auto qyx = qy * qx; + bool nonCommutative = (std::abs(qxy.w - qyx.w) > math::Tolerance()) || + (std::abs(qxy.x - qyx.x) > math::Tolerance()) || + (std::abs(qxy.y - qyx.y) > math::Tolerance()) || + (std::abs(qxy.z - qyx.z) > math::Tolerance()); + EXPECT_TRUE(nonCommutative); +} + +TEST_F(TestQuaternion, ConjugateInvertsUnitRotation) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 0.7f); + auto r = q * q.Conjugate(); + EXPECT_NEAR(r.w, 1.0f, math::Tolerance()); + EXPECT_NEAR(r.x, 0.0f, math::Tolerance()); + EXPECT_NEAR(r.y, 0.0f, math::Tolerance()); + EXPECT_NEAR(r.z, 0.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, RotateVectorMatchesMatrix) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 3.0f); + math::Vector3 v{ { 1.0f }, { 0.0f }, { 0.0f } }; + auto rotByQ = q.Rotate(v); + auto r = q.ToRotationMatrix(); + auto rotByM = r * v; + EXPECT_NEAR(rotByQ.at(0, 0), rotByM.at(0, 0), math::Tolerance()); + EXPECT_NEAR(rotByQ.at(1, 0), rotByM.at(1, 0), math::Tolerance()); + EXPECT_NEAR(rotByQ.at(2, 0), rotByM.at(2, 0), math::Tolerance()); +} + +TEST_F(TestQuaternion, AxisAngleRoundTrip) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 2.0f); + math::Vector3 xHat{ { 1.0f }, { 0.0f }, { 0.0f } }; + auto result = q.Rotate(xHat); + EXPECT_NEAR(result.at(0, 0), 0.0f, math::Tolerance()); + EXPECT_NEAR(result.at(1, 0), 1.0f, math::Tolerance()); + EXPECT_NEAR(result.at(2, 0), 0.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, MatrixRoundTrip) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 4.0f); + auto r = q.ToRotationMatrix(); + auto qPrime = math::Quaternion::FromRotationMatrix(r); + bool sameRotation = + (std::abs(qPrime.w - q.w) < math::Tolerance() && std::abs(qPrime.x - q.x) < math::Tolerance() && + std::abs(qPrime.y - q.y) < math::Tolerance() && std::abs(qPrime.z - q.z) < math::Tolerance()) || + (std::abs(qPrime.w + q.w) < math::Tolerance() && std::abs(qPrime.x + q.x) < math::Tolerance() && + std::abs(qPrime.y + q.y) < math::Tolerance() && std::abs(qPrime.z + q.z) < math::Tolerance()); + EXPECT_TRUE(sameRotation); +} + +TEST_F(TestQuaternion, SlerpEndpoints) +{ + auto a = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, 0.3f); + auto b = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, 0.9f); + auto s0 = math::Quaternion::Slerp(a, b, 0.0f); + auto s1 = math::Quaternion::Slerp(a, b, 1.0f); + EXPECT_NEAR(s0.w, a.w, math::Tolerance()); + EXPECT_NEAR(s0.x, a.x, math::Tolerance()); + EXPECT_NEAR(s0.y, a.y, math::Tolerance()); + EXPECT_NEAR(s0.z, a.z, math::Tolerance()); + EXPECT_NEAR(s1.w, b.w, math::Tolerance()); + EXPECT_NEAR(s1.x, b.x, math::Tolerance()); + EXPECT_NEAR(s1.y, b.y, math::Tolerance()); + EXPECT_NEAR(s1.z, b.z, math::Tolerance()); +} + +TEST_F(TestQuaternion, SlerpMidpointConstantSpeed) +{ + auto a = math::Quaternion::Identity(); + auto b = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 2.0f); + auto mid = math::Quaternion::Slerp(a, b, 0.5f); + auto expected = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v / 4.0f); + EXPECT_NEAR(mid.w, expected.w, math::Tolerance()); + EXPECT_NEAR(mid.x, expected.x, math::Tolerance()); + EXPECT_NEAR(mid.y, expected.y, math::Tolerance()); + EXPECT_NEAR(mid.z, expected.z, math::Tolerance()); +} + +TEST_F(TestQuaternion, NormalizeRestoresUnitNorm) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 0.5f); + math::Quaternion scaled{ q.w * 1.1f, q.x * 1.1f, q.y * 1.1f, q.z * 1.1f }; + scaled.Normalize(); + EXPECT_NEAR(scaled.Norm(), 1.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, EulerRoundTrip) +{ + float roll{ 0.3f }; + float pitch{ 0.2f }; + float yaw{ 0.5f }; + auto q = math::Quaternion::FromEulerZYX(roll, pitch, yaw); + auto euler = q.ToEulerZYX(); + EXPECT_NEAR(euler.at(0, 0), roll, math::Tolerance()); + EXPECT_NEAR(euler.at(1, 0), pitch, math::Tolerance()); + EXPECT_NEAR(euler.at(2, 0), yaw, math::Tolerance()); +} diff --git a/roadmap/math/Quaternion/explanation.md b/roadmap/math/Quaternion/explanation.md deleted file mode 100644 index d4a82eb6..00000000 --- a/roadmap/math/Quaternion/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# Quaternion — Overview - -## What it is -A four-component hypercomplex number `w + xi + yj + zk` that, constrained to unit norm, encodes a -3D rotation. Quaternions compose rotations by multiplication and rotate vectors by conjugation — -without the singularities that plague Euler angles. - -## Why it matters (embedded) -Attitude tracking on drones, robots, and wearables needs a rotation representation that is compact -(4 words vs a 9-word matrix), cheap to compose, and **free of gimbal lock**. A unit quaternion is -the standard state carried by every AHRS filter, and its components live in [-1, 1]. - -## How it works (intuition) -A rotation by angle `θ` about a unit axis `n̂` is stored as `(cos θ/2, n̂·sin θ/2)`. Multiplying two -quaternions composes their rotations (the Hamilton product). Rotating a vector is `q·(0,v)·q⁻¹`, -which simplifies to a pair of cross products. Interpolating between two orientations along the -shortest constant-speed arc is **SLERP** — spherical linear interpolation on the unit 4-sphere. - -## Key parameters -- **Unit-norm constraint** — only unit quaternions rotate; renormalize to fight drift. -- **SLERP parameter `t ∈ [0, 1]`** — fraction along the arc between two orientations. -- **Euler convention** — the roll-pitch-yaw order (ZYX here) for interoperability. - -## Reference -J. B. Kuipers, *Quaternions and Rotation Sequences* (1999); K. Shoemake, "Animating rotation with -quaternion curves," *SIGGRAPH*, 1985 (SLERP). - -## See also -`Geometry3D` (`RotationAboutAxis`, `CrossProduct`), `AhrsMadgwickMahony` (item 33, the main -consumer), `Cordic` (shift-add trig for the axis-angle conversions). diff --git a/roadmap/math/Quaternion/implementation.md b/roadmap/math/Quaternion/implementation.md deleted file mode 100644 index 330dcac6..00000000 --- a/roadmap/math/Quaternion/implementation.md +++ /dev/null @@ -1,89 +0,0 @@ -# Quaternion — Implementation Pseudocode - -> Roadmap ref: #18 (Tier 3) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class Quaternion: - T w, x, y, z # w + xi + yj + zk; unit norm for rotations -``` - -Store as four scalars (or reuse `Vector`). For a rotation every component lies in -[-1, 1]. - -## Interface - -``` -Quaternion(T w = 1, T x = 0, T y = 0, T z = 0) -static Quaternion Identity() -static Quaternion FromAxisAngle(Vector3 axis, T angle) -static Quaternion FromRotationMatrix(Matrix3 R) -Matrix3 ToRotationMatrix() -Vector3 ToEuler() # roll-pitch-yaw (ZYX) -Quaternion operator*(Quaternion rhs) # Hamilton product; hot path -Quaternion Conjugate() -Quaternion Inverse() -T Norm() / T SquaredNorm() -Quaternion& Normalize() -Vector3 Rotate(Vector3 v) # hot path -static Quaternion Slerp(Quaternion a, Quaternion b, T t) -``` - -## Algorithm (pseudocode) - -``` -function multiply(a, b): # OPTIMIZE_FOR_SPEED — Hamilton product - w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z - x = a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y - y = a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x - z = a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w - return Quaternion(w, x, y, z) - -function Rotate(v): # OPTIMIZE_FOR_SPEED - # v' = q * (0,v) * q⁻¹ via the cheaper cross-product form - u = (x, y, z) - t = 2 * CrossProduct(u, v) - return v + w*t + CrossProduct(u, t) - -function Slerp(a, b, t): - d = dot(a, b) # cosine of the half-angle - if d < 0: b = -b; d = -d # take the short arc (double cover) - if d > 0.9995: return Normalize(a + t*(b - a)) # near-parallel -> lerp - theta = acos(d) - return (sin((1-t)*theta)*a + sin(t*theta)*b) / sin(theta) - -function Normalize(): - n = sqrt(w² + x² + y² + z²) - divide every component by n -``` - -## Complexity & memory - -- Product / rotate: `O(1)` — 16 / 15 multiply-adds, no loops. -- SLERP: `O(1)` plus one `acos` and two `sin`. -- Memory: four scalars per quaternion; entirely on the stack, no heap. - -## Numerical / embedded notes - -- **Renormalize periodically** — repeated products drift off the unit sphere; renormalize when - `|SquaredNorm − 1| > eps` rather than every step to save cycles. -- Double cover: `q` and `−q` are the same rotation — the SLERP sign flip selects the short path. -- Prefer the cross-product `Rotate` form (15 MACs) when rotating a single vector; build - `ToRotationMatrix` only when many vectors share one rotation. -- `FromRotationMatrix` must branch on the largest diagonal term to avoid dividing by a near-zero. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/math/Quaternion.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `operator*`/`Rotate`, and - `extern template class Quaternion;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/Quaternion.cpp` → `template class Quaternion;` -- Test: `numerical/math/test/TestQuaternion.cpp` -- Doc: `doc/math/Quaternion.md` (new folder; math currently has no doc pages) -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestQuaternion.cpp` → `numerical.math_test`. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/math/Quaternion/tests.md b/roadmap/math/Quaternion/tests.md deleted file mode 100644 index 9815010d..00000000 --- a/roadmap/math/Quaternion/tests.md +++ /dev/null @@ -1,66 +0,0 @@ -# Quaternion — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestQuaternion : public ::testing::Test: - Quaternion identity = Quaternion::Identity() -# each case below is a TEST_F(TestQuaternion, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -identity_is_neutral_product: - Arrange: q = FromAxisAngle(ẑ, 0.5) - Act: r = q * Identity() - Assert: r ≈ q - -hamilton_product_noncommutative: - Arrange: qx = rot(x̂, 90°), qy = rot(ŷ, 90°) - Assert: qx*qy ≠ qy*qx (against documented reference values) - -conjugate_inverts_unit_rotation: - Arrange: unit q - Assert: q * Conjugate(q) ≈ Identity - -rotate_vector_matches_matrix: - Arrange: q from axis-angle, vector v - Assert: q.Rotate(v) ≈ q.ToRotationMatrix() * v - -axis_angle_round_trip: - Arrange: axis ẑ, angle 90° - Assert: rotates x̂ -> ŷ (±tol) - -matrix_round_trip: - Arrange: q -> R -> q' - Assert: q' ≈ ±q (same rotation, double cover) - -slerp_endpoints: - Assert: Slerp(a, b, 0) ≈ a and Slerp(a, b, 1) ≈ b - -slerp_midpoint_constant_speed: - Arrange: a = Identity, b = rot(ẑ, 90°) - Assert: Slerp(a, b, 0.5) ≈ rot(ẑ, 45°) - -normalize_restores_unit_norm: - Arrange: scale a unit q by 1.1 - Assert: Norm() ≈ 1 after Normalize() - -euler_round_trip: - Arrange: roll/pitch/yaw within the non-gimbal range - Assert: ToEuler(FromEuler(rpy)) ≈ rpy -``` - -## Reference vectors - -- `rot(ẑ, 90°)` ⇒ `q = (√2/2, 0, 0, √2/2)`, maps `x̂ → ŷ`. -- `Slerp(Identity, rot(ẑ, 90°), 0.5) = rot(ẑ, 45°)`. - -## Edge cases - -- Antipodal SLERP inputs (`dot < 0`) ⇒ short-arc sign flip. -- Near-parallel inputs (`dot > 0.9995`) ⇒ normalized-lerp fallback (no `acos` blow-up). -- Gimbal-lock pitch = ±90° in `ToEuler` ⇒ documented degenerate handling. From 11123531f71139b6dd00a86e1f51007e3da14eab Mon Sep 17 00:00:00 2001 From: gfs Date: Sun, 26 Jul 2026 22:24:45 +0200 Subject: [PATCH 2/4] Apply suggestions from code review Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- doc/math/Quaternion.md | 16 ++++++++-------- doc/math/README.md | 6 +++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/doc/math/Quaternion.md b/doc/math/Quaternion.md index ae63119d..b85984d0 100644 --- a/doc/math/Quaternion.md +++ b/doc/math/Quaternion.md @@ -88,14 +88,14 @@ the formula degenerates; a normalized linear interpolation (nlerp) is substitute ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------------------|--------|-------|----------------------------------------| -| Hamilton product | O(1) | O(1) | 16 multiply-adds, scalar only | -| Vector rotate | O(1) | O(1) | 15 multiply-adds via cross-product | -| To rotation matrix | O(1) | O(1) | 9 elements, 16 multiplications | -| From rotation matrix | O(1) | O(1) | Branch on largest diagonal | -| SLERP | O(1) | O(1) | 1 acos + 2 sin + scalar blends | -| Euler conversion | O(1) | O(1) | 2 atan2 + 1 asin | +| Operation | Time | Space | Notes | +|----------------------|------|-------|------------------------------------| +| Hamilton product | O(1) | O(1) | 16 multiply-adds, scalar only | +| Vector rotate | O(1) | O(1) | 15 multiply-adds via cross-product | +| To rotation matrix | O(1) | O(1) | 9 elements, 16 multiplications | +| From rotation matrix | O(1) | O(1) | Branch on largest diagonal | +| SLERP | O(1) | O(1) | 1 acos + 2 sin + scalar blends | +| Euler conversion | O(1) | O(1) | 2 atan2 + 1 asin | All operations are stack-only with no heap allocation. diff --git a/doc/math/README.md b/doc/math/README.md index 48f0de8e..d07bdc62 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -4,6 +4,6 @@ Core mathematical primitives for numerical computation. ## Algorithms -| Algorithm | Description | -|---------------------------------|-----------------------------------------------------------------------------------------------| -| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | +| Algorithm | Description | +|-----------------------------|-----------------------------------------------------------------------------------------------| +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | From fe8bd89e89c74accf8bd3a4d4bb00aa1581fd333 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Mon, 27 Jul 2026 06:01:14 +0000 Subject: [PATCH 3/4] increase unit tests --- numerical/math/test/TestQuaternion.cpp | 124 +++++++++++++++++++++++++ 1 file changed, 124 insertions(+) diff --git a/numerical/math/test/TestQuaternion.cpp b/numerical/math/test/TestQuaternion.cpp index 6cb7af32..eb1cc79b 100644 --- a/numerical/math/test/TestQuaternion.cpp +++ b/numerical/math/test/TestQuaternion.cpp @@ -148,3 +148,127 @@ TEST_F(TestQuaternion, EulerRoundTrip) EXPECT_NEAR(euler.at(1, 0), pitch, math::Tolerance()); EXPECT_NEAR(euler.at(2, 0), yaw, math::Tolerance()); } + +TEST_F(TestQuaternion, UnaryNegation) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 0.5f); + auto neg = -q; + EXPECT_NEAR(neg.w, -q.w, math::Tolerance()); + EXPECT_NEAR(neg.x, -q.x, math::Tolerance()); + EXPECT_NEAR(neg.y, -q.y, math::Tolerance()); + EXPECT_NEAR(neg.z, -q.z, math::Tolerance()); +} + +TEST_F(TestQuaternion, InverseOfUnitQuaternion) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, 0.8f); + auto r = q * q.Inverse(); + EXPECT_NEAR(r.w, 1.0f, math::Tolerance()); + EXPECT_NEAR(r.x, 0.0f, math::Tolerance()); + EXPECT_NEAR(r.y, 0.0f, math::Tolerance()); + EXPECT_NEAR(r.z, 0.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, SquaredNormOfUnitQuaternion) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, 1.2f); + EXPECT_NEAR(q.SquaredNorm(), 1.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, NormalizedReturnsNewCopy) +{ + math::Quaternion scaled{ 2.0f, 0.0f, 0.0f, 0.0f }; + auto n = scaled.Normalized(); + EXPECT_NEAR(n.Norm(), 1.0f, math::Tolerance()); + EXPECT_NEAR(scaled.Norm(), 2.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, FromRotationMatrixXDominant) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, + std::numbers::pi_v); + auto r = q.ToRotationMatrix(); + auto qPrime = math::Quaternion::FromRotationMatrix(r); + bool sameRotation = + (std::abs(qPrime.w - q.w) < math::Tolerance() && std::abs(qPrime.x - q.x) < math::Tolerance() && + std::abs(qPrime.y - q.y) < math::Tolerance() && std::abs(qPrime.z - q.z) < math::Tolerance()) || + (std::abs(qPrime.w + q.w) < math::Tolerance() && std::abs(qPrime.x + q.x) < math::Tolerance() && + std::abs(qPrime.y + q.y) < math::Tolerance() && std::abs(qPrime.z + q.z) < math::Tolerance()); + EXPECT_TRUE(sameRotation); +} + +TEST_F(TestQuaternion, FromRotationMatrixYDominant) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 1.0f }, { 0.0f } }, + std::numbers::pi_v); + auto r = q.ToRotationMatrix(); + auto qPrime = math::Quaternion::FromRotationMatrix(r); + bool sameRotation = + (std::abs(qPrime.w - q.w) < math::Tolerance() && std::abs(qPrime.x - q.x) < math::Tolerance() && + std::abs(qPrime.y - q.y) < math::Tolerance() && std::abs(qPrime.z - q.z) < math::Tolerance()) || + (std::abs(qPrime.w + q.w) < math::Tolerance() && std::abs(qPrime.x + q.x) < math::Tolerance() && + std::abs(qPrime.y + q.y) < math::Tolerance() && std::abs(qPrime.z + q.z) < math::Tolerance()); + EXPECT_TRUE(sameRotation); +} + +TEST_F(TestQuaternion, FromRotationMatrixZDominant) +{ + auto q = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, + std::numbers::pi_v); + auto r = q.ToRotationMatrix(); + auto qPrime = math::Quaternion::FromRotationMatrix(r); + bool sameRotation = + (std::abs(qPrime.w - q.w) < math::Tolerance() && std::abs(qPrime.x - q.x) < math::Tolerance() && + std::abs(qPrime.y - q.y) < math::Tolerance() && std::abs(qPrime.z - q.z) < math::Tolerance()) || + (std::abs(qPrime.w + q.w) < math::Tolerance() && std::abs(qPrime.x + q.x) < math::Tolerance() && + std::abs(qPrime.y + q.y) < math::Tolerance() && std::abs(qPrime.z + q.z) < math::Tolerance()); + EXPECT_TRUE(sameRotation); +} + +TEST_F(TestQuaternion, SlerpAntipodalFlipsSignAndMatchesDirectPath) +{ + auto a = math::Quaternion::FromAxisAngle( + math::Vector3{ { 1.0f }, { 0.0f }, { 0.0f } }, 0.4f); + auto b = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 0.6f); + auto s1 = math::Quaternion::Slerp(a, b, 0.5f); + auto s2 = math::Quaternion::Slerp(a, -b, 0.5f); + bool sameRotation = + (std::abs(s1.w - s2.w) < math::Tolerance() && std::abs(s1.x - s2.x) < math::Tolerance() && + std::abs(s1.y - s2.y) < math::Tolerance() && std::abs(s1.z - s2.z) < math::Tolerance()) || + (std::abs(s1.w + s2.w) < math::Tolerance() && std::abs(s1.x + s2.x) < math::Tolerance() && + std::abs(s1.y + s2.y) < math::Tolerance() && std::abs(s1.z + s2.z) < math::Tolerance()); + EXPECT_TRUE(sameRotation); +} + +TEST_F(TestQuaternion, SlerpNearParallelFallsBackToLerp) +{ + auto a = math::Quaternion::Identity(); + auto b = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 1e-4f); + auto mid = math::Quaternion::Slerp(a, b, 0.5f); + auto expected = math::Quaternion::FromAxisAngle( + math::Vector3{ { 0.0f }, { 0.0f }, { 1.0f } }, 5e-5f); + EXPECT_NEAR(mid.w, expected.w, math::Tolerance()); + EXPECT_NEAR(mid.z, expected.z, math::Tolerance()); +} + +TEST_F(TestQuaternion, ToEulerGimbalLockPositivePitch) +{ + auto q = math::Quaternion::FromEulerZYX(0.0f, std::numbers::pi_v / 2.0f, 0.0f); + auto euler = q.ToEulerZYX(); + EXPECT_NEAR(euler.at(1, 0), std::numbers::pi_v / 2.0f, math::Tolerance()); +} + +TEST_F(TestQuaternion, ToEulerGimbalLockNegativePitch) +{ + auto q = math::Quaternion::FromEulerZYX(0.0f, -std::numbers::pi_v / 2.0f, 0.0f); + auto euler = q.ToEulerZYX(); + EXPECT_NEAR(euler.at(1, 0), -std::numbers::pi_v / 2.0f, math::Tolerance()); +} From b17400f7e997d0adce146fcb65cf7f1c5a0b1938 Mon Sep 17 00:00:00 2001 From: Gabriel Santos Date: Mon, 27 Jul 2026 06:01:50 +0000 Subject: [PATCH 4/4] increase coverage --- numerical/math/test/TestQuaternion.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/numerical/math/test/TestQuaternion.cpp b/numerical/math/test/TestQuaternion.cpp index eb1cc79b..b37a2524 100644 --- a/numerical/math/test/TestQuaternion.cpp +++ b/numerical/math/test/TestQuaternion.cpp @@ -261,14 +261,16 @@ TEST_F(TestQuaternion, SlerpNearParallelFallsBackToLerp) TEST_F(TestQuaternion, ToEulerGimbalLockPositivePitch) { - auto q = math::Quaternion::FromEulerZYX(0.0f, std::numbers::pi_v / 2.0f, 0.0f); + // w=0.72, y=0.72 → sinP = 2*0.72*0.72 = 1.0368 > 1, forcing copysign branch + math::Quaternion q{ 0.72f, 0.0f, 0.72f, 0.0f }; auto euler = q.ToEulerZYX(); EXPECT_NEAR(euler.at(1, 0), std::numbers::pi_v / 2.0f, math::Tolerance()); } TEST_F(TestQuaternion, ToEulerGimbalLockNegativePitch) { - auto q = math::Quaternion::FromEulerZYX(0.0f, -std::numbers::pi_v / 2.0f, 0.0f); + // w=0.72, y=-0.72 → sinP = -1.0368 < -1, forcing copysign with negative sign + math::Quaternion q{ 0.72f, 0.0f, -0.72f, 0.0f }; auto euler = q.ToEulerZYX(); EXPECT_NEAR(euler.at(1, 0), -std::numbers::pi_v / 2.0f, math::Tolerance()); }