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
|
|
@ -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
|
||||
);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue