From 087c8b32c30248df08f7a14c86cd028da37436ec Mon Sep 17 00:00:00 2001 From: Jorijn van der Graaf Date: Tue, 20 Oct 2026 19:06:47 +0200 Subject: [PATCH] feat: quaternion operations, and transforms that mean the same in 4x4 and 4x3 - A 4-vector doubles as a unit quaternion in (x, y, z, w) order: QuanternionMultiply, QuanternionConjugate, QuanternionFromAxisAngle, QuanternionFromUnitVectors (shortest arc), QuanternionNlerp and QuanternionSlerp, next to the existing QuanternionFromBasis. - MatrixRowMajor gains Rotation (from a quaternion), TRS, InverseAffine, Transpose and Decompose. The 4x4 transforms row vectors and the 4x3 column vectors (the VkTransformMatrixKHR layout); each of these means the same transform in both. The 4x3 Rotation copied the 4x4's rows, which rotated by the inverse. - The static Normalize no longer assumes three components. - Tests: Quaternion, and Matrix extended. Co-Authored-By: Claude Opus 5.5 --- README.md | 4 +- interfaces/Crafter.Math-MatrixRowMajor.cppm | 136 +++++++++++++++- interfaces/Crafter.Math-Vector.cppm | 98 ++++++++++- project.cpp | 2 +- tests/Matrix/main.cpp | 130 ++++++++++++++- tests/Quaternion/main.cpp | 171 ++++++++++++++++++++ 6 files changed, 533 insertions(+), 8 deletions(-) create mode 100644 tests/Quaternion/main.cpp diff --git a/README.md b/README.md index 9135324..f6b6af4 100644 --- a/README.md +++ b/README.md @@ -11,8 +11,8 @@ The library is hardware-aware: it picks the widest SIMD path the target supports | Partition | Contents | | --- | --- | | `:Basic` | `ToRadian`, plus the `VectorF16 → VectorF32` alias on x86_64 without AVX512-FP16 and on AArch64 without ARMv8.2-FP16. | -| `:Vector` | Generic `Vector` with `.x/.y/.z/.w` and `.r/.g/.b/.a` accessors, arithmetic, comparison, and conversion operators. | -| `:MatrixRowMajor` | `MatrixRowMajor` — row-major matrices with packed-batch support via `Repeats`. | +| `:Vector` | Generic `Vector` with `.x/.y/.z/.w` and `.r/.g/.b/.a` accessors, arithmetic, comparison, and conversion operators. A 4-vector doubles as a unit quaternion in (x, y, z, w) order: `Rotate` applies one, and `QuanternionMultiply`, `QuanternionConjugate`, `QuanternionFromAxisAngle`, `QuanternionFromUnitVectors` (shortest arc), `QuanternionFromBasis`, `QuanternionFromEuler`, `QuanternionNlerp` and `QuanternionSlerp` build and combine them. | +| `:MatrixRowMajor` | `MatrixRowMajor` — row-major matrices with packed-batch support via `Repeats`. The 4x4 transforms row vectors (`v * M`, translation in row 3) and the 4x3 column vectors (`M * v`, translation in column 3, the `VkTransformMatrixKHR` layout); `Rotation`, `TRS` (from a translation, quaternion and scale), `InverseAffine` and `Decompose` mean the same transform in both. | | `:VectorF32` | `VectorF32` — SIMD-specialized 32-bit float vector with `Load`/`Store`, FMA, `Cross`, `Dot`, `Length`, `Normalize`, `Sin`/`Cos`, `Shuffle`, etc. | | `:VectorF16` | `VectorF16` — same surface as `VectorF32` for `_Float16` when the target supports it. | | `:Intersection` | `IntersectionTestRayTriangle`, `IntersectionTestRaySphere`, `IntersectionTestRayOrientedBox`. | diff --git a/interfaces/Crafter.Math-MatrixRowMajor.cppm b/interfaces/Crafter.Math-MatrixRowMajor.cppm index 67a5e58..22fc149 100755 --- a/interfaces/Crafter.Math-MatrixRowMajor.cppm +++ b/interfaces/Crafter.Math-MatrixRowMajor.cppm @@ -4,6 +4,7 @@ export module Crafter.Math:MatrixRowMajor; import :Basic; +import :Vector; import :VectorF32; import std; @@ -234,10 +235,13 @@ namespace Crafter { const float sy = s[1], cy = c[1]; const float sr = s[2], cr = c[2]; + // The 4x4's rows transposed: the 4x4 multiplies row vectors and the + // 4x3 column vectors, so the same rows here would rotate the other + // way. return MatrixRowMajor( - cr * cy + sr * sp * sy, sr * cp, sr * sp * cy - cr * sy, 0.0f, - cr * sp * sy - sr * cy, cr * cp, sr * sy + cr * sp * cy, 0.0f, - cp * sy, -sp, cp * cy, 0.0f + cr * cy + sr * sp * sy, cr * sp * sy - sr * cy, cp * sy, 0.0f, + sr * cp, cr * cp, -sp, 0.0f, + sr * sp * cy - cr * sy, sr * sy + cr * sp * cy, cp * cy, 0.0f ); } @@ -281,6 +285,132 @@ namespace Crafter { static MatrixRowMajor LookAt(VectorF32<3, 1> eyePosition, VectorF32<3, 1> focusPosition, VectorF32<3, 1> upDirection) requires(CollumSize == 4 && RowSize == 4 && Repeats == 1 && std::same_as) { return LookTo(eyePosition, eyePosition - focusPosition, upDirection); } + + // ─── Quaternions, TRS and the affine inverse ─────────────────── + // Each call means the same transform in both shapes: the 4x4 takes + // row vectors (v * M, translation in row 3) and the 4x3 column + // vectors (M * v, translation in column 3, the VkTransformMatrixKHR + // layout), so the 4x4's upper 3x3 is the 4x3's transposed. These are + // scalar code: they build matrices, they don't sit in inner loops. + + // The rotation by unit quaternion q (x, y, z, w) that Vector::Rotate + // applies. + template + static MatrixRowMajor Rotation(Vector q) requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as) { + return TRS(Vector(0, 0, 0), q, Vector(1, 1, 1)); + } + + // Scale by s, then rotate by unit quaternion q, then translate by t: + // a glTF node's transform. + template + static MatrixRowMajor TRS(Vector t, Vector q, Vector s) requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as) { + const float xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z; + const float xy = q.x * q.y, xz = q.x * q.z, yz = q.y * q.z; + const float wx = q.w * q.x, wy = q.w * q.y, wz = q.w * q.z; + // r[i][j]: q's rotation matrix in column-vector form. + const float r[3][3] = { + { 1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy) }, + { 2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx) }, + { 2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy) }, + }; + const float sc[3] = { s.x, s.y, s.z }; + if constexpr (RowSize == 3) { + // Column j is basis vector j, scaled and rotated. + return MatrixRowMajor( + r[0][0] * sc[0], r[0][1] * sc[1], r[0][2] * sc[2], t.x, + r[1][0] * sc[0], r[1][1] * sc[1], r[1][2] * sc[2], t.y, + r[2][0] * sc[0], r[2][1] * sc[1], r[2][2] * sc[2], t.z + ); + } else { + // Row i is basis vector i, scaled and rotated. + return MatrixRowMajor( + r[0][0] * sc[0], r[1][0] * sc[0], r[2][0] * sc[0], 0, + r[0][1] * sc[1], r[1][1] * sc[1], r[2][1] * sc[1], 0, + r[0][2] * sc[2], r[1][2] * sc[2], r[2][2] * sc[2], 0, + t.x, t.y, t.z, 1 + ); + } + } + + // The inverse of an affine transform: any rotation, scale or shear + // plus a translation, which is everything TRS builds. Not valid for a + // projection. + MatrixRowMajor InverseAffine() const requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as) { + const std::array a = Store(); + // l[i][j] and t: the linear part and the translation, column-vector form. + float l[3][3]; + float t[3]; + for (std::uint32_t i = 0; i < 3; ++i) { + for (std::uint32_t j = 0; j < 3; ++j) { + l[i][j] = RowSize == 3 ? a[i * 4 + j] : a[j * 4 + i]; + } + t[i] = RowSize == 3 ? a[i * 4 + 3] : a[12 + i]; + } + // n = l^-1 through the adjugate. + const float c00 = l[1][1] * l[2][2] - l[1][2] * l[2][1]; + const float c01 = l[1][2] * l[2][0] - l[1][0] * l[2][2]; + const float c02 = l[1][0] * l[2][1] - l[1][1] * l[2][0]; + const float invDet = 1.0f / (l[0][0] * c00 + l[0][1] * c01 + l[0][2] * c02); + const float n[3][3] = { + { c00 * invDet, (l[0][2] * l[2][1] - l[0][1] * l[2][2]) * invDet, (l[0][1] * l[1][2] - l[0][2] * l[1][1]) * invDet }, + { c01 * invDet, (l[0][0] * l[2][2] - l[0][2] * l[2][0]) * invDet, (l[0][2] * l[1][0] - l[0][0] * l[1][2]) * invDet }, + { c02 * invDet, (l[0][1] * l[2][0] - l[0][0] * l[2][1]) * invDet, (l[0][0] * l[1][1] - l[0][1] * l[1][0]) * invDet }, + }; + // The inverse translates by -n * t. + float nt[3]; + for (std::uint32_t i = 0; i < 3; ++i) { + nt[i] = -(n[i][0] * t[0] + n[i][1] * t[1] + n[i][2] * t[2]); + } + if constexpr (RowSize == 3) { + return MatrixRowMajor( + n[0][0], n[0][1], n[0][2], nt[0], + n[1][0], n[1][1], n[1][2], nt[1], + n[2][0], n[2][1], n[2][2], nt[2] + ); + } else { + return MatrixRowMajor( + n[0][0], n[1][0], n[2][0], 0, + n[0][1], n[1][1], n[2][1], 0, + n[0][2], n[1][2], n[2][2], 0, + nt[0], nt[1], nt[2], 1 + ); + } + } + + MatrixRowMajor Transpose() const requires(CollumSize == 4 && RowSize == 4 && Repeats == 1 && std::same_as) { + const std::array a = Store(); + return MatrixRowMajor( + a[0], a[4], a[8], a[12], + a[1], a[5], a[9], a[13], + a[2], a[6], a[10], a[14], + a[3], a[7], a[11], a[15] + ); + } + + // Splits a non-degenerate affine transform into the translation, + // rotation (unit quaternion) and scale that TRS builds it from. Shear + // is not represented. A mirroring transform (negative determinant) has + // no unique split; it comes back as a negative X scale, which TRS + // turns into the same transform. + std::tuple, Vector, Vector> Decompose() const requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as) { + const std::array a = Store(); + // basis[j]: where the transform takes unit vector j. + Vector basis[3]; + for (std::uint32_t j = 0; j < 3; ++j) { + basis[j] = RowSize == 3 + ? Vector(a[j], a[4 + j], a[8 + j]) + : Vector(a[j * 4], a[j * 4 + 1], a[j * 4 + 2]); + } + const Vector t = RowSize == 3 + ? Vector(a[3], a[7], a[11]) + : Vector(a[12], a[13], a[14]); + Vector s(basis[0].Length(), basis[1].Length(), basis[2].Length()); + if (Vector::Dot(Vector::Cross(basis[0], basis[1]), basis[2]) < 0) { + s.x = -s.x; + } + const Vector q = Vector::QuanternionFromBasis(basis[0] / s.x, basis[1] / s.y, basis[2] / s.z); + return { t, q, s }; + } }; } diff --git a/interfaces/Crafter.Math-Vector.cppm b/interfaces/Crafter.Math-Vector.cppm index 6f23a88..2488076 100755 --- a/interfaces/Crafter.Math-Vector.cppm +++ b/interfaces/Crafter.Math-Vector.cppm @@ -334,7 +334,7 @@ namespace Crafter { template constexpr static Vector Normalize(Vector a) requires(Len == Alen) { - Vector returned; + Vector returned; T fLength = a.Length(); if (fLength > 0) { @@ -438,6 +438,102 @@ namespace Crafter { cr * cp * cy + sr * sp * sy ); } + + // The quaternions here are Vector in (x, y, z, w) order, w the + // scalar part, applied as Rotate does (q v q*). They take and return + // unit quaternions. + + // The Hamilton product: rotation b followed by rotation a, so + // Rotate(v, QuanternionMultiply(a, b)) == Rotate(Rotate(v, b), a). + template + constexpr static Vector QuanternionMultiply(Vector a, Vector b) requires(Len == 4) { + return Vector( + a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z + ); + } + + // The inverse rotation. + template + constexpr static Vector QuanternionConjugate(Vector q) requires(Len == 4) { + return Vector(-q.x, -q.y, -q.z, q.w); + } + + // `angle` radians about the unit vector `axis`, right-handed: a + // positive angle about +Z turns +X towards +Y. + template + constexpr static Vector QuanternionFromAxisAngle(Vector axis, T angle) requires(Len == 4) { + const T s = std::sin(angle * T(0.5)); + return Vector(axis.x * s, axis.y * s, axis.z * s, std::cos(angle * T(0.5))); + } + + // The shortest rotation taking unit direction `from` onto unit + // direction `to`. For opposite directions every axis perpendicular + // to `from` is a shortest arc, and the cross product that picks the + // axis otherwise is zero; the axis is then taken against whichever of + // X and Y is further from parallel to `from`. + template + constexpr static Vector QuanternionFromUnitVectors(Vector from, Vector to) requires(Len == 4) { + const Vector f(from.x, from.y, from.z); + const T d = f.x * to.x + f.y * to.y + f.z * to.z; + if (d < T(-1) + T(1e-6)) { + Vector axis = std::abs(f.x) < T(0.9) + ? Vector::Cross(Vector(1, 0, 0), f) + : Vector::Cross(Vector(0, 1, 0), f); + axis.Normalize(); + return Vector(axis.x, axis.y, axis.z, T(0)); + } + const Vector c = Vector::Cross(f, Vector(to.x, to.y, to.z)); + Vector q(c.x, c.y, c.z, T(1) + d); + q.Normalize(); + return q; + } + + // Normalized linear interpolation from a to b, along the shorter arc + // (q and -q are the same rotation, and blending towards the wrong one + // swings the long way round). Cheaper than Slerp; its angular speed + // varies a little along the arc, which damping and blending between + // nearby poses don't notice. + template + constexpr static Vector QuanternionNlerp(Vector a, Vector b, T t) requires(Len == 4) { + const T sign = (a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w) < T(0) ? T(-1) : T(1); + Vector q( + a.x + (b.x * sign - a.x) * t, + a.y + (b.y * sign - a.y) * t, + a.z + (b.z * sign - a.z) * t, + a.w + (b.w * sign - a.w) * t + ); + q.Normalize(); + return q; + } + + // Spherical linear interpolation from a to b: constant angular speed, + // along the shorter arc. Nearly identical inputs go through Nlerp, + // which is exact enough there, instead of dividing by sin(~0). + template + constexpr static Vector QuanternionSlerp(Vector a, Vector b, T t) requires(Len == 4) { + T cosAngle = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + T sign = T(1); + if (cosAngle < T(0)) { + cosAngle = -cosAngle; + sign = T(-1); + } + if (cosAngle > T(0.9995)) { + return QuanternionNlerp(a, b, t); + } + const T angle = std::acos(cosAngle); + const T sinAngle = std::sin(angle); + const T wa = std::sin((T(1) - t) * angle) / sinAngle; + const T wb = std::sin(t * angle) / sinAngle * sign; + return Vector( + a.x * wa + b.x * wb, + a.y * wa + b.y * wb, + a.z * wa + b.z * wb, + a.w * wa + b.w * wb + ); + } }; } diff --git a/project.cpp b/project.cpp index be528b6..14d9814 100644 --- a/project.cpp +++ b/project.cpp @@ -58,7 +58,7 @@ extern "C" Configuration CrafterBuildProject(std::span a const bool isX86 = target.starts_with("x86_64") || target.starts_with("i686"); const bool isRiscv = target.starts_with("riscv"); const bool isArm = target.starts_with("aarch64"); - for (std::string_view name : { "Vector", "Intersection", "Matrix" }) { + for (std::string_view name : { "Vector", "Intersection", "Matrix", "Quaternion" }) { if (isX86) cfg.AddMarchVariants(name, mathInterfaces, x86Tiers); if (isRiscv) cfg.AddMarchVariants(name, mathInterfaces, riscvTiers); if (isArm) cfg.AddMarchVariants(name, mathInterfaces, armTiers); diff --git a/tests/Matrix/main.cpp b/tests/Matrix/main.cpp index cdca005..5da13fc 100644 --- a/tests/Matrix/main.cpp +++ b/tests/Matrix/main.cpp @@ -286,13 +286,135 @@ std::string* TestLookToBasis() { }, "LookTo basis"); } + +// ---------- Rotation conventions, quaternions, TRS, inverse ---------- +// +// The 4x4 is row-vector (v * M, translation in row 3) and the 4x3 is +// column-vector (M * v, translation in column 3, the VkTransformMatrixKHR +// layout). The same call has to mean the same rotation in both. + +using Mat43 = MatrixRowMajor; +using Mat44 = MatrixRowMajor; +using Q = Vector; +using V3 = Vector; + +// v as a row vector times a 4x4, translation included. +V3 RowTimes44(Mat44 const& m, V3 v) { + const std::array a = m.Store(); + return V3(v.x * a[0] + v.y * a[4] + v.z * a[8] + a[12], + v.x * a[1] + v.y * a[5] + v.z * a[9] + a[13], + v.x * a[2] + v.y * a[6] + v.z * a[10] + a[14]); +} + +// A 4x3 times v as a column vector, translation included. +V3 Times43(Mat43 const& m, V3 v) { + const std::array a = m.Store(); + return V3(a[0] * v.x + a[1] * v.y + a[2] * v.z + a[3], + a[4] * v.x + a[5] * v.y + a[6] * v.z + a[7], + a[8] * v.x + a[9] * v.y + a[10] * v.z + a[11]); +} + +std::string* CheckV3(V3 got, V3 expected, std::string_view label) { + if (!FloatEquals(got.x, expected.x) || !FloatEquals(got.y, expected.y) || !FloatEquals(got.z, expected.z)) + return new std::string(std::format("{}: expected ({},{},{}), got ({},{},{})", + label, expected.x, expected.y, expected.z, got.x, got.y, got.z)); + return nullptr; +} + +Q SampleRotation() { + return Q::QuanternionFromAxisAngle(V3::Normalize(V3(1, 2, 3)), 0.9f); +} + +std::string* TestRotation43MatchesRotation44() { + // The 4x3 Euler Rotation used to copy the 4x4's row-vector rows, so as a + // column-vector transform it rotated the other way. + const V3 v(1, 2, 3); + const V3 expected = RowTimes44(Mat44::Rotation(0.3f, 0.7f, -0.4f), v); + return CheckV3(Times43(Mat43::Rotation(0.3f, 0.7f, -0.4f), v), expected, "4x3 Rotation vs 4x4 Rotation"); +} + +std::string* TestRotationFromQuaternion() { + const Q q = SampleRotation(); + const V3 v(1, -2, 0.5f); + const V3 expected = V3::Rotate(v, q); + if (auto err = CheckV3(Times43(Mat43::Rotation(q), v), expected, "4x3 Rotation(q)")) return err; + return CheckV3(RowTimes44(Mat44::Rotation(q), v), expected, "4x4 Rotation(q)"); +} + +std::string* TestTRS() { + // Scale, then rotate, then translate. + const V3 t(1, -2, 3), s(2, 0.5f, 1.5f), v(0.3f, 1, -2); + const Q q = SampleRotation(); + const V3 rotated = V3::Rotate(V3(v.x * s.x, v.y * s.y, v.z * s.z), q); + const V3 expected(rotated.x + t.x, rotated.y + t.y, rotated.z + t.z); + if (auto err = CheckV3(Times43(Mat43::TRS(t, q, s), v), expected, "4x3 TRS")) return err; + return CheckV3(RowTimes44(Mat44::TRS(t, q, s), v), expected, "4x4 TRS"); +} + +std::string* TestInverseAffine() { + const V3 t(1, -2, 3), s(2, 0.5f, 1.5f), v(0.3f, 1, -2); + const Q q = SampleRotation(); + const Mat43 m43 = Mat43::TRS(t, q, s); + const Mat44 m44 = Mat44::TRS(t, q, s); + if (auto err = CheckV3(Times43(m43.InverseAffine(), Times43(m43, v)), v, "4x3 inverse undoes the transform")) return err; + if (auto err = CheckV3(RowTimes44(m44.InverseAffine(), RowTimes44(m44, v)), v, "4x4 inverse undoes the transform")) return err; + if (auto err = CheckMat43(m43 * m43.InverseAffine(), { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 }, "4x3 M * M^-1")) return err; + return CheckMat44(m44 * m44.InverseAffine(), { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }, "4x4 M * M^-1"); +} + +std::string* TestTranspose44() { + const Mat44 m( + 1, 2, 3, 4, + 5, 6, 7, 8, + 9, 10, 11, 12, + 13, 14, 15, 16); + return CheckMat44(m.Transpose(), { + 1, 5, 9, 13, + 2, 6, 10, 14, + 3, 7, 11, 15, + 4, 8, 12, 16, + }, "Transpose 4x4"); +} + +std::string* CheckSameRotation(Q got, Q expected, std::string_view label) { + const float sign = (got.x * expected.x + got.y * expected.y + got.z * expected.z + got.w * expected.w) < 0 ? -1.0f : 1.0f; + if (!FloatEquals(got.x * sign, expected.x) || !FloatEquals(got.y * sign, expected.y) + || !FloatEquals(got.z * sign, expected.z) || !FloatEquals(got.w * sign, expected.w)) + return new std::string(std::format("{}: expected ({},{},{},{}), got ({},{},{},{})", + label, expected.x, expected.y, expected.z, expected.w, got.x, got.y, got.z, got.w)); + return nullptr; +} + +std::string* TestDecompose() { + const V3 t(1, -2, 3), s(2, 0.5f, 1.5f); + const Q q = SampleRotation(); + { + const auto [dt, dq, ds] = Mat44::TRS(t, q, s).Decompose(); + if (auto err = CheckV3(dt, t, "4x4 Decompose translation")) return err; + if (auto err = CheckV3(ds, s, "4x4 Decompose scale")) return err; + if (auto err = CheckSameRotation(dq, q, "4x4 Decompose rotation")) return err; + } + { + const auto [dt, dq, ds] = Mat43::TRS(t, q, s).Decompose(); + if (auto err = CheckV3(dt, t, "4x3 Decompose translation")) return err; + if (auto err = CheckV3(ds, s, "4x3 Decompose scale")) return err; + if (auto err = CheckSameRotation(dq, q, "4x3 Decompose rotation")) return err; + } + // A mirrored matrix has no unique split; recomposing has to give the same + // transform back. + const Mat44 mirrored = Mat44::TRS(t, q, V3(-2, 0.5f, 1.5f)); + const auto [mt, mq, ms] = mirrored.Decompose(); + const V3 v(0.3f, 1, -2); + return CheckV3(RowTimes44(Mat44::TRS(mt, mq, ms), v), RowTimes44(mirrored, v), "mirrored Decompose recomposes"); +} + // ---------- Test harness ---------- } // namespace int main() { using Fn = std::string* (*)(); - constexpr std::array, 15> tests = {{ + constexpr std::array, 21> tests = {{ { "Identity43", TestIdentity43 }, { "Identity44", TestIdentity44 }, { "StoreRoundtrip", TestStoreRoundtrip }, @@ -308,6 +430,12 @@ int main() { { "MatMul44", TestMatMul44 }, { "MatMulTranslation", TestMatMulTranslationCompose }, { "MatMulScaleTranslate",TestMatMulScaleThenTranslate }, + { "Rotation43Matches44", TestRotation43MatchesRotation44 }, + { "RotationQuaternion", TestRotationFromQuaternion }, + { "TRS", TestTRS }, + { "InverseAffine", TestInverseAffine }, + { "Transpose44", TestTranspose44 }, + { "Decompose", TestDecompose }, }}; for (auto const& [name, fn] : tests) { diff --git a/tests/Quaternion/main.cpp b/tests/Quaternion/main.cpp new file mode 100644 index 0000000..5164711 --- /dev/null +++ b/tests/Quaternion/main.cpp @@ -0,0 +1,171 @@ +//SPDX-License-Identifier: LGPL-3.0-only +//SPDX-FileCopyrightText: Copyright (C) 2026 Catcrafts® + +// Quaternions on the scalar Vector (x, y, z, w), as Vector::Rotate +// applies them: the static Normalize on a 4-vector, the Hamilton product and +// its composition order, conjugate, axis-angle, shortest-arc between two +// directions (including the opposite-directions case), nlerp and slerp. + +#include +import Crafter.Math; +import std; +using namespace Crafter; + +namespace { + +using Q = Vector; +using V3 = Vector; + +constexpr float kEps = 1e-4f; +constexpr float kPi = std::numbers::pi_v; + +bool Near(float a, float b, float epsilon = kEps) { + return std::abs(a - b) < epsilon; +} + +std::string* CheckV3(V3 got, V3 expected, std::string_view label) { + if (!Near(got.x, expected.x) || !Near(got.y, expected.y) || !Near(got.z, expected.z)) { + return new std::string(std::format("{}: expected ({},{},{}), got ({},{},{})", + label, expected.x, expected.y, expected.z, got.x, got.y, got.z)); + } + return nullptr; +} + +// q and -q are the same rotation. +std::string* CheckSameRotation(Q got, Q expected, std::string_view label) { + const float sign = (got.x * expected.x + got.y * expected.y + got.z * expected.z + got.w * expected.w) < 0 ? -1.0f : 1.0f; + if (!Near(got.x * sign, expected.x) || !Near(got.y * sign, expected.y) + || !Near(got.z * sign, expected.z) || !Near(got.w * sign, expected.w)) { + return new std::string(std::format("{}: expected ({},{},{},{}), got ({},{},{},{})", + label, expected.x, expected.y, expected.z, expected.w, got.x, got.y, got.z, got.w)); + } + return nullptr; +} + +std::string* CheckUnit(Q q, std::string_view label) { + const float length = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + if (!Near(length, 1.0f)) { + return new std::string(std::format("{}: length {}, expected 1", label, length)); + } + return nullptr; +} + +Q AboutZ(float angle) { + return Q::QuanternionFromAxisAngle(V3(0, 0, 1), angle); +} + +// ---------- Normalize ---------- + +std::string* TestNormalizeLen4() { + // The static Normalize built a 3-vector whatever the length, so a + // quaternion could not be normalized through it. + const Q n = Q::Normalize(Q(1, 2, 2, 4)); + return CheckSameRotation(n, Q(0.2f, 0.4f, 0.4f, 0.8f), "Normalize of a 4-vector"); +} + +// ---------- Multiply / Conjugate ---------- + +std::string* TestMultiplyIdentity() { + const Q q = Q::QuanternionFromAxisAngle(V3::Normalize(V3(1, 2, 3)), 0.7f); + return CheckSameRotation(Q::QuanternionMultiply(q, Q(0, 0, 0, 1)), q, "q * identity"); +} + +std::string* TestConjugateUndoes() { + const Q q = Q::QuanternionFromAxisAngle(V3::Normalize(V3(-2, 1, 0.5f)), 1.3f); + return CheckSameRotation(Q::QuanternionMultiply(q, Q::QuanternionConjugate(q)), Q(0, 0, 0, 1), "q * conj(q)"); +} + +std::string* TestMultiplyOrder() { + // a * b applies b first, then a, as Rotate composes. + const Q a = AboutZ(kPi / 2); + const Q b = Q::QuanternionFromAxisAngle(V3(1, 0, 0), kPi / 2); + const V3 v(0, 1, 0); + const V3 composed = V3::Rotate(v, Q::QuanternionMultiply(a, b)); + const V3 stepwise = V3::Rotate(V3::Rotate(v, b), a); + return CheckV3(composed, stepwise, "Rotate(v, a * b) == Rotate(Rotate(v, b), a)"); +} + +// ---------- Axis-angle ---------- + +std::string* TestAxisAngle() { + // Right-handed: +90° about Z takes X to Y. + return CheckV3(V3::Rotate(V3(1, 0, 0), AboutZ(kPi / 2)), V3(0, 1, 0), "+90° about Z on X"); +} + +// ---------- Shortest arc ---------- + +std::string* TestFromUnitVectors() { + const std::array, 5> pairs = {{ + { V3(1, 0, 0), V3(0, 1, 0) }, + { V3(0, 0, 1), V3::Normalize(V3(1, 1, 1)) }, + { V3::Normalize(V3(1, -2, 3)), V3::Normalize(V3(-3, 0.5f, 2)) }, + { V3(0, 1, 0), V3(0, 1, 0) }, + { V3(1, 0, 0), V3(-1, 0, 0) }, // opposite: any perpendicular axis will do + }}; + for (const auto& [from, to] : pairs) { + const Q q = Q::QuanternionFromUnitVectors(from, to); + if (auto err = CheckUnit(q, "FromUnitVectors result")) return err; + if (auto err = CheckV3(V3::Rotate(from, q), to, "Rotate(from, FromUnitVectors(from, to))")) return err; + } + // The opposite case for every axis, since the fallback axis depends on it. + for (const V3 axis : { V3(0, 1, 0), V3(0, 0, 1), V3::Normalize(V3(1, 1, 0)) }) { + const V3 opposite(-axis.x, -axis.y, -axis.z); + if (auto err = CheckV3(V3::Rotate(axis, Q::QuanternionFromUnitVectors(axis, opposite)), opposite, + "FromUnitVectors to the opposite direction")) return err; + } + return nullptr; +} + +// ---------- Interpolation ---------- + +std::string* TestNlerp() { + const Q a(0, 0, 0, 1); + const Q b = AboutZ(kPi / 2); + if (auto err = CheckSameRotation(Q::QuanternionNlerp(a, b, 0.0f), a, "Nlerp t=0")) return err; + if (auto err = CheckSameRotation(Q::QuanternionNlerp(a, b, 1.0f), b, "Nlerp t=1")) return err; + if (auto err = CheckUnit(Q::QuanternionNlerp(a, b, 0.3f), "Nlerp t=0.3")) return err; + // Halfway is exact for nlerp too, by symmetry. + return CheckSameRotation(Q::QuanternionNlerp(a, b, 0.5f), AboutZ(kPi / 4), "Nlerp t=0.5"); +} + +std::string* TestSlerp() { + const Q a(0, 0, 0, 1); + const Q b = AboutZ(kPi / 2); + if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 0.0f), a, "Slerp t=0")) return err; + if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 1.0f), b, "Slerp t=1")) return err; + // Constant angular speed: a quarter of the way is a quarter of the angle. + if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, b, 0.25f), AboutZ(kPi / 8), "Slerp t=0.25")) return err; + // -b is the same rotation; slerp takes the short way either way. + const Q negB(-b.x, -b.y, -b.z, -b.w); + if (auto err = CheckSameRotation(Q::QuanternionSlerp(a, negB, 0.5f), AboutZ(kPi / 4), "Slerp toward -b")) return err; + // Nearly identical inputs fall back to nlerp instead of dividing by sin(~0). + const Q almost = AboutZ(1e-5f); + const Q mid = Q::QuanternionSlerp(a, almost, 0.5f); + if (std::isnan(mid.x) || std::isnan(mid.w)) return new std::string("Slerp of nearly identical quaternions is NaN"); + return CheckUnit(mid, "Slerp of nearly identical quaternions"); +} + +} // namespace + +int main() { + using Fn = std::string* (*)(); + constexpr std::array, 9> tests = {{ + { "NormalizeLen4", TestNormalizeLen4 }, + { "MultiplyIdentity", TestMultiplyIdentity }, + { "ConjugateUndoes", TestConjugateUndoes }, + { "MultiplyOrder", TestMultiplyOrder }, + { "AxisAngle", TestAxisAngle }, + { "FromUnitVectors", TestFromUnitVectors }, + { "Nlerp", TestNlerp }, + { "Slerp", TestSlerp }, + { "SlerpIdentity", [] { return CheckSameRotation(Q::QuanternionSlerp(Q(0, 0, 0, 1), Q(0, 0, 0, 1), 0.5f), Q(0, 0, 0, 1), "Slerp of identities"); } }, + }}; + + for (auto const& [name, fn] : tests) { + if (auto err = std::unique_ptr(fn())) { + std::println(std::cerr, "[{}] {}", name, *err); + return 1; + } + } + return 0; +}