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; } + /// + 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; + } + /// public static Vector2 FloorCopy(this Vector2 vec) { return new Vector2(vec.X.Floor(), vec.Y.Floor()); @@ -213,7 +218,7 @@ namespace MLEM.Extensions { /// /// Returns the rotation that the given matrix represents, as a . - /// Returns if the matrix does not contain valid rotation information. + /// Returns if the matrix does not contain valid rotation information, or is not rotated. /// /// The matrix /// The rotation of the matrix @@ -228,6 +233,30 @@ namespace MLEM.Extensions { 0, 0, 0, 1)); } + /// + /// Returns the rotation that the given matrix represents, as a that contains the x, y and z rotations in radians. + /// Returns if the matrix does not contain valid rotation information, or is not rotated. + /// + /// The matrix + /// The rotation of the matrix + public static Vector3 RotationVector(this Matrix matrix) { + return matrix.Rotation().RotationVector(); + } + + /// + /// Returns the rotation that the given quaternion represents, as a that contains the x, y and z rotations in radians. + /// Returns if the quaternion does not contain valid rotation information, or is not rotated. + /// + /// The quaternion + /// The rotation of the quaternion + 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))); + } + /// /// Calculates the amount that the rectangle is penetrating the rectangle 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]