Jorijn van der Graaf
087c8b32c3
- 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>
565 lines
No EOL
19 KiB
C++
Executable file
565 lines
No EOL
19 KiB
C++
Executable file
//SPDX-License-Identifier: LGPL-3.0-only
|
|
//SPDX-FileCopyrightText: Copyright (C) 2026 Catcrafts®
|
|
|
|
export module Crafter.Math:Vector;
|
|
import std;
|
|
|
|
namespace Crafter {
|
|
template <typename T, std::uint32_t Len, std::uint32_t Aligment>
|
|
struct __attribute__((packed)) VectorBase {
|
|
T v[Aligment];
|
|
};
|
|
|
|
template <typename T, std::uint32_t Aligment>
|
|
struct __attribute__((packed)) VectorBase<T, 1, Aligment> {
|
|
union {
|
|
T v[Aligment];
|
|
T x, r;
|
|
};
|
|
};
|
|
|
|
template <typename T, std::uint32_t Aligment>
|
|
struct __attribute__((packed)) VectorBase<T, 2, Aligment> {
|
|
union {
|
|
T v[Aligment];
|
|
struct __attribute__((packed)) {
|
|
T x, y;
|
|
};
|
|
struct __attribute__((packed)) {
|
|
T r, g;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 2, Aligment>(T x, T y): x(x), y(y) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 2, Aligment>() = default;
|
|
};
|
|
|
|
template <typename T, std::uint32_t Aligment>
|
|
struct __attribute__((packed)) VectorBase<T, 3, Aligment> {
|
|
union {
|
|
T v[Aligment];
|
|
struct __attribute__((packed)) {
|
|
T x, y, z;
|
|
};
|
|
struct __attribute__((packed)) {
|
|
T r, g, b;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 3, Aligment>(T x, T y, T z): x(x), y(y), z(z) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 3, Aligment>() = default;
|
|
};
|
|
|
|
template <typename T, std::uint32_t Aligment>
|
|
struct __attribute__((packed)) VectorBase<T, 4, Aligment> {
|
|
union {
|
|
T v[Aligment];
|
|
struct __attribute__((packed)) {
|
|
T x, y, z, w;
|
|
};
|
|
struct __attribute__((packed)) {
|
|
T r, g, b, a;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 4, Aligment>(T x, T y, T z, T w): x(x), y(y), z(z), w(w) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 4, Aligment>() = default;
|
|
};
|
|
|
|
template <typename T, std::uint32_t Len>
|
|
struct VectorBase<T, Len, 0> {
|
|
T v[Len];
|
|
};
|
|
|
|
template <typename T>
|
|
struct VectorBase<T, 1, 0> {
|
|
union {
|
|
T v[1];
|
|
T x, r;
|
|
};
|
|
};
|
|
|
|
template <typename T>
|
|
struct VectorBase<T, 2, 0> {
|
|
union {
|
|
T v[2];
|
|
struct {
|
|
T x, y;
|
|
};
|
|
struct {
|
|
T r, g;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 2, 0>(T x, T y): x(x), y(y) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 2, 0>() = default;
|
|
};
|
|
|
|
template <typename T>
|
|
struct VectorBase<T, 3, 0> {
|
|
union {
|
|
T v[3];
|
|
struct {
|
|
T x, y, z;
|
|
};
|
|
struct {
|
|
T r, g, b;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 3, 0>(T x, T y, T z): x(x), y(y), z(z) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 3, 0>() = default;
|
|
};
|
|
|
|
template <typename T>
|
|
struct VectorBase<T, 4, 0> {
|
|
union {
|
|
T v[4];
|
|
struct {
|
|
T x, y, z, w;
|
|
};
|
|
struct {
|
|
T r, g, b, a;
|
|
};
|
|
};
|
|
constexpr VectorBase<T, 4, 0>(T x, T y, T z, T w): x(x), y(y), z(z), w(w) {
|
|
|
|
}
|
|
constexpr VectorBase<T, 4, 0>() = default;
|
|
};
|
|
|
|
export template <typename T, std::uint32_t Len, std::uint32_t Aligment = 0>
|
|
class Vector : public VectorBase<T, Len, Aligment> {
|
|
public:
|
|
constexpr Vector(T x, T y, T z, T w ) requires(std::same_as<T, T> && Len == 4) : VectorBase<T, Len, Aligment>(x, y, z, w) {
|
|
|
|
}
|
|
constexpr Vector(T x, T y, T z) requires(std::same_as<T, T> && Len == 3) : VectorBase<T, Len, Aligment>(x, y, z) {
|
|
|
|
}
|
|
constexpr Vector(T x, T y) requires(std::same_as<T, T> && Len == 2) : VectorBase<T, Len, Aligment>(x, y) {
|
|
|
|
}
|
|
|
|
constexpr Vector() = default;
|
|
|
|
template <std::uint32_t BAlignment>
|
|
constexpr operator Vector<T, Len, BAlignment>() const {
|
|
Vector<T, Len, BAlignment> returnVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
returnVector.v[i] = this->v[i];
|
|
}
|
|
return returnVector;
|
|
}
|
|
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr Vector<T, Len, Aligment> operator+(Vector<T, Blen, BAlignment> b) const {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
resultVector.v[i] = this->v[i]+b.v[i];
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr Vector<T, Len, Aligment> operator-(Vector<T, Blen, BAlignment> b) const {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
resultVector.v[i] = this->v[i]-b.v[i];
|
|
}
|
|
return resultVector;
|
|
}
|
|
constexpr Vector<T, Len, Aligment> operator-(){
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
resultVector.v[i] = -this->v[i];
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr Vector<T, Len, Aligment> operator*(Vector<T, Blen, BAlignment> b) const {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
resultVector.v[i] = this->v[i]*b.v[i];
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr Vector<T, Len, Aligment> operator/(Vector<T, Blen, BAlignment> b) const {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
resultVector.v[i] = this->v[i]/b.v[i];
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr void operator+=(Vector<T, Blen, BAlignment> b){
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
this->v[i]+=b.v[i];
|
|
}
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr void operator-=(Vector<T, Blen, BAlignment> b){
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
this->v[i]-=b.v[i];
|
|
}
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr void operator*=(Vector<T, Blen, BAlignment> b){
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
this->v[i]*=b.v[i];
|
|
}
|
|
}
|
|
template <std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr void operator/=(Vector<T, Blen, BAlignment> b){
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
this->v[i]/=b.v[i];
|
|
}
|
|
}
|
|
|
|
template <typename BT>
|
|
constexpr Vector<T, Len, Aligment> operator+(BT b) const requires std::is_arithmetic_v<BT> {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
resultVector.v[i] = this->v[i] + b;
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <typename BT>
|
|
constexpr Vector<T, Len, Aligment> operator-(BT b) const requires std::is_arithmetic_v<BT> {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
resultVector.v[i] = this->v[i] - b;
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <typename BT>
|
|
constexpr Vector<T, Len, Aligment> operator*(BT b) const requires std::is_arithmetic_v<BT> {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
resultVector.v[i] = this->v[i] * b;
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <typename BT>
|
|
constexpr Vector<T, Len, Aligment> operator/(BT b) const requires std::is_arithmetic_v<BT> {
|
|
Vector<T, Len, Aligment> resultVector;
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
resultVector.v[i] = this->v[i] / b;
|
|
}
|
|
return resultVector;
|
|
}
|
|
template <typename BT>
|
|
constexpr void operator+=(BT b) requires std::is_arithmetic_v<BT>{
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] += b;
|
|
}
|
|
}
|
|
template <typename BT>
|
|
constexpr void operator-=(BT b) requires std::is_arithmetic_v<BT>{
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] -= b;
|
|
}
|
|
}
|
|
template <typename BT>
|
|
constexpr void operator*=(BT b) requires std::is_arithmetic_v<BT>{
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] *= b;
|
|
}
|
|
}
|
|
template <typename BT>
|
|
constexpr void operator/=(BT b) requires std::is_arithmetic_v<BT>{
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] /= b;
|
|
}
|
|
}
|
|
|
|
template <typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr bool operator==(Vector<BT, Blen, BAlignment> b) const {
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
if(this->v[i] != b.v[i]) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
template <typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr bool operator!=(Vector<BT, Blen, BAlignment> b) const {
|
|
for(std::uint32_t i = 0; i < std::min(Len, Blen); i++) {
|
|
if(this->v[i] != b.v[i]) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
constexpr void Normalize() {
|
|
T fLength = Length();
|
|
|
|
fLength = 1.0f / fLength;
|
|
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] *= fLength;
|
|
}
|
|
}
|
|
|
|
constexpr void Invert() {
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
this->v[i] = -this->v[i];
|
|
}
|
|
}
|
|
|
|
constexpr T Length() const {
|
|
T Result = LengthSq();
|
|
return std::sqrtf(Result);
|
|
}
|
|
|
|
constexpr T LengthSq() const {
|
|
return Dot(*this, *this);
|
|
}
|
|
|
|
template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment, typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr static Vector<T, Len, Aligment> Cross(Vector<AT, Alen, AAlignment> a, Vector<BT, Blen, BAlignment> b) requires(Len == 3 && Alen >= 3 && Blen >= 3) {
|
|
return Vector<T, Len, Aligment>(
|
|
(a.v[1] * b.v[2]) - (a.v[2] * b.v[1]),
|
|
(a.v[2] * b.v[0]) - (a.v[0] * b.v[2]),
|
|
(a.v[0] * b.v[1]) - (a.v[1] * b.v[0])
|
|
);
|
|
}
|
|
|
|
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, Len, Aligment> returned;
|
|
T fLength = a.Length();
|
|
|
|
if (fLength > 0) {
|
|
fLength = 1.0f / fLength;
|
|
}
|
|
|
|
for(std::uint32_t i = 0; i < Len; i++) {
|
|
returned.v[i] = a.v[i] * fLength;
|
|
}
|
|
return returned;
|
|
}
|
|
|
|
template <typename AT, std::uint32_t Alen, std::uint32_t AAlignment, typename BT, std::uint32_t Blen, std::uint32_t BAlignment>
|
|
constexpr static T Dot(Vector<AT, Alen, AAlignment> a, Vector<BT, Blen, BAlignment> b) requires(Alen >= Len && Blen >= Len) {
|
|
T accumulate = a.v[0] * b.v[0];
|
|
for(std::uint32_t i = 1; i < Len; i++) {
|
|
accumulate += a.v[i] * b.v[i];
|
|
}
|
|
return accumulate;
|
|
}
|
|
|
|
template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment>
|
|
constexpr static Vector<T, 3, Aligment> Rotate(Vector<AT, 3, AAlignment> v, Vector<BT, 4, BAlignment> q) requires(Len == 3) {
|
|
Vector<T, 3, 0> qv(q.x, q.y, q.z);
|
|
Vector<T, 3, 0> t = Vector<T, 3, Aligment>::Cross(qv, v) * T(2);
|
|
return v + t * q.w + Vector<T, 3, Aligment>::Cross(qv, t);
|
|
}
|
|
|
|
template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment, typename PT, std::uint32_t PAlignment>
|
|
constexpr static Vector<T, 3, Aligment> RotatePivot(Vector<AT, 3, AAlignment> v, Vector<BT, 4, BAlignment> q, Vector<PT, 3, PAlignment> pivot) requires(Len == 3) {
|
|
Vector<T, 3, 0> translated = v - pivot;
|
|
Vector<T, 3, 0> qv(q.x, q.y, q.z);
|
|
Vector<T, 3, 0> t = Cross(qv, translated) * T(2);
|
|
Vector<T, 3, 0> rotated = translated + t * q.w +Cross(qv, t);
|
|
return rotated + pivot;
|
|
}
|
|
|
|
template <typename AT, std::uint32_t AAlignment, typename BT, std::uint32_t BAlignment, typename CT, std::uint32_t CAlignment>
|
|
constexpr static Vector<T, 4, Aligment> QuanternionFromBasis(Vector<AT, 3, AAlignment> right, Vector<BT, 3, BAlignment> up, Vector<CT, 3, CAlignment> forward) requires(Len == 4) {
|
|
T m00 = right.x;
|
|
T m01 = up.x;
|
|
T m02 = forward.x;
|
|
|
|
T m10 = right.y;
|
|
T m11 = up.y;
|
|
T m12 = forward.y;
|
|
|
|
T m20 = right.z;
|
|
T m21 = up.z;
|
|
T m22 = forward.z;
|
|
|
|
T trace = m00 + m11 + m22;
|
|
|
|
Vector<T, 4, Aligment> q;
|
|
|
|
if (trace > std::numeric_limits<T>::epsilon()) {
|
|
T s = std::sqrt(trace + T(1)) * T(2);
|
|
q.w = T(0.25) * s;
|
|
q.x = (m21 - m12) / s;
|
|
q.y = (m02 - m20) / s;
|
|
q.z = (m10 - m01) / s;
|
|
}
|
|
else if ((m00 > m11) && (m00 > m22)) {
|
|
T s = std::sqrt(T(1) + m00 - m11 - m22) * T(2);
|
|
q.w = (m21 - m12) / s;
|
|
q.x = T(0.25) * s;
|
|
q.y = (m01 + m10) / s;
|
|
q.z = (m02 + m20) / s;
|
|
}
|
|
else if (m11 > m22) {
|
|
T s = std::sqrt(T(1) + m11 - m00 - m22) * T(2);
|
|
q.w = (m02 - m20) / s;
|
|
q.x = (m01 + m10) / s;
|
|
q.y = T(0.25) * s;
|
|
q.z = (m12 + m21) / s;
|
|
}
|
|
else {
|
|
T s = std::sqrt(T(1) + m22 - m00 - m11) * T(2);
|
|
q.w = (m10 - m01) / s;
|
|
q.x = (m02 + m20) / s;
|
|
q.y = (m12 + m21) / s;
|
|
q.z = T(0.25) * s;
|
|
}
|
|
|
|
q.Normalize();
|
|
return q;
|
|
}
|
|
|
|
constexpr static Vector<T, 4, Aligment> QuanternionFromEuler(T rollHalf, T pitchHalf, T yawHalf) {
|
|
T cr = std::cos(rollHalf);
|
|
T sr = std::sin(rollHalf);
|
|
T cp = std::cos(pitchHalf);
|
|
T sp = std::sin(pitchHalf);
|
|
T cy = std::cos(yawHalf);
|
|
T sy = std::sin(yawHalf);
|
|
|
|
return Vector<T, 4, Aligment>(
|
|
sr * cp * cy - cr * sp * sy,
|
|
cr * sp * cy + sr * cp * sy,
|
|
cr * cp * sy - sr * sp * cy,
|
|
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
|
|
);
|
|
}
|
|
};
|
|
}
|
|
|
|
|
|
|
|
template <typename T, std::uint32_t Len, std::uint32_t Aligment>
|
|
struct std::formatter<Crafter::Vector<T, Len, Aligment>> : std::formatter<std::string> {
|
|
auto format(const Crafter::Vector<T, Len, Aligment>& vec, format_context& ctx) const {
|
|
std::string out = "{";
|
|
for(std::uint32_t i2 = 0; i2 < Len; i2++) {
|
|
#ifdef __x86_64
|
|
if constexpr(std::same_as<T, _Float16>) {
|
|
out += std::format("{}", static_cast<float>(vec.v[i2]));
|
|
} else
|
|
#endif
|
|
{
|
|
out += std::format("{}", vec.v[i2]);
|
|
}
|
|
if (i2 + 1 < Len) out += ",";
|
|
}
|
|
out += "}";
|
|
return std::formatter<std::string>::format(out, ctx);
|
|
}
|
|
};
|
|
|
|
template <typename T, std::uint32_t Len, std::uint32_t Aligment, typename BT>
|
|
constexpr Crafter::Vector<T, Len, Aligment> operator*(BT b, const Crafter::Vector<T, Len, Aligment>& v) requires std::is_arithmetic_v<BT> {
|
|
return v * b;
|
|
} |