diff --git a/CMakeLists.txt b/CMakeLists.txt index be8a6f4..1ee48eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -110,6 +110,7 @@ add_library(fireengine SHARED src/graphics/shadow_caster_deformation.cpp src/graphics/shadow_render_view.cpp src/graphics/shadow_view_disposition.cpp + src/math/rotation3.cpp src/graphics/shadow_diagnostics.cpp src/graphics/shadow_view.cpp src/graphics/vipm.cpp @@ -410,6 +411,7 @@ add_executable(test_fire_engine tests/physics/test_physics_handle.cpp tests/physics/test_physics_determinism.cpp tests/physics/test_demos.cpp + tests/math/test_rotation3.cpp tests/math/test_scalar.cpp tests/math/test_mat3.cpp tests/math/test_singular_value.cpp diff --git a/include/fire_engine/math/rotation3.hpp b/include/fire_engine/math/rotation3.hpp new file mode 100644 index 0000000..fb1e752 --- /dev/null +++ b/include/fire_engine/math/rotation3.hpp @@ -0,0 +1,414 @@ +#pragma once + +#include +#include + +#include +#include +#include + +// A 3D ROTATION — a value that is always a rotation, as opposed to four numbers that usually are +// (tier-0 review, finding 3). +// +// `Quaternion` is freely constructible and mutable while most of its API assumes unit length: +// `rotate`, `slerp`, `toMat4`, `toEulerXYZ` and `Mat3::fromQuaternion` all produce nonsense from a +// non-unit value, quietly. `fromAxisAngle({0, 0, 0}, angle)` is the clearest case — there is no +// rotation being described, and what comes back is not one either. +// +// WHY THIS EXISTS, given that nothing violates the invariant today. It was measured before it was +// designed: about 24 million observations across the whole test suite and three scenes (including a +// 900-frame ragdoll run) found ZERO values off unit by more than 1e-4, with the worst deviation +// anywhere at 7.15e-07 — roughly six ulps, and not growing. So this type is not fixing a live bug. +// It removes a representable invalid state, which is a different and longer-lived kind of value: +// the reason nothing violates the invariant today is that a handful of scattered `normalise()` +// calls happen to be in the right places, and nothing makes that true tomorrow. +// +// THE INVARIANT IS MAINTAINED BY EVERY OPERATION, not by a periodic repair, and it is maintained +// DEFINITIVELY. That distinction is what the measurement actually supports: `Quaternion::integrate` +// normalises its result and `slerp` normalises its near-linear path, so the bounded drift observed +// is evidence that THOSE repairs are sufficient — not that a rotation type could skip them. Every +// operation here that does floating-point algebra therefore ends at one private choke point, and +// that choke point cannot complete with a non-unit or non-finite value: it terminates instead. +// Normalising a NaN would otherwise store NaNs in a type whose whole claim is that it holds a +// rotation. +// +// NAMED FOR THE VALUE, not the representation: a caller wants a rotation, and that it is stored as +// a unit quaternion is this type's business. The quaternion is reachable (`quaternion()`) for the +// places that genuinely need four-component algebra — swing-twist decomposition in the joint +// solver, glTF CUBICSPLINE tangents, which are derivatives rather than orientations — and those +// stay on `Quaternion` deliberately. + +namespace fire_engine +{ + +// TWO TOLERANCES, because they answer two different questions and one number cannot. +// +// ADMISSION: how far a raw four-component value may sit from unit length and still be understood as +// a rotation that merely needs normalising. Sized for AUTHORED DATA, not for the engine's own +// 7e-07 internal drift, and specifically for the COARSEST encoding glTF permits for rotation +// animation outputs — normalised signed bytes, where each component decodes as round(c·127)/127. +// +// The bound is derived, not guessed. A component's quantisation error is at most 0.5/127, so +// |‖q‖² − 1| ≤ 2·Σ|cᵢ|·(0.5/127) + 4·(0.5/127)² +// and Σ|cᵢ| is maximal at 2 for a unit quaternion (all four components ±0.5), giving 0.01581. That +// worst case is real rather than theoretical: {0.5, 0.5, 0.5, 0.5} encodes as 64/127 per component +// and decodes to a squared norm of 1.015810, which a 1e-2 tolerance would have rejected — a +// conforming asset refused by the loader. 0.05 clears it by about three times. +// +// Anything beyond this is not an imprecise rotation but a different kind of value (a derivative, an +// uninitialised field, a scaled quaternion someone forgot to normalise), and it is refused rather +// than silently rescued: {0, 0, 10, 10} deviates by 199. +inline constexpr float kRotationAdmissionToleranceSquared = 0.05f; + +// INVARIANT: how far a value this type has already accepted may sit from unit length before +// something is wrong with this type. Sized from the reconnaissance — the worst deviation observed +// anywhere in the engine was 7.15e-07, about 1.4e-06 squared — so 1e-4 leaves two decades of +// headroom while still catching a genuine failure of the normalisation path. +// +// BOTH ARE MEASURED ON |‖q‖² − 1|, the SQUARED deviation, and that is pinned here because the two +// spellings differ by a factor of two (|‖q‖² − 1| ≈ 2·|‖q‖ − 1| for small deviations): a linear +// tolerance transcribed in unchanged would be twice as strict as intended, and vice versa. +inline constexpr float kRotationUnitToleranceSquared = 1.0e-4f; + +class Rotation3 +{ +public: + // The identity rotation. A default-constructed `Rotation3` IS a rotation — there is no empty or + // invalid state to check for, which is the whole point of the type. + constexpr Rotation3() noexcept = default; + constexpr Rotation3(const Rotation3&) noexcept = default; + constexpr Rotation3(Rotation3&&) noexcept = default; + constexpr Rotation3& operator=(const Rotation3&) noexcept = default; + constexpr Rotation3& operator=(Rotation3&&) noexcept = default; + ~Rotation3() = default; + + [[nodiscard]] static constexpr Rotation3 identity() noexcept + { + return Rotation3{}; + } + + // ADMISSION from four raw components. Normalises what it accepts, so an orientation that has + // drifted a few ulps — or a quantised glTF keyframe — becomes an exact rotation rather than + // being rejected for imprecision. + // + // It refuses what is not a rotation: a non-finite component, a magnitude too small to carry a + // direction, or a deviation beyond `kRotationAdmissionToleranceSquared`. That last one is the + // difference between "imprecise" and "a different kind of value": {0, 0, 10, 10} normalises to + // a perfectly good rotation, and accepting it would mean this factory could not tell a rotation + // from a derivative. Answering the identity for any of them would launder a producer bug into a + // plausible value that rotates nothing. + [[nodiscard]] static std::optional tryFromQuaternion(const Quaternion& q) noexcept + { + if (!std::isfinite(q.x()) || !std::isfinite(q.y()) || !std::isfinite(q.z()) || + !std::isfinite(q.w())) + { + return std::nullopt; + } + const float squared = q.magnitudeSquared(); + if (!std::isfinite(squared) || + std::fabs(squared - 1.0f) > kRotationAdmissionToleranceSquared) + { + return std::nullopt; + } + // NOT `Rotation3{Quaternion::normalise(q)}`: the choke point normalises, so pre-normalising + // here would round twice for one value. + return Rotation3{q}; + } + + // A rotation of `angle` radians about `axis`. The AXIS IS NORMALISED — a caller with a + // direction of length 0.9998, or of length 7, means the direction — so the failure condition is + // degenerate geometry (zero-length or non-finite), never "you did not hand me a unit vector". + // `angle` itself must be finite; there is no rotation by NaN radians. + [[nodiscard]] static std::optional tryFromAxisAngle(const Vec3& axis, + float angle) noexcept + { + if (!std::isfinite(angle) || !isFinite(axis) || axis.magnitude() < float_normalise_cutoff) + { + return std::nullopt; + } + const Vec3 unitAxis = Vec3::normalise(axis); + const float half = angle * 0.5f; + const float s = std::sin(half); + return Rotation3{ + Quaternion{unitAxis.x() * s, unitAxis.y() * s, unitAxis.z() * s, std::cos(half)}}; + } + + // The shortest rotation taking `from` to `to`. Both are normalised, so lengths are irrelevant + // and only directions matter; degenerate or non-finite input is refused. Antiparallel input is + // NOT degenerate — it is a well-defined 180° rotation about some perpendicular axis. + [[nodiscard]] static std::optional tryFromVectors(const Vec3& from, + const Vec3& to) noexcept + { + if (!isFinite(from) || !isFinite(to) || from.magnitude() < float_normalise_cutoff || + to.magnitude() < float_normalise_cutoff) + { + return std::nullopt; + } + const Vec3 f = Vec3::normalise(from); + const Vec3 t = Vec3::normalise(to); + const float d = Vec3::dotProduct(f, t); + + // ANTIPARALLEL: the half-angle form below degenerates to the zero quaternion, because there + // is no shortest arc — every axis perpendicular to `f` turns it into `t` through 180°. One + // is chosen deterministically, from whichever cardinal axis `f` is least aligned with, so + // the result is stable rather than dependent on rounding. + constexpr float kAntiparallel = -1.0f + 1.0e-6f; + if (d <= kAntiparallel) + { + const Vec3 reference = + std::fabs(f.x()) < 0.9f ? Vec3{1.0f, 0.0f, 0.0f} : Vec3{0.0f, 1.0f, 0.0f}; + const Vec3 axis = Vec3::normalise(Vec3::crossProduct(f, reference)); + return Rotation3{Quaternion{axis.x(), axis.y(), axis.z(), 0.0f}}; + } + + // The half-angle form, left UNNORMALISED for the choke point to finish — one rounding for + // the whole factory rather than one here and another on construction. + const Vec3 axis = Vec3::crossProduct(f, t); + return Rotation3{Quaternion{axis.x(), axis.y(), axis.z(), 1.0f + d}}; + } + + // The unit quaternion behind this rotation, for the places that genuinely need four-component + // algebra (swing-twist decomposition, spline tangents) and for conversion. Always unit, so a + // caller can rely on that without checking. + [[nodiscard]] constexpr const Quaternion& quaternion() const noexcept + { + return q_; + } + + [[nodiscard]] constexpr Vec3 rotate(const Vec3& v) const noexcept + { + return q_.rotate(v); + } + + // The inverse rotation. Exact for a unit quaternion — conjugation flips three signs and touches + // no magnitudes — so this is the one algebraic member that cannot perturb the invariant. + [[nodiscard]] constexpr Rotation3 inverse() const noexcept + { + return Rotation3{q_.conjugate(), AlreadyUnit{}}; + } + + // Composition: `a * b` applies b, then a. Renormalised, because a product of two unit + // quaternions is unit only in exact arithmetic — and a chain of them (a skeleton, an + // articulation) compounds the error link by link. + [[nodiscard]] Rotation3 operator*(const Rotation3& rhs) const noexcept + { + return Rotation3{q_ * rhs.q_}; + } + + [[nodiscard]] Vec3 operator*(const Vec3& v) const noexcept + { + return rotate(v); + } + + // Advance by an angular velocity over `dt` (exponential map): build the incremental rotation + // from the rotation vector ω·dt and compose. NORMALISED EXACTLY ONCE, at the choke point — the + // formula lives here rather than delegating to `Quaternion::integrate`, which normalises its + // own result and would leave this rounding twice for no benefit. + // + // Non-finite ω or dt is a corrupt simulation state, not a case to interpolate through, and the + // choke point terminates on it rather than storing NaNs. + [[nodiscard]] Rotation3 integrate(const Vec3& omega, float dt) const noexcept + { + const Vec3 rotationVector = omega * dt; + const float angle = rotationVector.magnitude(); + Quaternion delta; + if (angle < float_normalise_cutoff) + { + // Small angle: Δq ≈ {ω·dt/2, 1}, normalised by the choke point below. + delta = Quaternion{rotationVector.x() * 0.5f, rotationVector.y() * 0.5f, + rotationVector.z() * 0.5f, 1.0f}; + } + else + { + const float half = angle * 0.5f; + const float s = std::sin(half) / angle; + delta = Quaternion{rotationVector.x() * s, rotationVector.y() * s, + rotationVector.z() * s, std::cos(half)}; + } + return Rotation3{delta * q_}; + } + + // Spherical interpolation. Also normalised exactly once: the shortest-arc flip and the + // near-parallel linear fallback both feed the same choke point. + [[nodiscard]] static Rotation3 slerp(const Rotation3& a, const Rotation3& b, float t) noexcept + { + float dot = Quaternion::dotProduct(a.q_, b.q_); + Quaternion target = b.q_; + if (dot < 0.0f) // take the shortest arc across the double cover + { + target = Quaternion{-target.x(), -target.y(), -target.z(), -target.w()}; + dot = -dot; + } + + // Near-parallel: the general form divides by sin(θ), which vanishes. Linear blending of two + // nearly equal rotations is accurate to well within the normalisation that follows. + constexpr float kLinearBlendThreshold = 0.9995f; + if (dot > kLinearBlendThreshold) + { + return Rotation3{Quaternion{ + a.q_.x() + (target.x() - a.q_.x()) * t, a.q_.y() + (target.y() - a.q_.y()) * t, + a.q_.z() + (target.z() - a.q_.z()) * t, a.q_.w() + (target.w() - a.q_.w()) * t}}; + } + + const float theta = std::acos(dot); + const float sinTheta = std::sin(theta); + const float wa = std::sin((1.0f - t) * theta) / sinTheta; + const float wb = std::sin(t * theta) / sinTheta; + return Rotation3{ + Quaternion{a.q_.x() * wa + target.x() * wb, a.q_.y() * wa + target.y() * wb, + a.q_.z() * wa + target.z() * wb, a.q_.w() * wa + target.w() * wb}}; + } + + // HEMISPHERE SELECTION, and deliberately not spelled `-q`. Negating a quaternion does not + // produce a different rotation, so an `operator-` on this type would read as an inverse and + // silently be a no-op — the kind of name that lies. Interpolation and error metrics need the + // representative on the same side of the double cover as a reference, and that is what this + // says out loud. Unary negation stays on `Quaternion`, where it means what it looks like. + [[nodiscard]] constexpr Rotation3 alignedTo(const Rotation3& reference) const noexcept + { + const float dot = Quaternion::dotProduct(q_, reference.q_); + return dot < 0.0f ? Rotation3{Quaternion{-q_.x(), -q_.y(), -q_.z(), -q_.w()}, AlreadyUnit{}} + : *this; + } + + // The angle of the shortest rotation between the two, in radians, in [0, π]. + // + // COMPUTED FROM THE RELATIVE ROTATION, in double, and not as `2·acos(|dot|)`. That form loses + // the answer exactly where it matters: for a small angle θ the dot product is cos(θ/2), which + // rounds to 1.0f below about θ = 5e-4, so every difference finer than that reports as zero — + // coarser than the default tolerance of this type's own approximate comparison. Taking + // `atan2(‖vec‖, |w|)` of the relative quaternion reads the angle off the vector part, which is + // ≈ θ/2 for small θ and suffers no cancellation at all. + [[nodiscard]] float angleTo(const Rotation3& other) const noexcept + { + const auto ax = static_cast(q_.x()); + const auto ay = static_cast(q_.y()); + const auto az = static_cast(q_.z()); + const auto aw = static_cast(q_.w()); + const auto bx = static_cast(other.q_.x()); + const auto by = static_cast(other.q_.y()); + const auto bz = static_cast(other.q_.z()); + const auto bw = static_cast(other.q_.w()); + + // conj(this) * other — the rotation taking this to other. + const double rx = aw * bx - ax * bw - ay * bz + az * by; + const double ry = aw * by + ax * bz - ay * bw - az * bx; + const double rz = aw * bz - ax * by + ay * bx - az * bw; + const double rw = aw * bw + ax * bx + ay * by + az * bz; + + const double vectorPart = std::sqrt(rx * rx + ry * ry + rz * rz); + return static_cast(2.0 * std::atan2(vectorPart, std::fabs(rw))); + } + + // SAME ROTATION, not same representation: `q` and `-q` are equal here because they rotate every + // vector identically. A rotation type whose equality said otherwise would be answering about + // its storage, and every caller comparing orientations would have to know about the double + // cover. + // + // EXACT up to that double cover — it is equality, not approximation. Two independently computed + // orientations will rarely satisfy it (a rotation composed with its own inverse is the identity + // only in exact arithmetic); `approxEqual` is for those, and it answers in radians. + [[nodiscard]] friend constexpr bool operator==(const Rotation3& lhs, + const Rotation3& rhs) noexcept + { + return lhs.q_ == rhs.q_ || + lhs.q_ == Quaternion{-rhs.q_.x(), -rhs.q_.y(), -rhs.q_.z(), -rhs.q_.w()}; + } + + // Exact component equality, for the rare caller that means the REPRESENTATION — a serialiser + // checking round-trip fidelity, a test pinning which hemisphere a factory chose. Named so that + // reaching for it is a decision rather than an accident. + [[nodiscard]] constexpr bool sameComponents(const Rotation3& other) const noexcept + { + return q_ == other.q_; + } + + // Approximate equality as an ANGLE, not as four component tolerances. Comparing components + // independently answers a question nobody asks: two rotations can differ in every component and + // be a thousandth of a degree apart, or agree closely in three and be far apart. + // + // `toleranceRadians` is validated the way `almostEqual`'s tolerances are: negative, NaN or + // infinite is a caller defect, and the answer to one is false rather than "everything matches". + [[nodiscard]] bool approxEqual(const Rotation3& other, + float toleranceRadians = 1.0e-4f) const noexcept + { + if (!std::isfinite(toleranceRadians) || toleranceRadians < 0.0f) + { + return false; + } + return angleTo(other) <= toleranceRadians; + } + + // Whether a quaternion is unit to within `kRotationUnitToleranceSquared`, on the SQUARED + // deviation. Exposed because tests and assertions both need to ask it in the same units. + [[nodiscard]] static bool isUnit(const Quaternion& q) noexcept + { + const float squared = q.magnitudeSquared(); + return std::isfinite(squared) && std::fabs(squared - 1.0f) <= kRotationUnitToleranceSquared; + } + +private: + // The failure path for a violated invariant — logged and terminal. A PRIVATE member, defined + // out of line: it is this type's own business, and a free function in the `fire_engine` + // namespace would be an externally callable "terminate the process" that nobody should have. + // Out of line so this header does not pull the logger into every translation unit that needs a + // rotation, for a path that never runs. + [[noreturn]] static void invariantViolated(const char* what) noexcept; + + [[nodiscard]] static bool isFinite(const Vec3& v) noexcept + { + return std::isfinite(v.x()) && std::isfinite(v.y()) && std::isfinite(v.z()); + } + + // Tag for the constructions that provably preserve unit length (conjugation, negation), so they + // can skip a `sqrt` without anybody being able to skip it by accident: the tag is private, so + // only members of this class can make that claim. + struct AlreadyUnit + { + }; + + // THE CHOKE POINT — private, so there is no way to build a `Rotation3` that is not one, and + // TERMINAL, so there is no way for one to finish construction holding something that is not a + // rotation. + // + // `Quaternion::normalise` answers NaNs for non-finite input (deliberately: visibly invalid + // beats a laundered zero) and the identity for a degenerate one. Storing either would defeat + // the entire type — the first with values that poison everything downstream, the second with a + // confident "no rotation" that came from a bug. Public fallible boundaries (`tryFrom*`) filter + // those cases into `nullopt` before they reach here, so arriving with one means an internal + // operation was handed corrupt input: a NaN angular velocity, an infinite timestep. There is no + // useful way to continue a simulation from that, and continuing quietly is how it surfaces + // three seconds later somewhere unrelated. + explicit Rotation3(const Quaternion& q) noexcept + : q_{normalisedOrTerminate(q)} + { + } + + constexpr Rotation3(const Quaternion& q, AlreadyUnit) noexcept + : q_{q} + { + } + + [[nodiscard]] static Quaternion normalisedOrTerminate(const Quaternion& q) noexcept + { + if (!std::isfinite(q.x()) || !std::isfinite(q.y()) || !std::isfinite(q.z()) || + !std::isfinite(q.w())) + { + invariantViolated("a rotation was computed from a non-finite quaternion"); + } + if (q.magnitude() < float_normalise_cutoff) + { + invariantViolated("a rotation was computed from a degenerate quaternion"); + } + const Quaternion normalised = Quaternion::normalise(q); + if (!isUnit(normalised)) + { + invariantViolated("normalisation failed to produce a unit rotation"); + } + return normalised; + } + + Quaternion q_{Quaternion::identity()}; +}; + +} // namespace fire_engine diff --git a/src/math/rotation3.cpp b/src/math/rotation3.cpp new file mode 100644 index 0000000..1c7e44f --- /dev/null +++ b/src/math/rotation3.cpp @@ -0,0 +1,25 @@ +#include "fire_engine/math/rotation3.hpp" + +#include + +#include + +namespace fire_engine +{ + +// Out of line so `rotation3.hpp` — included by scene, animation, physics and render — does not pull +// the logger into every one of their translation units for a path that never runs. +// +// TERMINAL, and deliberately so. Reaching here means an operation that promises to produce a +// rotation was handed something that cannot become one: a non-finite angular velocity, an infinite +// timestep, a corrupt orientation from upstream. The alternatives are worse in the ways this branch +// exists to prevent — storing NaNs gives every later operation a plausible-looking value that +// poisons whatever it touches, and substituting the identity turns a corrupt orientation into a +// confident "no rotation" that surfaces three seconds later somewhere unrelated. +void Rotation3::invariantViolated(const char* what) noexcept +{ + log::error(log::category::general, "Rotation3 invariant violated: {}", what); + std::abort(); +} + +} // namespace fire_engine diff --git a/tests/math/test_rotation3.cpp b/tests/math/test_rotation3.cpp new file mode 100644 index 0000000..d90635e --- /dev/null +++ b/tests/math/test_rotation3.cpp @@ -0,0 +1,396 @@ +#include + +#include +#include + +#include +#include + +using namespace fire_engine; + +namespace +{ + +constexpr float kNaN = std::numeric_limits::quiet_NaN(); +constexpr float kInf = std::numeric_limits::infinity(); + +[[nodiscard]] Rotation3 aboutZ(float angle) +{ + const auto r = Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, angle); + REQUIRE(r.has_value()); + return *r; +} + +} // namespace + +TEST_CASE("Rotation3.TheUnitToleranceIsSquaredDeviation", "[Rotation3]") +{ + // The convention is pinned here because the two spellings differ by a factor of two and nothing + // in a type name says which one a constant means. `kRotationUnitToleranceSquared` is measured + // on |‖q‖² − 1|, so a quaternion whose LENGTH is off by d registers as roughly 2d. + const float linearDeviation = 2.0e-5f; + const Quaternion slightlyLong{0.0f, 0.0f, 0.0f, 1.0f + linearDeviation}; + const float squaredDeviation = std::fabs(slightlyLong.magnitudeSquared() - 1.0f); + // The RELATION is the claim, not the arithmetic: subtracting 1 from a number just above 1 + // cancels most of a float's significant digits, so this is checked to a few percent rather than + // to the last bit. + CHECK(squaredDeviation == Catch::Approx(2.0f * linearDeviation).epsilon(0.05)); + CHECK(Rotation3::isUnit(slightlyLong)); + + // And a value outside it is rejected by the same measure, so `isUnit` and the constant cannot + // drift apart. + const Quaternion clearlyNotUnit{0.0f, 0.0f, 0.0f, 1.01f}; + CHECK(std::fabs(clearlyNotUnit.magnitudeSquared() - 1.0f) > kRotationUnitToleranceSquared); + CHECK_FALSE(Rotation3::isUnit(clearlyNotUnit)); + + // A non-finite quaternion is not unit, whatever the arithmetic would say. + CHECK_FALSE(Rotation3::isUnit(Quaternion{kNaN, 0.0f, 0.0f, 1.0f})); + CHECK_FALSE(Rotation3::isUnit(Quaternion{kInf, 0.0f, 0.0f, 1.0f})); +} + +TEST_CASE("Rotation3.FactoriesNormaliseImpreciseInputAndRefuseDegenerateInput", "[Rotation3]") +{ + // IMPRECISE IS NOT INVALID. A keyframe authored to six decimals, or an orientation that has + // drifted a few ulps, describes a rotation perfectly well — it is normalised, not rejected. + const auto drifted = Rotation3::tryFromQuaternion(Quaternion{0.0f, 0.0f, 0.0f, 1.0f + 1.0e-4f}); + REQUIRE(drifted.has_value()); + CHECK(Rotation3::isUnit(drifted->quaternion())); + + // THE WORST CASE glTF PERMITS, pinned exactly. Rotation animation outputs may be normalised + // signed BYTES, decoded as round(c·127)/127 — the coarsest encoding in the format. The worst + // deviation is {0.5, 0.5, 0.5, 0.5}, which encodes as 64/127 per component and decodes to a + // squared norm of 1.015810. An admission tolerance of 1e-2 would refuse this: a conforming + // asset rejected by the loader, which is why the bound is derived from the encoding rather than + // from the engine's own drift. + constexpr float kByteQuantised = 64.0f / 127.0f; + const Quaternion byteEncoded{kByteQuantised, kByteQuantised, kByteQuantised, kByteQuantised}; + CHECK(std::fabs(byteEncoded.magnitudeSquared() - 1.0f) == + Catch::Approx(0.015810).epsilon(1e-3)); + const auto authored = Rotation3::tryFromQuaternion(byteEncoded); + REQUIRE(authored.has_value()); + CHECK(Rotation3::isUnit(authored->quaternion())); + + // Shorts are finer and therefore also accepted. + const float shortQuantised = std::round(0.5f * 32767.0f) / 32767.0f; + const auto fromShorts = Rotation3::tryFromQuaternion( + Quaternion{shortQuantised, shortQuantised, shortQuantised, shortQuantised}); + REQUIRE(fromShorts.has_value()); + + // A SCALED quaternion is refused. {0, 0, 10, 10} normalises to a perfectly good rotation, which + // is exactly why accepting it would be wrong: this factory could then not tell a rotation from + // a derivative or an unnormalised intermediate, and the constant promising an admission + // tolerance would be decorative. + CHECK_FALSE(Rotation3::tryFromQuaternion(Quaternion{0.0f, 0.0f, 10.0f, 10.0f}).has_value()); + CHECK_FALSE(Rotation3::tryFromQuaternion(Quaternion{0.0f, 0.0f, 0.0f, 0.5f}).has_value()); + + // DEGENERATE AND NON-FINITE ARE REFUSED, because the alternative is laundering a producer bug + // into the identity — a value that rotates nothing and looks deliberate. + CHECK_FALSE(Rotation3::tryFromQuaternion(Quaternion{0.0f, 0.0f, 0.0f, 0.0f}).has_value()); + CHECK_FALSE(Rotation3::tryFromQuaternion(Quaternion{kNaN, 0.0f, 0.0f, 1.0f}).has_value()); + CHECK_FALSE(Rotation3::tryFromQuaternion(Quaternion{0.0f, kInf, 0.0f, 1.0f}).has_value()); +} + +TEST_CASE("Rotation3.AxisAngleNormalisesTheAxisAndRefusesDegenerateGeometry", "[Rotation3]") +{ + // A non-unit axis means the DIRECTION, so these must agree exactly in angle — the failure + // condition is geometry with no direction, never "the caller did not pre-normalise". + const auto unitAxis = Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, 0.75f); + const auto longAxis = Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 7.0f}, 0.75f); + REQUIRE(unitAxis.has_value()); + REQUIRE(longAxis.has_value()); + CHECK(unitAxis->approxEqual(*longAxis, 1.0e-6f)); + CHECK(Rotation3::isUnit(longAxis->quaternion())); + + // `fromAxisAngle({0,0,0}, angle)` was the review's example of a value that is not a rotation. + CHECK_FALSE(Rotation3::tryFromAxisAngle(Vec3{}, 1.0f).has_value()); + CHECK_FALSE(Rotation3::tryFromAxisAngle(Vec3{1.0e-30f, 0.0f, 0.0f}, 1.0f).has_value()); + CHECK_FALSE(Rotation3::tryFromAxisAngle(Vec3{kNaN, 0.0f, 1.0f}, 1.0f).has_value()); + CHECK_FALSE(Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, kNaN).has_value()); + CHECK_FALSE(Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, kInf).has_value()); +} + +TEST_CASE("Rotation3.FromVectorsNormalisesAndHandlesAntiparallel", "[Rotation3]") +{ + const auto r = Rotation3::tryFromVectors(Vec3{3.0f, 0.0f, 0.0f}, Vec3{0.0f, 5.0f, 0.0f}); + REQUIRE(r.has_value()); + CHECK(r->rotate(Vec3{1.0f, 0.0f, 0.0f}).approxEqual(Vec3{0.0f, 1.0f, 0.0f}, 1.0e-5f)); + + // ANTIPARALLEL IS NOT DEGENERATE: it is a 180° rotation about some perpendicular axis, and the + // result must take `from` to `to` like any other. + const auto flipped = Rotation3::tryFromVectors(Vec3{1.0f, 0.0f, 0.0f}, Vec3{-1.0f, 0.0f, 0.0f}); + REQUIRE(flipped.has_value()); + CHECK(Rotation3::isUnit(flipped->quaternion())); + CHECK(flipped->rotate(Vec3{1.0f, 0.0f, 0.0f}).approxEqual(Vec3{-1.0f, 0.0f, 0.0f}, 1.0e-5f)); + + CHECK_FALSE(Rotation3::tryFromVectors(Vec3{}, Vec3{0.0f, 1.0f, 0.0f}).has_value()); + CHECK_FALSE(Rotation3::tryFromVectors(Vec3{1.0f, 0.0f, 0.0f}, Vec3{}).has_value()); + CHECK_FALSE( + Rotation3::tryFromVectors(Vec3{kInf, 0.0f, 0.0f}, Vec3{0.0f, 1.0f, 0.0f}).has_value()); +} + +TEST_CASE("Rotation3.EveryOperationPreservesTheInvariant", "[Rotation3]") +{ + // The property the type exists for, and the one the reconnaissance did NOT prove: the engine's + // bounded drift today comes from `integrate` and `slerp` normalising their own results, so a + // rotation type must carry that responsibility rather than assume it. Composition especially — + // a product of two unit quaternions is unit only in exact arithmetic, and a skeleton compounds + // the error link by link. + Rotation3 chained = Rotation3::identity(); + const Rotation3 step = aboutZ(0.017f); + for (int i = 0; i < 4096; ++i) + { + chained = chained * step; + REQUIRE(Rotation3::isUnit(chained.quaternion())); + } + + Rotation3 integrated = Rotation3::identity(); + for (int i = 0; i < 4096; ++i) + { + integrated = integrated.integrate(Vec3{0.3f, -1.1f, 0.7f}, 1.0f / 120.0f); + REQUIRE(Rotation3::isUnit(integrated.quaternion())); + } + + const Rotation3 a = aboutZ(0.2f); + const Rotation3 b = aboutZ(2.7f); + for (const float t : {0.0f, 0.001f, 0.25f, 0.5f, 0.999f, 1.0f}) + { + CHECK(Rotation3::isUnit(Rotation3::slerp(a, b, t).quaternion())); + } + CHECK(Rotation3::isUnit(a.inverse().quaternion())); + CHECK(Rotation3::isUnit(a.alignedTo(b).quaternion())); + CHECK(Rotation3::isUnit(Rotation3::identity().quaternion())); +} + +TEST_CASE("Rotation3.EqualityIsAboutRotationsNotRepresentations", "[Rotation3]") +{ + // `q` and `-q` rotate every vector identically, so for a ROTATION type they are equal. A type + // whose equality answered about its storage would make every caller learn about the double + // cover, which is the knowledge this type exists to absorb. + const Rotation3 r = aboutZ(1.3f); + const Quaternion negated{-r.quaternion().x(), -r.quaternion().y(), -r.quaternion().z(), + -r.quaternion().w()}; + const auto mirrored = Rotation3::tryFromQuaternion(negated); + REQUIRE(mirrored.has_value()); + + CHECK(r == *mirrored); + CHECK(r.angleTo(*mirrored) == Catch::Approx(0.0f).margin(1.0e-6)); + CHECK(r.rotate(Vec3{1.0f, 2.0f, 3.0f}) + .approxEqual(mirrored->rotate(Vec3{1.0f, 2.0f, 3.0f}), 1.0e-5f)); + + // The representation still differs, and `sameComponents` is how a caller says it means that — + // a serialiser checking round-trip fidelity, or a test pinning which hemisphere was chosen. + CHECK_FALSE(r.sameComponents(*mirrored)); + CHECK(r.sameComponents(r)); + + // `alignedTo` is the explicit hemisphere choice. There is deliberately no unary `operator-`: + // negation does not produce a different rotation, so it would read as an inverse and silently + // be a no-op. + CHECK(mirrored->alignedTo(r).sameComponents(r)); + CHECK(r.alignedTo(r).sameComponents(r)); +} + +TEST_CASE("Rotation3.ApproximateEqualityIsAnAngle", "[Rotation3]") +{ + // Four independent component tolerances answer a question nobody asks: two rotations can differ + // in every component and be a thousandth of a degree apart, or agree in three and be far apart. + const Rotation3 a = aboutZ(1.0f); + const Rotation3 b = aboutZ(1.0f + 1.0e-5f); + CHECK(a.approxEqual(b, 1.0e-4f)); + CHECK_FALSE(a.approxEqual(aboutZ(1.1f), 1.0e-4f)); + + CHECK(a.angleTo(a) == Catch::Approx(0.0f).margin(1.0e-6)); + CHECK(aboutZ(0.0f).angleTo(aboutZ(pi * 0.5f)) == Catch::Approx(pi * 0.5f).epsilon(1e-4)); + // Symmetric, and bounded by π: the distance between rotations, never between representations. + CHECK(aboutZ(0.3f).angleTo(aboutZ(2.9f)) == + Catch::Approx(aboutZ(2.9f).angleTo(aboutZ(0.3f))).epsilon(1e-5)); + CHECK(aboutZ(0.0f).angleTo(aboutZ(pi * 1.999f)) <= pi + 1.0e-4f); +} + +TEST_CASE("Rotation3.IdentityAndCompositionBehave", "[Rotation3]") +{ + const Rotation3 r = aboutZ(0.9f); + + // `operator==` is EXACT up to the double cover — it answers "the same rotation", not "near + // enough". Composing with the identity is exact (multiplying by {0,0,0,1} perturbs nothing), so + // these hold bit-for-bit... + CHECK((r * Rotation3::identity()) == r); + CHECK((Rotation3::identity() * r) == r); + + // ...while a rotation composed with its own inverse is identity only in exact arithmetic, and + // computed rotations are what `approxEqual` is for. A caller comparing two independently + // computed orientations wants the angle, not the bits; `==` is for the cases where one value + // provably came from the other. + CHECK(r.inverse().inverse() == r); // conjugation twice is exact + CHECK((r * r.inverse()).approxEqual(Rotation3::identity(), 1.0e-6f)); + CHECK((r.inverse() * r).approxEqual(Rotation3::identity(), 1.0e-6f)); + + // Composition applies the right-hand rotation first, matching the quaternion convention it is + // built on: rotating a vector by (a * b) equals rotating it by b and then by a. + const Rotation3 a = aboutZ(0.4f); + const Rotation3 b = *Rotation3::tryFromAxisAngle(Vec3{1.0f, 0.0f, 0.0f}, 0.6f); + const Vec3 v{0.3f, -0.7f, 1.1f}; + CHECK((a * b).rotate(v).approxEqual(a.rotate(b.rotate(v)), 1.0e-5f)); +} + +TEST_CASE("Rotation3.AngleResolvesDifferencesFinerThanItsOwnTolerance", "[Rotation3]") +{ + // `2·acos(|dot|)` cannot answer this. For a small angle θ the dot product is cos(θ/2), which + // rounds to exactly 1.0f below about θ = 5e-4 — so every difference finer than that reports as + // ZERO, and this type's own default comparison tolerance (1e-4 rad) sits inside the blind spot. + // Reading the angle off the relative rotation's vector part has no such floor. + const Rotation3 base = aboutZ(0.7f); + const Rotation3 nudged = aboutZ(0.7f + 1.0e-5f); + + CHECK(base.angleTo(nudged) > 0.0f); + CHECK(base.angleTo(nudged) == Catch::Approx(1.0e-5f).epsilon(0.05)); + // And the comparison built on it can tell them apart at a tolerance below the difference. + CHECK_FALSE(base.approxEqual(nudged, 1.0e-6f)); + CHECK(base.approxEqual(nudged, 1.0e-4f)); + + // Finer still: a tenth of the old resolution floor, and two decades below it. + for (const float delta : {1.0e-4f, 1.0e-5f, 1.0e-6f}) + { + const Rotation3 other = aboutZ(0.7f + delta); + CHECK(base.angleTo(other) == Catch::Approx(delta).epsilon(0.1)); + } + + // The degenerate direction still answers exactly zero rather than a small noise floor. + CHECK(base.angleTo(base) == Catch::Approx(0.0f).margin(1.0e-9)); +} + +TEST_CASE("Rotation3.ApproxEqualRefusesAnInvalidTolerance", "[Rotation3]") +{ + // Consistent with `almostEqual`: a negative, NaN or infinite tolerance is a caller defect, and + // the answer to one is false — never "everything matches", which is what an infinite tolerance + // would otherwise mean. + const Rotation3 a = aboutZ(0.2f); + const Rotation3 b = aboutZ(2.0f); + CHECK_FALSE(a.approxEqual(a, -1.0f)); + CHECK_FALSE(a.approxEqual(a, kNaN)); + CHECK_FALSE(a.approxEqual(b, kInf)); + CHECK_FALSE(a.approxEqual(a, kInf)); + CHECK(a.approxEqual(a, 0.0f)); // zero is a legitimate, if strict, tolerance +} + +TEST_CASE("Rotation3.OperationsNormaliseExactlyOnce", "[Rotation3]") +{ + // Not a performance point — an accuracy one. Normalising twice rounds twice, and phase 1's norm + // work showed what an extra ulp per component costs a solver (a settling box stack went from + // step 169 to 425). It also muddies attribution: if the goldens move during this migration, the + // cause should be the conversion authority changing, not an operation quietly rounding twice. + // + // EACH CASE PROVES ITS OWN SENSITIVITY FIRST. Normalising an already-unit value is usually + // idempotent to the bit, so a fixture chosen at random passes whether the implementation rounds + // once or twice — the comparison would assert nothing at all. The `REQUIRE` in each section + // establishes that its raw value DOES change under a second normalisation, and only then is the + // operation's output compared against a single one. The constants were found by search; a + // search over 200,000 quaternions found about 37% to be sensitive, so they are not rare, but + // they do have to be chosen deliberately. + const auto sensitive = [](const Quaternion& raw) + { + const Quaternion once = Quaternion::normalise(raw); + return !(once == Quaternion::normalise(once)); + }; + + SECTION("composition") + { + const auto a = Rotation3::tryFromAxisAngle(Vec3{0.3f, -0.8f, 0.5f}, 0.0091f); + const auto b = Rotation3::tryFromAxisAngle(Vec3{1.0f, 0.2f, -0.4f}, 1.1f); + REQUIRE(a.has_value()); + REQUIRE(b.has_value()); + const Quaternion raw = a->quaternion() * b->quaternion(); + REQUIRE(sensitive(raw)); // this fixture can tell one normalisation from two + CHECK((*a * *b).quaternion() == Quaternion::normalise(raw)); + } + + SECTION("integration") + { + // The path that used to round twice most clearly: `Quaternion::integrate` normalises its + // own result, and passing that to the choke point normalised it again. + const auto r = Rotation3::tryFromAxisAngle(Vec3{0.2f, 0.9f, -0.3f}, 0.61f); + REQUIRE(r.has_value()); + const Vec3 omega{0.83f, -1.37f, 0.44f}; + const float dt = 0.0002329f; + const Vec3 rotationVector = omega * dt; + const float angle = rotationVector.magnitude(); + const float half = angle * 0.5f; + const float scale = std::sin(half) / angle; + const Quaternion delta{rotationVector.x() * scale, rotationVector.y() * scale, + rotationVector.z() * scale, std::cos(half)}; + const Quaternion raw = delta * r->quaternion(); + REQUIRE(sensitive(raw)); + CHECK(r->integrate(omega, dt).quaternion() == Quaternion::normalise(raw)); + } + + SECTION("slerp, general branch") + { + const auto a = Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, 0.2f); + const auto b = Rotation3::tryFromAxisAngle(Vec3{0.0f, 0.0f, 1.0f}, 2.7f); + REQUIRE(a.has_value()); + REQUIRE(b.has_value()); + const float t = 0.015f; + const Quaternion qa = a->quaternion(); + const Quaternion qb = b->quaternion(); + const float dot = Quaternion::dotProduct(qa, qb); + REQUIRE(dot <= 0.9995f); // the general branch, not the linear fallback + const float theta = std::acos(dot); + const float sinTheta = std::sin(theta); + const float wa = std::sin((1.0f - t) * theta) / sinTheta; + const float wb = std::sin(t * theta) / sinTheta; + const Quaternion raw{qa.x() * wa + qb.x() * wb, qa.y() * wa + qb.y() * wb, + qa.z() * wa + qb.z() * wb, qa.w() * wa + qb.w() * wb}; + REQUIRE(sensitive(raw)); + CHECK(Rotation3::slerp(*a, *b, t).quaternion() == Quaternion::normalise(raw)); + } + + SECTION("slerp, linear branch — not provable by observation") + { + // AN HONEST LIMIT, stated rather than papered over. The linear fallback blends two + // rotations that are already nearly equal, so its result is unit to the bit and a second + // normalisation changes nothing: a search over 60,000 fixtures found no input where once + // and twice differ. The single-normalisation property holds here by construction and is NOT + // asserted — a comparison that cannot fail is worse than no comparison, because it reads as + // coverage. + // + // What can be checked is that this branch is the one taken, and that it produces a unit + // rotation genuinely between its inputs. + const auto a = Rotation3::tryFromAxisAngle(Vec3{0.4f, -0.2f, 0.9f}, 0.33f); + const auto b = Rotation3::tryFromAxisAngle(Vec3{0.4f, -0.2f, 0.9f}, 0.3305f); + REQUIRE(a.has_value()); + REQUIRE(b.has_value()); + REQUIRE(Quaternion::dotProduct(a->quaternion(), b->quaternion()) > 0.9995f); + const Rotation3 blended = Rotation3::slerp(*a, *b, 0.5f); + CHECK(Rotation3::isUnit(blended.quaternion())); + CHECK(blended.angleTo(*a) == Catch::Approx(blended.angleTo(*b)).epsilon(0.05)); + } + + SECTION("factories") + { + const Quaternion drifted{0.0f, 0.0f, 0.0f, 1.0f + 1.0e-4f}; + const auto admitted = Rotation3::tryFromQuaternion(drifted); + REQUIRE(admitted.has_value()); + CHECK(admitted->quaternion() == Quaternion::normalise(drifted)); + + const Vec3 axis{0.3f, -0.8f, 0.5f}; + const float angle = 1.234f; + const auto viaFactory = Rotation3::tryFromAxisAngle(axis, angle); + REQUIRE(viaFactory.has_value()); + const Vec3 unitAxis = Vec3::normalise(axis); + const float half = angle * 0.5f; + const float s = std::sin(half); + const Quaternion raw{unitAxis.x() * s, unitAxis.y() * s, unitAxis.z() * s, std::cos(half)}; + CHECK(viaFactory->quaternion() == Quaternion::normalise(raw)); + + // From-vectors builds the half-angle form and hands it over unnormalised, so the result is + // a single normalisation of that — not a normalised `fromVectors` normalised again. + const Vec3 from{2.0f, 0.0f, 0.0f}; + const Vec3 to{0.0f, 3.0f, 0.0f}; + const auto fromVectors = Rotation3::tryFromVectors(from, to); + REQUIRE(fromVectors.has_value()); + const Vec3 f = Vec3::normalise(from); + const Vec3 t = Vec3::normalise(to); + const Vec3 cross = Vec3::crossProduct(f, t); + const Quaternion halfAngle{cross.x(), cross.y(), cross.z(), 1.0f + Vec3::dotProduct(f, t)}; + CHECK(fromVectors->quaternion() == Quaternion::normalise(halfAngle)); + } +}