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 <noreply@anthropic.com>
This commit is contained in:
parent
0c55e76873
commit
087c8b32c3
GPG key ID:
2937E59CDCC1BCFB
6 changed files with 533 additions and 8 deletions
|
|
@ -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<float, 4, 3, 1>;
|
||||
using Mat44 = MatrixRowMajor<float, 4, 4, 1>;
|
||||
using Q = Vector<float, 4, 0>;
|
||||
using V3 = Vector<float, 3, 0>;
|
||||
|
||||
// v as a row vector times a 4x4, translation included.
|
||||
V3 RowTimes44(Mat44 const& m, V3 v) {
|
||||
const std::array<float, 16> 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<float, 12> 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<std::pair<const char*, Fn>, 15> tests = {{
|
||||
constexpr std::array<std::pair<const char*, Fn>, 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) {
|
||||
|
|
|
|||
171
tests/Quaternion/main.cpp
Normal file
171
tests/Quaternion/main.cpp
Normal file
|
|
@ -0,0 +1,171 @@
|
|||
//SPDX-License-Identifier: LGPL-3.0-only
|
||||
//SPDX-FileCopyrightText: Copyright (C) 2026 Catcrafts®
|
||||
|
||||
// Quaternions on the scalar Vector<T, 4> (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 <cmath>
|
||||
import Crafter.Math;
|
||||
import std;
|
||||
using namespace Crafter;
|
||||
|
||||
namespace {
|
||||
|
||||
using Q = Vector<float, 4, 0>;
|
||||
using V3 = Vector<float, 3, 0>;
|
||||
|
||||
constexpr float kEps = 1e-4f;
|
||||
constexpr float kPi = std::numbers::pi_v<float>;
|
||||
|
||||
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<std::pair<V3, V3>, 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<std::pair<const char*, Fn>, 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<std::string>(fn())) {
|
||||
std::println(std::cerr, "[{}] {}", name, *err);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in a new issue