feat: quaternion to matrix4x4 methods

Engine.Core/Primitives/Quaternion.cs

@@ -217,6 +217,63 @@ public readonly struct Quaternion(float x, float y, float z, float w)
         return new Quaternion(axis.X * sinHalf, axis.Y * sinHalf, axis.Z * sinHalf, MathF.Cos(halfAngle));
     }
 
+    /// <summary>
+    /// Calculates the <see cref="Quaternion"/> from given yaw, pitch and roll values.
+    /// </summary>
+    /// <returns>The rotation <see cref="Quaternion"/> calculated by the given parameters.</returns>
+    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
+        );
+    }
+
+    /// <summary>
+    /// Calculates the <see cref="System.Numerics.Matrix4x4"/> from given <see cref="Quaternion"/>.
+    /// </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="System.Numerics.Matrix4x4"/> calculated by the given <see cref="Quaternion"/>.</returns>
+    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
+        );
+    }
+
     /// <summary>
     /// Checks if two <see cref="Quaternion"/>s are approximately equal within a specified epsilon range.
     /// </summary>
@@ -291,6 +348,9 @@ public static class QuaternionExtensions
     /// <inheritdoc cref="Quaternion.Dot(Quaternion, Quaternion)" />
     public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right);
 
+    /// <inheritdoc cref="Quaternion.ToRotationMatrix4x4(Quaternion, Quaternion)" />
+    public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion);
+
     /// <inheritdoc cref="Quaternion.FromAxisAngle(Vector3D, float)" />
     public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);