From 9d6dbaaa23bbf71a3ed0c1ed0a192ffbcd407122 Mon Sep 17 00:00:00 2001
From: Oliver Booth <me@olivr.me>
Date: Tue, 6 Dec 2022 00:58:15 +0000
Subject: [PATCH] Add Quaternion.ToAxisAngle and Quaternion.ToVector3

---
 CHANGELOG.md                               |  2 ++
 X10D.Tests/src/Numerics/QuaternionTests.cs | 21 +++++++++++++++
 X10D/src/Numerics/QuaternionExtensions.cs  | 31 ++++++++++++++++++++++
 3 files changed, 54 insertions(+)
 create mode 100644 X10D.Tests/src/Numerics/QuaternionTests.cs

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e4354cf..d259b43 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -31,6 +31,8 @@
 - X10D: Added `PointF.ToSizeF()`
 - X10D: Added `PointF.ToVector2()` for .NET < 6
 - X10D: Added `PopCount()` for built-in integer types
+- X10D: Added `Quaternion.ToAxisAngle(out float, out float)`
+- X10D: Added `Quaternion.ToVector3()`
 - X10D: Added `ReadOnlySpan<char>.CountSubstring(char)`
 - X10D: Added `ReadOnlySpan<char>.CountSubstring(ReadOnlySpan<char>[, StringComparison])`
 - X10D: Added `ReadOnlySpan<T>.Split(T)`
diff --git a/X10D.Tests/src/Numerics/QuaternionTests.cs b/X10D.Tests/src/Numerics/QuaternionTests.cs
new file mode 100644
index 0000000..c55ce9f
--- /dev/null
+++ b/X10D.Tests/src/Numerics/QuaternionTests.cs
@@ -0,0 +1,21 @@
+using System.Numerics;
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using X10D.Numerics;
+
+namespace X10D.Tests.Numerics;
+
+[TestClass]
+public class QuaternionTests
+{
+    [TestMethod]
+    public void ToAxisAngle_ShouldGiveAngle180VectorZero_GivenQuaternion()
+    {
+        Vector3 axis = Vector3.UnitY;
+        const float angle = MathF.PI;
+        var quaternion = Quaternion.CreateFromAxisAngle(axis, angle);
+
+        (Vector3 Axis, float Angle) axisAngle = quaternion.ToAxisAngle();
+        Assert.AreEqual(axis, axisAngle.Axis);
+        Assert.AreEqual(angle, axisAngle.Angle);
+    }
+}
diff --git a/X10D/src/Numerics/QuaternionExtensions.cs b/X10D/src/Numerics/QuaternionExtensions.cs
index a8e8b5b..89d576e 100644
--- a/X10D/src/Numerics/QuaternionExtensions.cs
+++ b/X10D/src/Numerics/QuaternionExtensions.cs
@@ -38,4 +38,35 @@ public static class QuaternionExtensions
             (xz - wy) * px + (yz + wx) * py + (1.0f - (xx + yy)) * pz
         );
     }
+
+    /// <summary>
+    ///     Converts this quaternion to an axis/angle pair.
+    /// </summary>
+    /// <param name="value">The quaternion to convert.</param>
+    /// <returns>A tuple containing the converted axis, and the angle in radians.</returns>
+    public static (Vector3 Axis, float Angle) ToAxisAngle(this in Quaternion value)
+    {
+        float angle = 2.0f * MathF.Acos(value.W);
+        Vector3 axis = Vector3.Normalize(new Vector3(value.X, value.Y, value.Z));
+        return (axis, angle);
+    }
+
+    /// <summary>
+    ///     Converts this quaternion to a <see cref="Vector3" /> containing an Euler representation of the rotation. 
+    /// </summary>
+    /// <param name="value">The quaternion to convert.</param>
+    /// <returns>The Euler representation of <paramref name="value" />, in radians.</returns>
+    public static Vector3 ToVector3(this in Quaternion value)
+    {
+        Quaternion normalized = Quaternion.Normalize(value);
+        float qx = normalized.X;
+        float qy = normalized.Y;
+        float qz = normalized.Z;
+        float qw = normalized.W;
+
+        float x = MathF.Atan2(2 * (qx * qw - qy * qz), 1 - 2 * (qx * qx + qz * qz));
+        float y = MathF.Asin(2 * (qx * qz + qy * qw));
+        float z = MathF.Atan2(2 * (qz * qw - qx * qy), 1 - 2 * (qy * qy + qz * qz));
+        return new Vector3(x, y, z);
+    }
 }