Catcrafts/Crafter.Math
Watch
2
0
Fork
You've already forked Crafter.Math
 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity Actions

feat: quaternion operations, and transforms that mean the same in 4x4 and 4x3

Browse source 
- 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:
Jorijn van der Graaf 2026-10-20 19:06:47 +02:00
commit 087c8b32c3
Signed by: jorijnvdgraaf
GPG key ID: 2937E59CDCC1BCFB
6 changed files with 533 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

98
interfaces/Crafter.Math-Vector.cppm
View file

@ -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
);
}
};
}