300 lines
16 KiB
C#
300 lines
16 KiB
C#
using System;
|
|
|
|
namespace Syntriax.Engine.Core;
|
|
|
|
/// <summary>
|
|
/// Represents a 3D space rotation.
|
|
/// </summary>
|
|
/// <param name="x">X(i) position of the <see cref="Quaternion"/>.</param>
|
|
/// <param name="y">Y(j) position of the <see cref="Quaternion"/>.</param>
|
|
/// <param name="z">Z(k) position of the <see cref="Quaternion"/>.</param>
|
|
/// <param name="w">W(a) position of the <see cref="Quaternion"/>.</param>
|
|
/// <remarks>
|
|
/// Initializes a new instance of the <see cref="Quaternion"/> struct with the specified positions.
|
|
/// </remarks>
|
|
[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)
|
|
{
|
|
/// <summary>
|
|
/// The X(i) imaginary of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public readonly float X = x;
|
|
|
|
/// <summary>
|
|
/// The Y(j) imaginary of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public readonly float Y = y;
|
|
|
|
/// <summary>
|
|
/// The Z(k) imaginary of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public readonly float Z = z;
|
|
|
|
/// <summary>
|
|
/// The W(a) scalar of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public readonly float W = w;
|
|
|
|
/// <summary>
|
|
/// The magnitude (length) of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public float Magnitude => Length(this);
|
|
|
|
/// <summary>
|
|
/// The squared magnitude (length) of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
public float MagnitudeSquared => LengthSquared(this);
|
|
|
|
/// <summary>
|
|
/// The normalized form of the <see cref="Quaternion"/> (a <see cref="Quaternion"/> with the same direction and a magnitude of 1).
|
|
/// </summary>
|
|
public Quaternion Normalized => Normalize(this);
|
|
|
|
/// <summary>
|
|
/// Represents the <see cref="Quaternion"/> with no rotation.
|
|
/// </summary>
|
|
public readonly static Quaternion Zero = new(0f, 0f, 0f, 0f);
|
|
|
|
/// <summary>
|
|
/// Represents the identity <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
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);
|
|
|
|
/// <summary>
|
|
/// Calculates the length of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <returns>The length of the <see cref="Quaternion"/>.</returns>
|
|
public static float Length(Quaternion quaternion) => Math.Sqrt(LengthSquared(quaternion));
|
|
|
|
/// <summary>
|
|
/// Calculates the squared length of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <returns>The squared length of the <see cref="Quaternion"/>.</returns>
|
|
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;
|
|
|
|
/// <summary>
|
|
/// Adds two <see cref="Quaternion"/>s.
|
|
/// </summary>
|
|
/// <param name="left">The first <see cref="Quaternion"/>.</param>
|
|
/// <param name="right">The second <see cref="Quaternion"/>.</param>
|
|
/// <returns>The sum of the two <see cref="Quaternion"/>s.</returns>
|
|
public static Quaternion Add(Quaternion left, Quaternion right) => left + right;
|
|
|
|
/// <summary>
|
|
/// Subtracts one <see cref="Quaternion"/> from another.
|
|
/// </summary>
|
|
/// <param name="left">The <see cref="Quaternion"/> to subtract from.</param>
|
|
/// <param name="right">The <see cref="Quaternion"/> to subtract.</param>
|
|
/// <returns>The result of subtracting the second <see cref="Quaternion"/> from the first.</returns>
|
|
public static Quaternion Subtract(Quaternion left, Quaternion right) => left - right;
|
|
|
|
/// <summary>
|
|
/// Multiplies a <see cref="Quaternion"/> by a scalar value.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <param name="value">The scalar value.</param>
|
|
/// <returns>The result of multiplying the <see cref="Quaternion"/> by the scalar value.</returns>
|
|
public static Quaternion Multiply(Quaternion quaternion, float value) => quaternion * value;
|
|
|
|
/// <summary>
|
|
/// Divides a <see cref="Quaternion"/> by a scalar value.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <param name="value">The scalar value.</param>
|
|
/// <returns>The result of dividing the <see cref="Quaternion"/> by the scalar value.</returns>
|
|
public static Quaternion Divide(Quaternion quaternion, float value) => quaternion / value;
|
|
|
|
/// <summary>
|
|
/// Normalizes the <see cref="Quaternion"/> (creates a unit <see cref="Quaternion"/> with the same direction).
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/> to normalize.</param>
|
|
/// <returns>The normalized <see cref="Quaternion"/>.</returns>
|
|
public static Quaternion Normalize(Quaternion quaternion) => quaternion / Length(quaternion);
|
|
|
|
/// <summary>
|
|
/// Inverts the direction of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <returns>The inverted <see cref="Quaternion"/>.</returns>
|
|
public static Quaternion Invert(Quaternion quaternion) => Conjugate(quaternion) / LengthSquared(quaternion);
|
|
|
|
/// <summary>
|
|
/// Conjugate of the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/>.</param>
|
|
/// <returns>The inverted <see cref="Quaternion"/>.</returns>
|
|
public static Quaternion Conjugate(Quaternion quaternion) => -quaternion;
|
|
|
|
/// <summary>
|
|
/// Rotates a <see cref="Vector3D"/> by applying the provided <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="vector">The <see cref="Vector3D"/> to be rotated.</param>
|
|
/// <param name="quaternion">The <see cref="Quaternion"/> to used for applying rotation.</param>
|
|
/// <returns>The rotated <see cref="Vector3D"/>.</returns>
|
|
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);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs spherical linear interpolation between two <see cref="Quaternion"/>s.
|
|
/// </summary>
|
|
/// <param name="from">The starting <see cref="Quaternion"/> (t = 0).</param>
|
|
/// <param name="to">The target <see cref="Quaternion"/> (t = 1).</param>
|
|
/// <param name="t">The interpolation parameter.</param>
|
|
/// <returns>The interpolated <see cref="Quaternion"/>.</returns>
|
|
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;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs linear interpolation between two <see cref="Quaternion"/>s.
|
|
/// </summary>
|
|
/// <param name="from">The starting <see cref="Quaternion"/> (t = 0).</param>
|
|
/// <param name="to">The target <see cref="Quaternion"/> (t = 1).</param>
|
|
/// <param name="t">The interpolation parameter.</param>
|
|
/// <returns>The interpolated <see cref="Quaternion"/>.</returns>
|
|
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)));
|
|
|
|
/// <summary>
|
|
/// Calculates the dot product of two <see cref="Quaternion"/>s.
|
|
/// </summary>
|
|
/// <param name="left">The first <see cref="Quaternion"/>.</param>
|
|
/// <param name="right">The second <see cref="Quaternion"/>.</param>
|
|
/// <returns>The dot product of the two <see cref="Quaternion"/>s.</returns>
|
|
public static float Dot(Quaternion left, Quaternion right) => left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W;
|
|
|
|
/// <summary>
|
|
/// Calculates the <see cref="Quaternion"/> from given axis and angle.
|
|
/// </summary>
|
|
/// <param name="axis">The axis of the rotation in <see cref="Vector3D"/>.</param>
|
|
/// <param name="angle">The angle in radians.</param>
|
|
/// <returns>The rotation <see cref="Quaternion"/> calculated by the given parameters.</returns>
|
|
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));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Checks if two <see cref="Quaternion"/>s are approximately equal within a specified epsilon range.
|
|
/// </summary>
|
|
/// <param name="left">The first <see cref="Quaternion"/>.</param>
|
|
/// <param name="right">The second <see cref="Quaternion"/>.</param>
|
|
/// <param name="epsilon">The epsilon range.</param>
|
|
/// <returns><see cref="true"/> if the <see cref="Quaternion"/>s are approximately equal; otherwise, <see cref="false"/>.</returns>
|
|
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);
|
|
|
|
/// <summary>
|
|
/// Converts the <see cref="Quaternion"/> to its string representation.
|
|
/// </summary>
|
|
/// <returns>A string representation of the <see cref="Quaternion"/>.</returns>
|
|
public override string ToString() => $"{nameof(Quaternion)}({W}, {X}, {Y}, {Z})";
|
|
|
|
/// <summary>
|
|
/// Determines whether the specified object is equal to the current <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="obj">The object to compare with the current <see cref="Quaternion"/>.</param>
|
|
/// <returns><see cref="true"/> if the specified object is equal to the current <see cref="Quaternion"/>; otherwise, <see cref="false"/>.</returns>
|
|
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);
|
|
|
|
/// <summary>
|
|
/// Generates a hash code for the <see cref="Quaternion"/>.
|
|
/// </summary>
|
|
/// <returns>A hash code for the <see cref="Quaternion"/>.</returns>
|
|
public override int GetHashCode() => HashCode.Combine(X, Y, Z);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Provides extension methods for <see cref="Quaternion"/> type.
|
|
/// </summary>
|
|
public static class QuaternionExtensions
|
|
{
|
|
/// <inheritdoc cref="Quaternion.Length(Quaternion)" />
|
|
public static float Length(this Quaternion quaternion) => Quaternion.Length(quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.LengthSquared(Quaternion)" />
|
|
public static float LengthSquared(this Quaternion quaternion) => Quaternion.LengthSquared(quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.Add(Quaternion, Quaternion)" />
|
|
public static Quaternion Add(this Quaternion left, Quaternion right) => Quaternion.Add(left, right);
|
|
|
|
/// <inheritdoc cref="Quaternion.Subtract(Quaternion, Quaternion)" />
|
|
public static Quaternion Subtract(this Quaternion left, Quaternion right) => Quaternion.Subtract(left, right);
|
|
|
|
/// <inheritdoc cref="Quaternion.Multiply(Quaternion, float)" />
|
|
public static Quaternion Multiply(this Quaternion quaternion, float value) => Quaternion.Multiply(quaternion, value);
|
|
|
|
/// <inheritdoc cref="Quaternion.Divide(Quaternion, float)" />
|
|
public static Quaternion Divide(this Quaternion quaternion, float value) => Quaternion.Divide(quaternion, value);
|
|
|
|
/// <inheritdoc cref="Quaternion.Normalize(Quaternion)" />
|
|
public static Quaternion Normalize(this Quaternion quaternion) => Quaternion.Normalize(quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.Invert(Quaternion)" />
|
|
public static Quaternion Invert(this Quaternion quaternion) => Quaternion.Invert(quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.Conjugate(Quaternion)" />
|
|
public static Quaternion Conjugate(this Quaternion quaternion) => Quaternion.Conjugate(quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.RotateVector(Vector3D, Quaternion)" />
|
|
public static Vector3D RotateVector(this Vector3D vector, Quaternion quaternion) => Quaternion.RotateVector(vector, quaternion);
|
|
|
|
/// <inheritdoc cref="Quaternion.SLerp(Quaternion, Quaternion, float)" />
|
|
public static Quaternion SLerp(this Quaternion from, Quaternion to, float t) => Quaternion.SLerp(from, to, t);
|
|
|
|
/// <inheritdoc cref="Quaternion.Lerp(Quaternion, Quaternion, float)" />
|
|
public static Quaternion Lerp(this Quaternion from, Quaternion to, float t) => Quaternion.Lerp(from, to, t);
|
|
|
|
/// <inheritdoc cref="Quaternion.Dot(Quaternion, Quaternion)" />
|
|
public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right);
|
|
|
|
/// <inheritdoc cref="Quaternion.FromAxisAngle(Vector3D, float)" />
|
|
public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);
|
|
|
|
/// <inheritdoc cref="Quaternion.ApproximatelyEquals(Quaternion, Quaternion, float)" />
|
|
public static bool ApproximatelyEquals(this Quaternion left, Quaternion right, float epsilon = float.Epsilon) => Quaternion.ApproximatelyEquals(left, right, epsilon);
|
|
}
|