diff --git a/X10D.Tests/src/Numerics/QuaternionTests.cs b/X10D.Tests/src/Numerics/QuaternionTests.cs index c55ce9f..22e4fec 100644 --- a/X10D.Tests/src/Numerics/QuaternionTests.cs +++ b/X10D.Tests/src/Numerics/QuaternionTests.cs @@ -18,4 +18,22 @@ public class QuaternionTests Assert.AreEqual(axis, axisAngle.Axis); Assert.AreEqual(angle, axisAngle.Angle); } + + [TestMethod] + public void ToVector3_ShouldReturnZeroVector_GivenIdentityQuaternion() + { + Assert.AreEqual(Vector3.Zero, Quaternion.Identity.ToVector3()); + } + + [TestMethod] + public void ToVector3_ShouldReturnVector_0_PI_0_GivenQuaternionCreatedFrom_PI_0_0() + { + Quaternion quaternion = Quaternion.CreateFromYawPitchRoll(MathF.PI, 0, 0); + var expected = new Vector3(0, MathF.PI, 0); + var actual = quaternion.ToVector3(); + + Assert.AreEqual(expected.X, actual.X, 1e-5f); + Assert.AreEqual(expected.Y, actual.Y, 1e-5f); + Assert.AreEqual(expected.Z, actual.Z, 1e-5f); + } } diff --git a/X10D/src/Numerics/QuaternionExtensions.cs b/X10D/src/Numerics/QuaternionExtensions.cs index 2ad4fe4..ece66d1 100644 --- a/X10D/src/Numerics/QuaternionExtensions.cs +++ b/X10D/src/Numerics/QuaternionExtensions.cs @@ -64,9 +64,9 @@ public static class QuaternionExtensions 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); + float x = MathF.Asin(-2 * (qx * qy - qz * qw)); + float y = MathF.Atan2(2 * (qy * qw + qx * qz), 1 - 2 * (qy * qy + qx * qx)); + float z = MathF.Atan2(2 * (qx * qw + qy * qz), 1 - 2 * (qz * qz + qw * qw)); + return new Vector3(-x, -y, -z); } }