using System;
namespace Engine.Core;
///
/// Represents a 3D space rotation.
///
/// X(i) position of the .
/// Y(j) position of the .
/// Z(k) position of the .
/// W(a) position of the .
///
/// Initializes a new instance of the struct with the specified positions.
///
[System.Diagnostics.DebuggerDisplay("{ToString(),nq}, Length: {Magnitude}, LengthSquared: {MagnitudeSquared}, Normalized: {Normalized.ToString(),nq}")]
public readonly struct Quaternion(float x, float y, float z, float w) : IEquatable
{
///
/// The X(i) imaginary of the .
///
public readonly float X = x;
///
/// The Y(j) imaginary of the .
///
public readonly float Y = y;
///
/// The Z(k) imaginary of the .
///
public readonly float Z = z;
///
/// The W(a) scalar of the .
///
public readonly float W = w;
///
/// The magnitude (length) of the .
///
public float Magnitude => Length(this);
///
/// The squared magnitude (length) of the .
///
public float MagnitudeSquared => LengthSquared(this);
///
/// The normalized form of the (a with the same direction and a magnitude of 1).
///
public Quaternion Normalized => Normalize(this);
///
/// Represents the with no rotation.
///
public readonly static Quaternion Zero = new(0f, 0f, 0f, 0f);
///
/// Represents the identity .
///
public readonly static Quaternion Identity = new(0f, 0f, 0f, 1f);
public static Quaternion operator -(Quaternion quaternion) => new(-quaternion.X, -quaternion.Y, -quaternion.Z, quaternion.W);
public static Quaternion operator +(Quaternion left, Quaternion right) => new(left.X + right.X, left.Y + right.Y, left.Z + right.Z, left.W + right.W);
public static Quaternion operator -(Quaternion left, Quaternion right) => new(left.X - right.X, left.Y - right.Y, left.Z - right.Z, left.W - right.W);
public static Quaternion operator *(Quaternion quaternion, float value) => new(quaternion.X * value, quaternion.Y * value, quaternion.Z * value, quaternion.W * value);
public static Quaternion operator *(float value, Quaternion quaternion) => new(quaternion.X * value, quaternion.Y * value, quaternion.Z * value, quaternion.W * value);
public static Quaternion operator *(Quaternion left, Quaternion right)
=> new(
left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y,
left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z,
left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X,
left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z
);
public static Quaternion operator /(Quaternion quaternion, float value) => new(quaternion.X / value, quaternion.Y / value, quaternion.Z / value, quaternion.W / value);
public static bool operator ==(Quaternion left, Quaternion right) => left.X == right.X && left.Y == right.Y && left.Z == right.Z && left.W == right.W;
public static bool operator !=(Quaternion left, Quaternion right) => left.X != right.X || left.Y != right.Y || left.Z != right.Z || left.W != right.W;
public static implicit operator Quaternion(System.Numerics.Quaternion quaternion) => new(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
public static implicit operator System.Numerics.Quaternion(Quaternion quaternion) => new(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
///
/// Get the Pitch, Yaw and Roll of the .
///
public static Vector3D ToAngles(Quaternion quaternion)
{
// Quaternion to Euler angles (in 3-2-1 sequence) conversion
float sinr_cosp = 2f * (quaternion.W * quaternion.X + quaternion.Y * quaternion.Z);
float cosr_cosp = 1f - 2f * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y);
float pitch = MathF.Atan2(sinr_cosp, cosr_cosp);
float sinp = 2f * (quaternion.W * quaternion.Y - quaternion.Z * quaternion.X);
float yaw;
if (MathF.Abs(sinp) >= 1f)
yaw = MathF.CopySign(MathF.PI / 2f, sinp);
else
yaw = MathF.Asin(sinp);
float siny_cosp = 2f * (quaternion.W * quaternion.Z + quaternion.X * quaternion.Y);
float cosy_cosp = 1f - 2f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
float roll = MathF.Atan2(siny_cosp, cosy_cosp);
return new Vector3D(pitch, yaw, roll) * Math.RadianToDegree;
}
///
/// Calculates the length of the .
///
/// The .
/// The length of the .
public static float Length(Quaternion quaternion) => Math.Sqrt(LengthSquared(quaternion));
///
/// Calculates the squared length of the .
///
/// The .
/// The squared length of the .
public static float LengthSquared(Quaternion quaternion) => quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W;
///
/// Adds two s.
///
/// The first .
/// The second .
/// The sum of the two s.
public static Quaternion Add(Quaternion left, Quaternion right) => left + right;
///
/// Subtracts one from another.
///
/// The to subtract from.
/// The to subtract.
/// The result of subtracting the second from the first.
public static Quaternion Subtract(Quaternion left, Quaternion right) => left - right;
///
/// Multiplies a by a scalar value.
///
/// The .
/// The scalar value.
/// The result of multiplying the by the scalar value.
public static Quaternion Multiply(Quaternion quaternion, float value) => quaternion * value;
///
/// Divides a by a scalar value.
///
/// The .
/// The scalar value.
/// The result of dividing the by the scalar value.
public static Quaternion Divide(Quaternion quaternion, float value) => quaternion / value;
///
/// Normalizes the (creates a unit with the same direction).
///
/// The to normalize.
/// The normalized .
public static Quaternion Normalize(Quaternion quaternion) => quaternion / Length(quaternion);
///
/// Rotates a around a axis by the specified angle (in radians).
///
/// The to rotate.
/// The to rotate around.
/// The angle to rotate by, in radians.
/// The rotated .
public static Quaternion Rotate(Quaternion vector, Vector3D axis, float angleInRadian) => vector * Quaternion.FromAxisAngle(axis, angleInRadian);
///
/// Inverts the direction of the .
///
/// The .
/// The inverted .
public static Quaternion Invert(Quaternion quaternion) => Conjugate(quaternion) / LengthSquared(quaternion);
///
/// Conjugate of the .
///
/// The .
/// The inverted .
public static Quaternion Conjugate(Quaternion quaternion) => -quaternion;
///
/// Rotates a by applying the provided .
///
/// The to be rotated.
/// The to used for applying rotation.
/// The rotated .
public static Vector3D RotateVector(Vector3D vector, Quaternion quaternion)
{
// https://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/
// t = 2 * cross(q.xyz, v)
// v' = v + q.w * t + cross(q.xyz, t)
Vector3D quaternionVector = new(quaternion.X, quaternion.Y, quaternion.Z);
Vector3D t = 2f * quaternionVector.Cross(vector);
return vector + quaternion.W * t + quaternionVector.Cross(t);
}
///
/// Performs spherical linear interpolation between two s.
///
/// The starting (t = 0).
/// The target (t = 1).
/// The interpolation parameter.
/// The interpolated .
public static Quaternion SLerp(Quaternion from, Quaternion to, float t)
{
float dot = Dot(from, to);
if (dot < 0.0f)
{
from = new Quaternion(-from.X, -from.Y, -from.Z, -from.W);
dot = -dot;
}
if (dot > 0.9995f)
return Lerp(from, to, t);
float angle = Math.Acos(dot);
float sinAngle = Math.Sin(angle);
float fromWeight = Math.Sin((1f - t) * angle) / sinAngle;
float toWeight = Math.Sin(t * angle) / sinAngle;
return from * fromWeight + to * toWeight;
}
///
/// Performs linear interpolation between two s.
///
/// The starting (t = 0).
/// The target (t = 1).
/// The interpolation parameter.
/// The interpolated .
public static Quaternion Lerp(Quaternion from, Quaternion to, float t) => Normalize(new(from.X.Lerp(to.X, t), from.W.Lerp(to.W, t), from.Z.Lerp(to.Z, t), from.W.Lerp(to.W, t)));
///
/// Calculates the dot product of two s.
///
/// The first .
/// The second .
/// The dot product of the two s.
public static float Dot(Quaternion left, Quaternion right) => left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W;
///
/// Calculates the from given axis and angle.
///
/// The axis of the rotation in .
/// The angle in radians.
/// The rotation calculated by the given parameters.
public static Quaternion FromAxisAngle(Vector3D axis, float angleInRadian)
{
float halfAngle = angleInRadian * .5f;
float sinHalf = Math.Sin(halfAngle);
return new Quaternion(axis.X * sinHalf, axis.Y * sinHalf, axis.Z * sinHalf, Math.Cos(halfAngle));
}
///
/// Calculates the from given yaw, pitch and roll values.
///
/// The rotation calculated by the given parameters.
public static Quaternion FromAngles(float x, float y, float z)
{
float cosX = Math.Cos(x * .5f);
float sinX = Math.Sin(x * .5f);
float cosY = Math.Cos(y * .5f);
float sinY = Math.Sin(y * .5f);
float cozZ = Math.Cos(z * .5f);
float sinZ = Math.Sin(z * .5f);
return new Quaternion(
x: sinX * cosY * cozZ - cosX * sinY * sinZ,
y: cosX * sinY * cozZ + sinX * cosY * sinZ,
z: cosX * cosY * sinZ - sinX * sinY * cozZ,
w: cosX * cosY * cozZ + sinX * sinY * sinZ
);
}
///
/// Calculates the from given .
///
/// The axis of the rotation in .
/// The angle in radians.
/// The rotation calculated by the given .
public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion)
{
float m00 = 1 - 2 * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
float m01 = 2 * (quaternion.X * quaternion.Y - quaternion.W * quaternion.Z);
float m02 = 2 * (quaternion.X * quaternion.Z + quaternion.W * quaternion.Y);
float m03 = 0;
float m10 = 2 * (quaternion.X * quaternion.Y + quaternion.W * quaternion.Z);
float m11 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Z * quaternion.Z);
float m12 = 2 * (quaternion.Y * quaternion.Z - quaternion.W * quaternion.X);
float m13 = 0;
float m20 = 2 * (quaternion.X * quaternion.Z - quaternion.W * quaternion.Y);
float m21 = 2 * (quaternion.Y * quaternion.Z + quaternion.W * quaternion.X);
float m22 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y);
float m23 = 0;
float m30 = 0;
float m31 = 0;
float m32 = 0;
float m33 = 1;
return new(
m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23,
m30, m31, m32, m33
);
}
///
/// Checks if two s are approximately equal within a specified epsilon range.
///
/// The first .
/// The second .
/// The epsilon range.
/// if the s are approximately equal; otherwise, .
public static bool ApproximatelyEquals(Quaternion left, Quaternion right, float epsilon = float.Epsilon)
=> left.X.ApproximatelyEquals(right.X, epsilon) && left.Y.ApproximatelyEquals(right.Y, epsilon) && left.Z.ApproximatelyEquals(right.Z, epsilon) && left.W.ApproximatelyEquals(right.W, epsilon);
///
/// Determines whether the specified object is equal to the current .
///
/// The object to compare with the current .
/// if the specified object is equal to the current ; otherwise, .
public override bool Equals(object? obj) => obj is Quaternion quaternion && this == quaternion;
public bool Equals(Quaternion other) => this == other;
///
/// Generates a hash code for the .
///
/// A hash code for the .
public override int GetHashCode() => System.HashCode.Combine(X, Y, Z, W);
///
/// Converts the to its string representation.
///
/// A string representation of the .
public override string ToString() => $"{nameof(Quaternion)}({X}, {Y}, {Z}, {W})";
}
///
/// Provides extension methods for type.
///
public static class QuaternionExtensions
{
///
public static Vector3D ToAngles(this Quaternion quaternion) => Quaternion.ToAngles(quaternion);
///
public static float Length(this Quaternion quaternion) => Quaternion.Length(quaternion);
///
public static float LengthSquared(this Quaternion quaternion) => Quaternion.LengthSquared(quaternion);
///
public static Quaternion Add(this Quaternion left, Quaternion right) => Quaternion.Add(left, right);
///
public static Quaternion Subtract(this Quaternion left, Quaternion right) => Quaternion.Subtract(left, right);
///
public static Quaternion Multiply(this Quaternion quaternion, float value) => Quaternion.Multiply(quaternion, value);
///
public static Quaternion Divide(this Quaternion quaternion, float value) => Quaternion.Divide(quaternion, value);
///
public static Quaternion Normalize(this Quaternion quaternion) => Quaternion.Normalize(quaternion);
///
public static Quaternion Rotate(this Quaternion vector, Vector3D normal, float angleInRadian) => Quaternion.Rotate(vector, normal, angleInRadian);
///
public static Quaternion Invert(this Quaternion quaternion) => Quaternion.Invert(quaternion);
///
public static Quaternion Conjugate(this Quaternion quaternion) => Quaternion.Conjugate(quaternion);
///
public static Vector3D RotateVector(this Vector3D vector, Quaternion quaternion) => Quaternion.RotateVector(vector, quaternion);
///
public static Quaternion SLerp(this Quaternion from, Quaternion to, float t) => Quaternion.SLerp(from, to, t);
///
public static Quaternion Lerp(this Quaternion from, Quaternion to, float t) => Quaternion.Lerp(from, to, t);
///
public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right);
///
public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion);
///
public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);
///
public static bool ApproximatelyEquals(this Quaternion left, Quaternion right, float epsilon = float.Epsilon) => Quaternion.ApproximatelyEquals(left, right, epsilon);
}