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
|
|
@ -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<T, Len, Alignment>` with `.x/.y/.z/.w` and `.r/.g/.b/.a` accessors, arithmetic, comparison, and conversion operators. |
|
||||
| `:MatrixRowMajor` | `MatrixRowMajor<T, ColumnSize, RowSize, Repeats>` — row-major matrices with packed-batch support via `Repeats`. |
|
||||
| `:Vector` | Generic `Vector<T, Len, Alignment>` 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<T, ColumnSize, RowSize, Repeats>` — 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<Len, Packing>` — SIMD-specialized 32-bit float vector with `Load`/`Store`, FMA, `Cross`, `Dot`, `Length`, `Normalize`, `Sin`/`Cos`, `Shuffle`, etc. |
|
||||
| `:VectorF16` | `VectorF16<Len, Packing>` — same surface as `VectorF32` for `_Float16` when the target supports it. |
|
||||
| `:Intersection` | `IntersectionTestRayTriangle`, `IntersectionTestRaySphere`, `IntersectionTestRayOrientedBox`. |
|
||||
|
|
|
|||
|
|
@ -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<T, float>) {
|
||||
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 <std::uint32_t QAlignment>
|
||||
static MatrixRowMajor Rotation(Vector<float, 4, QAlignment> q) requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as<T, float>) {
|
||||
return TRS(Vector<float, 3, 0>(0, 0, 0), q, Vector<float, 3, 0>(1, 1, 1));
|
||||
}
|
||||
|
||||
// Scale by s, then rotate by unit quaternion q, then translate by t:
|
||||
// a glTF node's transform.
|
||||
template <std::uint32_t TAlignment, std::uint32_t QAlignment, std::uint32_t SAlignment>
|
||||
static MatrixRowMajor TRS(Vector<float, 3, TAlignment> t, Vector<float, 4, QAlignment> q, Vector<float, 3, SAlignment> s) requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as<T, float>) {
|
||||
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<T, float>) {
|
||||
const std::array<float, RowSize * 4> 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<T, float>) {
|
||||
const std::array<float, 16> 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<float, 3, 0>, Vector<float, 4, 0>, Vector<float, 3, 0>> Decompose() const requires(CollumSize == 4 && (RowSize == 3 || RowSize == 4) && Repeats == 1 && std::same_as<T, float>) {
|
||||
const std::array<float, RowSize * 4> a = Store();
|
||||
// basis[j]: where the transform takes unit vector j.
|
||||
Vector<float, 3, 0> basis[3];
|
||||
for (std::uint32_t j = 0; j < 3; ++j) {
|
||||
basis[j] = RowSize == 3
|
||||
? Vector<float, 3, 0>(a[j], a[4 + j], a[8 + j])
|
||||
: Vector<float, 3, 0>(a[j * 4], a[j * 4 + 1], a[j * 4 + 2]);
|
||||
}
|
||||
const Vector<float, 3, 0> t = RowSize == 3
|
||||
? Vector<float, 3, 0>(a[3], a[7], a[11])
|
||||
: Vector<float, 3, 0>(a[12], a[13], a[14]);
|
||||
Vector<float, 3, 0> s(basis[0].Length(), basis[1].Length(), basis[2].Length());
|
||||
if (Vector<float, 3, 0>::Dot(Vector<float, 3, 0>::Cross(basis[0], basis[1]), basis[2]) < 0) {
|
||||
s.x = -s.x;
|
||||
}
|
||||
const Vector<float, 4, 0> q = Vector<float, 4, 0>::QuanternionFromBasis(basis[0] / s.x, basis[1] / s.y, basis[2] / s.z);
|
||||
return { t, q, s };
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -334,7 +334,7 @@ namespace Crafter {
|
|||
|
||||
template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment>
|
||||
constexpr static Vector<T, Len, Aligment> Normalize(Vector<AT, Alen, AAlignment> a) requires(Len == Alen) {
|
||||
Vector<T, 3, 0> returned;
|
||||
Vector<T, Len, Aligment> 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<T, 4> 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 <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionMultiply(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> b) requires(Len == 4) {
|
||||
return Vector<T, 4, Aligment>(
|
||||
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 <typename AT, std::uint32_t AAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionConjugate(Vector<AT, 4, AAlignment> q) requires(Len == 4) {
|
||||
return Vector<T, 4, Aligment>(-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 <typename AT, std::uint32_t AAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionFromAxisAngle(Vector<AT, 3, AAlignment> axis, T angle) requires(Len == 4) {
|
||||
const T s = std::sin(angle * T(0.5));
|
||||
return Vector<T, 4, Aligment>(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 <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionFromUnitVectors(Vector<AT, 3, AAlignment> from, Vector<BT, 3, BAlignment> to) requires(Len == 4) {
|
||||
const Vector<T, 3, 0> 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<T, 3, 0> axis = std::abs(f.x) < T(0.9)
|
||||
? Vector<T, 3, 0>::Cross(Vector<T, 3, 0>(1, 0, 0), f)
|
||||
: Vector<T, 3, 0>::Cross(Vector<T, 3, 0>(0, 1, 0), f);
|
||||
axis.Normalize();
|
||||
return Vector<T, 4, Aligment>(axis.x, axis.y, axis.z, T(0));
|
||||
}
|
||||
const Vector<T, 3, 0> c = Vector<T, 3, 0>::Cross(f, Vector<T, 3, 0>(to.x, to.y, to.z));
|
||||
Vector<T, 4, Aligment> 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 <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionNlerp(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> 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<T, 4, Aligment> 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 <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
|
||||
constexpr static Vector<T, 4, Aligment> QuanternionSlerp(Vector<AT, 4, AAlignment> a, Vector<BT, 4, BAlignment> 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<T, 4, Aligment>(
|
||||
a.x * wa + b.x * wb,
|
||||
a.y * wa + b.y * wb,
|
||||
a.z * wa + b.z * wb,
|
||||
a.w * wa + b.w * wb
|
||||
);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ extern "C" Configuration CrafterBuildProject(std::span<const std::string_view> 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);
|
||||
|
|
|
|||
|
|
@ -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