From 5c8ef3d254b930bd933f15c92c59d5a3a0208e55 Mon Sep 17 00:00:00 2001
From: Ellpeck <me@ellpeck.de>
Date: Mon, 22 Nov 2021 14:48:38 +0100
Subject: [PATCH] Added RotationVector extension methods for Matrix and
 Quaternion

---
 CHANGELOG.md                        |  1 +
 MLEM/Extensions/NumberExtensions.cs | 31 ++++++++++++++++++++++++++++-
 Tests/NumberTests.cs                | 13 +++++++++---
 3 files changed, 41 insertions(+), 4 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9037974..3c40423 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -13,6 +13,7 @@ Additions
 - Added GenericFont SplitStringSeparate which differentiates between existing newline characters and splits due to maximum width
 - Added StaticSpriteBatch class
 - Added missing easing functions Quart and Quint to Easings
+- Added RotationVector extension methods for Matrix and Quaternion
 
 Improvements
 - Cache TokenizedString inner offsets for non-Left text alignments to improve performance
diff --git a/MLEM/Extensions/NumberExtensions.cs b/MLEM/Extensions/NumberExtensions.cs
index 3b66648..da27da2 100644
--- a/MLEM/Extensions/NumberExtensions.cs
+++ b/MLEM/Extensions/NumberExtensions.cs
@@ -45,6 +45,11 @@ namespace MLEM.Extensions {
             return Math.Abs(first.X - second.X) <= tolerance && Math.Abs(first.Y - second.Y) <= tolerance && Math.Abs(first.Z - second.Z) <= tolerance && Math.Abs(first.W - second.W) <= tolerance;
         }
 
+        /// <inheritdoc cref="Equals(float,float,float)"/>
+        public static bool Equals(this Quaternion first, Quaternion second, float tolerance) {
+            return Math.Abs(first.X - second.X) <= tolerance && Math.Abs(first.Y - second.Y) <= tolerance && Math.Abs(first.Z - second.Z) <= tolerance && Math.Abs(first.W - second.W) <= tolerance;
+        }
+
         /// <inheritdoc cref="Math.Floor(decimal)"/>
         public static Vector2 FloorCopy(this Vector2 vec) {
             return new Vector2(vec.X.Floor(), vec.Y.Floor());
@@ -213,7 +218,7 @@ namespace MLEM.Extensions {
 
         /// <summary>
         /// Returns the rotation that the given matrix represents, as a <see cref="Quaternion"/>.
-        /// Returns <see cref="Quaternion.Identity"/> if the matrix does not contain valid rotation information.
+        /// Returns <see cref="Quaternion.Identity"/> if the matrix does not contain valid rotation information, or is not rotated.
         /// </summary>
         /// <param name="matrix">The matrix</param>
         /// <returns>The rotation of the matrix</returns>
@@ -228,6 +233,30 @@ namespace MLEM.Extensions {
                 0, 0, 0, 1));
         }
 
+        /// <summary>
+        /// Returns the rotation that the given matrix represents, as a <see cref="Vector3"/> that contains the x, y and z rotations in radians.
+        /// Returns <see cref="Vector3.Zero"/> if the matrix does not contain valid rotation information, or is not rotated.
+        /// </summary>
+        /// <param name="matrix">The matrix</param>
+        /// <returns>The rotation of the matrix</returns>
+        public static Vector3 RotationVector(this Matrix matrix) {
+            return matrix.Rotation().RotationVector();
+        }
+
+        /// <summary>
+        /// Returns the rotation that the given quaternion represents, as a <see cref="Vector3"/> that contains the x, y and z rotations in radians.
+        /// Returns <see cref="Vector3.Zero"/> if the quaternion does not contain valid rotation information, or is not rotated.
+        /// </summary>
+        /// <param name="quaternion">The quaternion</param>
+        /// <returns>The rotation of the quaternion</returns>
+        public static Vector3 RotationVector(this Quaternion quaternion) {
+            var (x, y, z, w) = quaternion;
+            return new Vector3(
+                (float) Math.Atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
+                (float) Math.Asin(MathHelper.Clamp(2 * (w * y - z * x), -1, 1)),
+                (float) Math.Atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));
+        }
+
         /// <summary>
         /// Calculates the amount that the rectangle <paramref name="rect"/> is penetrating the rectangle <paramref name="other"/> by.
         /// If a penetration on both axes is occuring, the one with the lower value is returned.
diff --git a/Tests/NumberTests.cs b/Tests/NumberTests.cs
index 5eb1f44..8da134d 100644
--- a/Tests/NumberTests.cs
+++ b/Tests/NumberTests.cs
@@ -43,10 +43,17 @@ namespace Tests {
         }
 
         [Test]
-        public void TestMatrixOps([Range(0.5F, 2, 0.5F)] float scale, [Range(-1, 1, 1F)] float rotationX) {
-            var matrix = Matrix.CreateRotationX(rotationX) * Matrix.CreateScale(scale, scale, scale);
+        public void TestMatrixOps([Range(0.5F, 2, 0.5F)] float scale, [Range(-1, 1, 0.5F)] float rotationX, [Range(-1, 1, 0.5F)] float rotationY, [Range(-1, 1, 0.5F)] float rotationZ) {
+            var rotation = Matrix.CreateRotationX(rotationX) * Matrix.CreateRotationY(rotationY) * Matrix.CreateRotationZ(rotationZ);
+            var matrix = rotation * Matrix.CreateScale(scale, scale, scale);
             Assert.IsTrue(matrix.Scale().Equals(new Vector3(scale), 0.001F), $"{matrix.Scale()} does not equal {new Vector2(scale)}");
-            Assert.AreEqual(matrix.Rotation(), Quaternion.CreateFromAxisAngle(Vector3.UnitX, rotationX));
+            Assert.IsTrue(matrix.Rotation().Equals(Quaternion.CreateFromRotationMatrix(rotation), 0.001F), $"{matrix.Rotation()} does not equal {Quaternion.CreateFromRotationMatrix(rotation)}");
+            Assert.IsTrue(matrix.RotationVector().Equals(new Vector3(rotationX, rotationY, rotationZ), 0.001F), $"{matrix.RotationVector()} does not equal {new Vector3(rotationX, rotationY, rotationZ)}");
+
+            // check against decomposed results
+            matrix.Decompose(out var sc, out var rot, out _);
+            Assert.AreEqual(matrix.Rotation(), rot);
+            Assert.AreEqual(matrix.Scale(), sc);
         }
 
         [Test]