mirror of
https://github.com/aharabada/glitchy-engine-beef.git
synced 2026-09-05 13:01:52 +00:00
381 lines
9.4 KiB
Beef
381 lines
9.4 KiB
Beef
using Bon;
|
|
using System;
|
|
|
|
namespace GlitchyEngine.Math
|
|
{
|
|
[BonTarget]
|
|
public struct Quaternion
|
|
{
|
|
public const Quaternion Zero = .();
|
|
public const Quaternion One = .(1.0f, 1.0f, 1.0f, 1.0f);
|
|
public const Quaternion Identity = .(0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
public float X, Y, Z, W;
|
|
|
|
public this() => this = default;
|
|
|
|
public this(float x, float y, float z, float w)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
W = w;
|
|
}
|
|
|
|
public this(Vector3 xy, float z, float w)
|
|
{
|
|
X = xy.X;
|
|
Y = xy.Y;
|
|
Z = z;
|
|
W = w;
|
|
}
|
|
|
|
public this(Vector3 xyz, float w)
|
|
{
|
|
X = xyz.X;
|
|
Y = xyz.Y;
|
|
Z = xyz.Z;
|
|
W = w;
|
|
}
|
|
|
|
public this(Vector4 vector)
|
|
{
|
|
X = vector.X;
|
|
Y = vector.Y;
|
|
Z = vector.Z;
|
|
W = vector.W;
|
|
}
|
|
|
|
public Vector3 Vector
|
|
{
|
|
get => .(X, Y, Z);
|
|
set mut
|
|
{
|
|
X = value.X;
|
|
Y = value.Y;
|
|
Z = value.Z;
|
|
}
|
|
}
|
|
|
|
public float Scalar
|
|
{
|
|
get => W;
|
|
set mut => W = value;
|
|
}
|
|
|
|
public void Normalize() mut
|
|
{
|
|
float invLength = 1.0f / Length();
|
|
|
|
X *= invLength;
|
|
Y *= invLength;
|
|
Z *= invLength;
|
|
W *= invLength;
|
|
}
|
|
|
|
public static Quaternion Normalize(Quaternion q)
|
|
{
|
|
float invLength = 1.0f / Length(q);
|
|
|
|
return Quaternion(q.X * invLength, q.Y * invLength, q.Z * invLength, q.W * invLength);
|
|
}
|
|
|
|
public float Length()
|
|
{
|
|
return Math.Sqrt([Inline]LengthSquared());
|
|
}
|
|
|
|
public static float Length(Quaternion q)
|
|
{
|
|
return Math.Sqrt([Inline]LengthSquared(q));
|
|
}
|
|
|
|
public float LengthSquared()
|
|
{
|
|
return X * X + Y * Y + Z * Z + W * W;
|
|
}
|
|
|
|
public static float LengthSquared(Quaternion q)
|
|
{
|
|
return q.X * q.X + q.Y * q.Y + q.Z * q.Z + q.W * q.W;
|
|
}
|
|
|
|
public static float Dot(Self l, Self r)
|
|
{
|
|
return l.X * r.X + l.Y * r.Y + l.Z * r.Z + l.W * r.W;
|
|
}
|
|
|
|
public static Quaternion Lerp(Quaternion a, Quaternion b, float interpolationValue)
|
|
{
|
|
return a + interpolationValue * (b - a);
|
|
}
|
|
|
|
public static Quaternion Slerp(Quaternion previousQuaternion, Quaternion nextQuaternion, float interpolationValue)
|
|
{
|
|
// Based on https://github.com/KhronosGroup/glTF-Tutorials/blob/master/gltfTutorial/gltfTutorial_007_Animations.md
|
|
|
|
var nextQuaternion;
|
|
|
|
float dot = Dot(previousQuaternion, nextQuaternion);
|
|
|
|
//make sure we take the shortest path in case dot Product is negative
|
|
if(dot < 0.0f)
|
|
{
|
|
nextQuaternion = -nextQuaternion;
|
|
dot = -dot;
|
|
}
|
|
|
|
//if the two quaternions are too close to each other, just linear interpolate between the 4D vector
|
|
if(dot > 0.9995f)
|
|
return Normalize(previousQuaternion + interpolationValue * (nextQuaternion - previousQuaternion));
|
|
|
|
//perform the spherical linear interpolation
|
|
var theta_0 = Math.Acos(dot);
|
|
var theta = interpolationValue * theta_0;
|
|
var sin_theta = Math.Sin(theta);
|
|
var sin_theta_0 = Math.Sin(theta_0);
|
|
|
|
var scalePreviousQuat = Math.Cos(theta) - dot * sin_theta / sin_theta_0;
|
|
var scaleNextQuat = sin_theta / sin_theta_0;
|
|
return scalePreviousQuat * previousQuaternion + scaleNextQuat * nextQuaternion;
|
|
}
|
|
|
|
public static Quaternion FromMatrix(Matrix matrix)
|
|
{
|
|
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
|
|
|
|
var m = matrix.V;
|
|
|
|
Quaternion result = ?;
|
|
|
|
float tr = m._11 + m._22 + m._33;
|
|
|
|
if (tr > 0) {
|
|
float S = Math.Sqrt(tr + 1.0f) * 2f; // S=4*result.W
|
|
result.W = 0.25f * S;
|
|
result.X = (m._32 - m._23) / S;
|
|
result.Y = (m._13 - m._31) / S;
|
|
result.Z = (m._21 - m._12) / S;
|
|
} else if ((m._11 > m._22)&(m._11 > m._33)) {
|
|
float S = Math.Sqrt(1.0f + m._11 - m._22 - m._33) * 2f; // S=4*result.X
|
|
result.W = (m._32 - m._23) / S;
|
|
result.X = 0.25f * S;
|
|
result.Y = (m._12 + m._21) / S;
|
|
result.Z = (m._13 + m._31) / S;
|
|
} else if (m._22 > m._33) {
|
|
float S = Math.Sqrt(1.0f + m._22 - m._11 - m._33) * 2f; // S=4*result.Y
|
|
result.W = (m._13 - m._31) / S;
|
|
result.X = (m._12 + m._21) / S;
|
|
result.Y = 0.25f * S;
|
|
result.Z = (m._23 + m._32) / S;
|
|
} else {
|
|
float S = Math.Sqrt(1.0f + m._33 - m._11 - m._22) * 2f; // S=4*result.Z
|
|
result.W = (m._21 - m._12) / S;
|
|
result.X = (m._13 + m._31) / S;
|
|
result.Y = (m._23 + m._32) / S;
|
|
result.Z = 0.25f * S;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
public static Self operator +(Self value) => value;
|
|
|
|
public static Self operator +(Self l, Self r) => Self(l.X + r.X, l.Y + r.Y, l.Z + r.Z, l.W + r.W);
|
|
|
|
public static Self operator -(Self value) => Self(-value.X, -value.Y, -value.Z, -value.W);
|
|
|
|
public static Self operator -(Self l, Self r) => Self(l.X - r.X, l.Y - r.Y, l.Z - r.Z, l.W - r.W);
|
|
|
|
public static Self operator *(float l, Self r) => Self(l * r.X, l * r.Y, l * r.Z, l * r.W);
|
|
|
|
public static Self operator /(Self l, float r) => Self(l.X * r, l.Y * r, l.Z * r, l.W * r);
|
|
|
|
public static Self operator *(Self l, Self r)
|
|
{
|
|
Quaternion result;
|
|
|
|
Vector3 v = l.Vector * r.Vector + (l.W * r.Vector) + (r.W * l.Vector);
|
|
result.X = v.X;
|
|
result.Y = v.Y;
|
|
result.Z = v.Z;
|
|
result.W = (l.W * r.W) - Vector3.Dot(l.Vector, r.Vector);
|
|
|
|
return result;
|
|
}
|
|
|
|
public static Quaternion Conjugate(Quaternion q)
|
|
{
|
|
return Quaternion(-q.Vector, q.Scalar);
|
|
}
|
|
|
|
public static Quaternion Inverse(Quaternion q)
|
|
{
|
|
Quaternion conjugate = Conjugate(q);
|
|
|
|
float magSquared = LengthSquared(q);
|
|
|
|
return conjugate / magSquared;
|
|
}
|
|
|
|
[Inline]
|
|
#unwarn
|
|
public static implicit operator Vector4(in Self value) => *(Vector4*)&value;
|
|
|
|
[Inline]
|
|
#unwarn
|
|
public static implicit operator Quaternion(in Vector4 value) => *(Quaternion*)&value;
|
|
|
|
public static bool operator ==(Quaternion l, Quaternion r)
|
|
{
|
|
return l.X == r.X && l.Y == r.Y && l.Z == r.Z && l.W == r.W;
|
|
}
|
|
|
|
public static bool operator !=(Quaternion l, Quaternion r)
|
|
{
|
|
return l.X != r.X && l.Y != r.Y && l.Z != r.Z && l.W != r.W;
|
|
}
|
|
|
|
public (Vector3 Axis, float Angle) ToAxisAngle()
|
|
{
|
|
// scalar part = cos(θ/2)
|
|
// So, we can extract the angle directly.
|
|
float angle = 2.0f * Math.Acos(W);
|
|
|
|
// vector part = axis * sin(θ/2)
|
|
// In other words, the vector part is the axis, but with length of sin(θ/2).
|
|
// We assume quaternion is unit length, so subtracting w^2 gives us length of just vector part (aka sin(θ/2)).
|
|
float length = Math.Sqrt(1.0f - (W * W));
|
|
|
|
Vector3 axis;
|
|
|
|
// Normalize vector part to get the axis!
|
|
if(length == 0)
|
|
{
|
|
axis = Vector3.Zero;
|
|
}
|
|
else
|
|
{
|
|
length = 1.0f / length;
|
|
axis.X = X * length;
|
|
axis.Y = Y * length;
|
|
axis.Z = Z * length;
|
|
}
|
|
|
|
return (axis, angle);
|
|
}
|
|
|
|
public static Quaternion FromAxisAngle(Vector3 axis, float angle)
|
|
{
|
|
float lengthSq = axis.MagnitudeSquared();
|
|
|
|
if(lengthSq == 0)
|
|
{
|
|
return .Identity;
|
|
}
|
|
|
|
float halfAngle = angle * 0.5f;
|
|
|
|
float sin = Math.Sin(halfAngle) / Math.Sqrt(lengthSq);
|
|
|
|
Quaternion result;
|
|
result.X = axis.X * sin;
|
|
result.Y = axis.Y * sin;
|
|
result.Z = axis.Z * sin;
|
|
result.W = Math.Cos(halfAngle);
|
|
|
|
return result;
|
|
}
|
|
|
|
// Assumes YZX-Order meaning Y applied first, Z second and x last
|
|
public static Quaternion FromEulerAngles(float yaw, float pitch, float roll)
|
|
{
|
|
float halfYaw = yaw / 2.0f;
|
|
float halfPitch = pitch / 2.0f;
|
|
float halfRoll = roll / 2.0f;
|
|
|
|
float cosYaw = Math.Cos(halfYaw);//heading
|
|
float sinYaw = Math.Sin(halfYaw);
|
|
float cosRoll = Math.Cos(halfRoll);//attitude
|
|
float sinRoll = Math.Sin(halfRoll);
|
|
float cosPitch = Math.Cos(halfPitch);//bank
|
|
float sinPitch = Math.Sin(halfPitch);
|
|
|
|
float cosYawCosRoll = cosYaw * cosRoll;
|
|
float sinYawSinRoll = sinYaw * sinRoll;
|
|
float cosYawSinRoll = cosYaw * sinRoll;
|
|
float sinYawCosRoll = sinYaw * cosRoll;
|
|
|
|
Quaternion result;
|
|
|
|
result.W = cosYawCosRoll * cosPitch - sinYawSinRoll * sinPitch;
|
|
result.X = cosYawCosRoll * sinPitch + sinYawSinRoll * cosPitch;
|
|
result.Y = sinYawCosRoll * cosPitch + cosYawSinRoll * sinPitch;
|
|
result.Z = cosYawSinRoll * cosPitch - sinYawCosRoll * sinPitch;
|
|
|
|
return result;
|
|
}
|
|
|
|
public static Vector3 ToEulerAngles(Quaternion q)
|
|
{
|
|
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
|
|
|
|
Vector3 result;
|
|
|
|
float sqw = q.W*q.W;
|
|
float sqx = q.X*q.X;
|
|
float sqy = q.Y*q.Y;
|
|
float sqz = q.Z*q.Z;
|
|
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
|
|
float test = q.X*q.Y + q.Z*q.W;
|
|
if (test > 0.4999f*unit) { // singularity at north pole
|
|
result.Y = 2.0f * Math.Atan2(q.X,q.W);
|
|
result.Z = Math.PI_f / 2.0f;
|
|
result.X = 0.0f;
|
|
return result;
|
|
}
|
|
if (test < -0.4999f*unit) { // singularity at south pole
|
|
result.Y = -2.0f * Math.Atan2(q.X,q.W);
|
|
result.Z = -Math.PI_f / 2.0f;
|
|
result.X = 0.0f;
|
|
return result;
|
|
}
|
|
result.Y = Math.Atan2(2*q.Y*q.W-2*q.X*q.Z , sqx - sqy - sqz + sqw);
|
|
result.Z = Math.Asin(2*test/unit);
|
|
result.X = Math.Atan2(2*q.X*q.W-2*q.Y*q.Z , -sqx + sqy - sqz + sqw);
|
|
|
|
return result;
|
|
|
|
//return .(q.Pitch(), q.Yaw(), q.Roll());
|
|
}
|
|
/*
|
|
public float Pitch()
|
|
{
|
|
float y = 2.0f * (Y * Z + W * X);
|
|
float x = W * W - X * X - Y * Y + Z * Z;
|
|
|
|
if (Vector2(x, y).Equals(.Zero)) //avoid atan2(0,0) - handle singularity - Matiis
|
|
return 2.0f * Math.Atan2(X, W);
|
|
|
|
return Math.Atan2(y, x);
|
|
}
|
|
|
|
public float Yaw()
|
|
{
|
|
return Math.Asin(Math.Clamp(-2.0f * (X * Z - W * Y), -1.0f, 1.0f));
|
|
}
|
|
|
|
public float Roll()
|
|
{
|
|
float y = 2.0f * (X * Y + W * Z);
|
|
float x = W * W + X * X - Y * Y - Z * Z;
|
|
|
|
if (Vector2(x, y).Equals(.Zero)) //avoid atan2(0,0) - handle singularity - Matiis
|
|
return 0;
|
|
|
|
return Math.Atan2(y, x);
|
|
}
|
|
*/
|
|
}
|
|
}
|