diff --git a/Engine.Core/Primitives/Quaternion.cs b/Engine.Core/Primitives/Quaternion.cs new file mode 100644 index 0000000..383a6bf --- /dev/null +++ b/Engine.Core/Primitives/Quaternion.cs @@ -0,0 +1,372 @@ +using System; + +namespace Syntriax.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) +{ + /// + /// 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); + + /// + /// 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); + + /// + /// 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) + { + Quaternion rotation = quaternion * new Quaternion(vector.X, vector.Y, vector.Z, 0) * Invert(quaternion); + return new(rotation.X, rotation.Y, rotation.Z); + } + + /// + /// 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 = MathF.Acos(dot); + float sinAngle = MathF.Sin(angle); + + float fromWeight = MathF.Sin((1f - t) * angle) / sinAngle; + float toWeight = MathF.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 angle) + { + float halfAngle = angle * .5f; + float sinHalf = MathF.Sin(halfAngle); + return new Quaternion(axis.X * sinHalf, axis.Y * sinHalf, axis.Z * sinHalf, MathF.Cos(halfAngle)); + } + + /// + /// 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); + + /// + /// Converts the to its string representation. + /// + /// A string representation of the . + public override string ToString() => $"{nameof(Quaternion)}({W}, {X}, {Y}, {Z})"; + + /// + /// 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 objVec && X.Equals(objVec.X) && Y.Equals(objVec.Y) && Z.Equals(objVec.Z) && W.Equals(objVec.W); + + /// + /// Generates a hash code for the . + /// + /// A hash code for the . + public override int GetHashCode() => HashCode.Combine(X, Y, Z); +} + +/// +/// Provides extension methods for type. +/// +public static class QuaternionExtensions +{ + /// + /// Calculates the length of the . + /// + /// The . + /// The length of the . + public static float Length(this Quaternion quaternion) => Quaternion.Length(quaternion); + + /// + /// Calculates the squared length of the . + /// + /// The . + /// The squared length of the . + public static float LengthSquared(this Quaternion quaternion) => Quaternion.LengthSquared(quaternion); + + /// + /// Adds two s. + /// + /// The first . + /// The second . + /// The sum of the two s. + public static Quaternion Add(this Quaternion left, Quaternion right) => Quaternion.Add(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(this Quaternion left, Quaternion right) => Quaternion.Subtract(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(this Quaternion quaternion, float value) => Quaternion.Multiply(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(this Quaternion quaternion, float value) => Quaternion.Divide(quaternion, value); + + /// + /// Normalizes the (creates a unit with the same direction). + /// + /// The to normalize. + /// The normalized . + public static Quaternion Normalize(this Quaternion quaternion) => Quaternion.Normalize(quaternion); + + /// + /// Inverts the direction of the . + /// + /// The . + /// The inverted . + public static Quaternion Invert(this Quaternion quaternion) => Quaternion.Invert(quaternion); + + /// + /// Conjugate of the . + /// + /// The . + /// The inverted . + public static Quaternion Conjugate(this Quaternion quaternion) => Quaternion.Conjugate(quaternion); + + /// + /// Rotates a by applying the provided . + /// + /// The to be rotated. + /// The to used for applying rotation. + /// The rotated . + public static Vector3D RotateVector(this Vector3D vector, Quaternion quaternion) => Quaternion.RotateVector(vector, quaternion); + + /// + /// Performs spherical linear interpolation between two s. + /// + /// The starting (t = 0). + /// The target (t = 1). + /// The interpolation parameter. + /// The interpolated . + public static Quaternion SLerp(this Quaternion from, Quaternion to, float t) => Quaternion.SLerp(from, to, t); + + /// + /// Performs linear interpolation between two s. + /// + /// The starting (t = 0). + /// The target (t = 1). + /// The interpolation parameter. + /// The interpolated . + public static Quaternion Lerp(this Quaternion from, Quaternion to, float t) => Quaternion.Lerp(from, to, t); + + /// + /// Calculates the dot product of two s. + /// + /// The first . + /// The second . + /// The dot product of the two s. + public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right); + + /// + /// 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 CreateRotationFromAxis(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle); + + /// + /// 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(this Quaternion left, Quaternion right, float epsilon = float.Epsilon) => Quaternion.ApproximatelyEquals(left, right, epsilon); +}