172 lines
7.7 KiB
C#
172 lines
7.7 KiB
C#
using System.Runtime.InteropServices;
|
|
|
|
namespace KKdBaseLib
|
|
{
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
public struct Quat
|
|
{
|
|
[FieldOffset(0x00)] public float X;
|
|
[FieldOffset(0x04)] public float Y;
|
|
[FieldOffset(0x08)] public float Z;
|
|
[FieldOffset(0x0C)] public float W;
|
|
|
|
public static Quat Identity => new Quat(0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
public Quat(float x, float y, float z)
|
|
{
|
|
x *= 0.5f;
|
|
y *= 0.5f;
|
|
z *= 0.5f;
|
|
|
|
var c1 = (float)System.Math.Cos(x);
|
|
var c2 = (float)System.Math.Cos(y);
|
|
var c3 = (float)System.Math.Cos(z);
|
|
var s1 = (float)System.Math.Sin(x);
|
|
var s2 = (float)System.Math.Sin(y);
|
|
var s3 = (float)System.Math.Sin(z);
|
|
|
|
X = s1 * c2 * c3 + c1 * s2 * s3;
|
|
Y = c1 * s2 * c3 - s1 * c2 * s3;
|
|
Z = c1 * c2 * s3 + s1 * s2 * c3;
|
|
W = c1 * c2 * c3 - s1 * s2 * s3;
|
|
}
|
|
|
|
public Quat(Vec3 vec) : this(vec.X, vec.Y, vec.Z)
|
|
{ }
|
|
|
|
public Quat(float x, float y, float z, float w)
|
|
{ X = x; Y = y; Z = z; W = w; }
|
|
|
|
public Quat(Vec3 vec, float w)
|
|
{ X = vec.X; Y = vec.Y; Z = vec.Z; W = w; }
|
|
|
|
public Quat(Vec4 vec)
|
|
{ X = vec.X; Y = vec.Y; Z = vec.Z; W = vec.W; }
|
|
|
|
public Vec3 XYZ { get => new Vec3(X, Y, Z); set { X = value.X; Y = value.Y; Z = value.Z; } }
|
|
|
|
public float Length => (X * X + Y * Y + Z * Z + W * W).Sqrt();
|
|
public float LengthSquared => X * X + Y * Y + Z * Z + W * W;
|
|
public Quat Normalized => new Quat(X, Y, Z, W) * (Length == 0.0f ? 1.0f : (1.0f / Length));
|
|
public Vec3 Euler { get
|
|
{
|
|
if (LengthSquared == 0) return default;
|
|
|
|
const float SINGULARITY_THRESHOLD = 0.4999995f;
|
|
const float HalfPI = (float)(System.Math.PI * 0.5);
|
|
|
|
float sqx = X * X, sqy = Y * Y, sqz = Z * Z, sqw = W * W;
|
|
float unit = sqx + sqy + sqz + sqw;
|
|
float singularityTest = X * Z + Y * W;
|
|
|
|
if (singularityTest > SINGULARITY_THRESHOLD * unit)
|
|
return new Vec3(0.0f, HalfPI, (float)(System.Math.Atan2(X, W) * 2.0));
|
|
else if (singularityTest < -SINGULARITY_THRESHOLD * unit)
|
|
return new Vec3(0.0f, -HalfPI, -(float)(System.Math.Atan2(X, W) * 2.0));
|
|
else
|
|
return new Vec3((float)System.Math.Atan2(2 * (X * W - Y * Z), sqw - sqx - sqy + sqz),
|
|
(float)System.Math.Asin (2 * singularityTest / unit),
|
|
(float)System.Math.Atan2(2 * (Z * W - X * Y), sqw + sqx - sqy - sqz));
|
|
}
|
|
}
|
|
public Vec3 EulerDeg { get
|
|
{
|
|
if (LengthSquared == 0) return default;
|
|
|
|
const float SINGULARITY_THRESHOLD = 0.4999995f;
|
|
const float HalfPI = (float)(System.Math.PI * 0.5);
|
|
|
|
float sqx = X * X, sqy = Y * Y, sqz = Z * Z, sqw = W * W;
|
|
float unit = sqx + sqy + sqz + sqw;
|
|
float singularityTest = X * Z + Y * W;
|
|
|
|
if (singularityTest > SINGULARITY_THRESHOLD * unit)
|
|
return new Vec3(0.0f, HalfPI, (float)(System.Math.Atan2(X, W) * 2.0));
|
|
else if (singularityTest < -SINGULARITY_THRESHOLD * unit)
|
|
return new Vec3(0.0f, -HalfPI, -(float)(System.Math.Atan2(X, W) * 2.0));
|
|
else
|
|
return new Vec3((float)System.Math.Atan2(2 * (X * W - Y * Z), sqw - sqx - sqy + sqz),
|
|
(float)System.Math.Asin (2 * singularityTest / unit),
|
|
(float)System.Math.Atan2(2 * (Z * W - X * Y), sqw + sqx - sqy - sqz))
|
|
* (float)(180.0 / System.Math.PI);
|
|
}
|
|
}
|
|
|
|
public static Quat operator +(Quat left, Quat right)
|
|
{ left.X += right.X; left.Y += right.Y; left.Z += right.Z; left.W += right.W; return left; }
|
|
public static Quat operator -(Quat left, Quat right)
|
|
{ left.X -= right.X; left.Y -= right.Y; left.Z -= right.Z; left.W -= right.W; return left; }
|
|
public static Quat operator -(Quat quat) =>
|
|
new Quat(-quat.X, -quat.Y, -quat.Z, -quat.W);
|
|
public static Quat operator *( Quat quat, float scale)
|
|
{ quat.X *= scale ; quat.Y *= scale ; quat.Z *= scale ; quat.W *= scale ; return quat; }
|
|
public static Quat operator *(float scale, Quat quat)
|
|
{ quat.X *= scale ; quat.Y *= scale ; quat.Z *= scale ; quat.W *= scale ; return quat; }
|
|
public static Quat operator *( Quat quat, Vec4 scale)
|
|
{ quat.X *= scale.X; quat.Y *= scale.Y; quat.Z *= scale.Z; quat.W *= scale.W; return quat; }
|
|
public static Quat operator *( Quat left, Quat right) =>
|
|
new Quat(right.W * left.XYZ + left.W * right.XYZ + Vec3.Cross(left.XYZ, right.XYZ),
|
|
left.W * right.W - Vec3.Dot(left.XYZ, right.XYZ)).Normalized;
|
|
public static Quat operator /( Quat left, Quat right) =>
|
|
left * -right;
|
|
public static bool operator ==(Quat A, Quat B) => A.Equals(B);
|
|
public static bool operator !=(Quat A, Quat B) => !A.Equals(B);
|
|
|
|
public static float Distance (Quat left, Quat right) =>
|
|
((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y) +
|
|
(right.Z - left.Z) * (right.Z - left.Z) + (right.W - left.W) * (right.W - left.W)).Sqrt();
|
|
public static float DistanceSquared(Quat left, Quat right) =>
|
|
(right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y) +
|
|
(right.Z - left.Z) * (right.Z - left.Z) + (right.W - left.W) * (right.W - left.W);
|
|
public static float Dot(Quat left, Quat right) =>
|
|
left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W;
|
|
public Quat Round( ) =>
|
|
new Quat { X = X.Round( ), Y = Y.Round( ), Z = Z.Round( ), W = W.Round( ) };
|
|
public Quat Round(int d) =>
|
|
new Quat { X = X.Round(d), Y = Y.Round(d), Z = Z.Round(d), W = W.Round(d) };
|
|
|
|
public static Quat Slerp(Quat q1, Quat q2, float blend)
|
|
{
|
|
q1 = q1.Normalized;
|
|
q2 = q2.Normalized;
|
|
|
|
float dot = Dot(q1, q2);
|
|
if (dot < 0.0f)
|
|
{
|
|
q2 = -q2;
|
|
dot = -dot;
|
|
}
|
|
|
|
Quat result;
|
|
const float DOT_THRESHOLD = 0.9995f;
|
|
if (dot <= DOT_THRESHOLD)
|
|
{
|
|
float theta_0 = (float)System.Math.Acos(dot);
|
|
float theta = theta_0 * blend;
|
|
float sin_theta = (float)System.Math.Sin(theta);
|
|
float sin_theta_0 = (float)System.Math.Sin(theta_0);
|
|
|
|
float s0 = (float)System.Math.Cos(theta) - dot * sin_theta / sin_theta_0;
|
|
float s1 = sin_theta / sin_theta_0;
|
|
result = s0 * q1 + s1 * q2;
|
|
}
|
|
else
|
|
result = (1.0f - blend) * q1 + blend * q2;
|
|
return result.Normalized;
|
|
}
|
|
|
|
public static explicit operator Vec4(Quat quat) =>
|
|
new Vec4(quat.XYZ, quat.W);
|
|
public static explicit operator Quat(Vec4 vec) =>
|
|
new Quat( vec.XYZ, vec.W);
|
|
|
|
public bool Equals(Quat other) =>
|
|
X == other.X && Y == other.Y && Z == other.Z && W == other.W;
|
|
|
|
public override bool Equals(object obj) => base.Equals(obj);
|
|
public override int GetHashCode() => base.GetHashCode();
|
|
public override string ToString() => $"({X}; {Y}; {Z}; {W})";
|
|
public string ToString(int d) => $"({X.Round(d)}; {Y.Round(d)}; {Z.Round(d)}; {W.Round(d)})";
|
|
}
|
|
}
|