From eeaca3a6c7c68c13cce4df817f45778f633d544c Mon Sep 17 00:00:00 2001
From: Syntriax <syntriax@gmail.com>
Date: Sun, 19 Oct 2025 00:13:59 +0300
Subject: [PATCH] feat: quaternion to angles conversion added

---
 Engine.Core/Primitives/Quaternion.cs | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/Engine.Core/Primitives/Quaternion.cs b/Engine.Core/Primitives/Quaternion.cs
index 0a4b957..bdf48cb 100644
--- a/Engine.Core/Primitives/Quaternion.cs
+++ b/Engine.Core/Primitives/Quaternion.cs
@@ -79,6 +79,30 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab
     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>
+    /// Get the Pitch, Yaw and Roll of the <see cref="Quaternion"/>.
+    /// </summary>
+    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;
+    }
+
     /// <summary>
     /// Calculates the length of the <see cref="Quaternion"/>.
     /// </summary>
@@ -310,6 +334,9 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab
 /// </summary>
 public static class QuaternionExtensions
 {
+    /// <inheritdoc cref="Quaternion.ToAngles(Quaternion)" />
+    public static Vector3D ToAngles(this Quaternion quaternion) => Quaternion.ToAngles(quaternion);
+
     /// <inheritdoc cref="Quaternion.Length(Quaternion)" />
     public static float Length(this Quaternion quaternion) => Quaternion.Length(quaternion);