diff --git a/VpSharp/src/Entities/VirtualParadiseAvatar.cs b/VpSharp/src/Entities/VirtualParadiseAvatar.cs index 66852b9..f5cb4f0 100644 --- a/VpSharp/src/Entities/VirtualParadiseAvatar.cs +++ b/VpSharp/src/Entities/VirtualParadiseAvatar.cs @@ -384,7 +384,7 @@ public sealed class VirtualParadiseAvatar : IEquatable lock (_client.Lock) { (double x, double y, double z) = position; - (double pitch, double yaw, double _) = rotation.ToEulerAngles(false); + (double pitch, double yaw, double _) = rotation.ToEulerAngles(); _ = vp_double_set(handle, FloatAttribute.MyX, x); _ = vp_double_set(handle, FloatAttribute.MyY, y); @@ -404,7 +404,7 @@ public sealed class VirtualParadiseAvatar : IEquatable lock (_client.Lock) { (float x, float y, float z) = (Vector3)position; - (float pitch, float yaw, float _) = (Vector3)rotation.ToEulerAngles(false); + (float pitch, float yaw, float _) = (Vector3)rotation.ToEulerAngles(); var reason = (ReasonCode)vp_teleport_avatar(handle, Session, world, x, y, z, yaw, pitch); if (reason == ReasonCode.NotInWorld) diff --git a/VpSharp/src/Extensions/QuaternionExtensions.cs b/VpSharp/src/Extensions/QuaternionExtensions.cs index d7bc39b..63fd1fd 100644 --- a/VpSharp/src/Extensions/QuaternionExtensions.cs +++ b/VpSharp/src/Extensions/QuaternionExtensions.cs @@ -11,70 +11,31 @@ public static class QuaternionExtensions /// Converts this quaternion to a containing an Euler representation of the rotation. /// /// The quaternion to convert. - /// - /// if the resulting vector should be in radians; or if the resulting - /// vector should be in degrees. - /// /// The Euler representation of . /// - public static Vector3d ToEulerAngles(this Quaternion value, bool radians = true) + public static Vector3d ToEulerAngles(this Quaternion value) { - double a = (2.0 * value.Y * value.W) - (2.0 * value.X * value.Z); - double b = 1.0 - (2.0 * value.Y * value.Y) - (2.0 * value.Z * value.Z); - double y = -Math.Atan2(a, b); - - a = 2.0 * value.X * value.Y; - b = 2.0 * value.Z * value.W; - double z = Math.Asin(a + b); - - a = (2.0 * value.X * value.W) - (2.0 * value.Y * value.Z); - b = 1.0 - (2.0 * value.X * value.X) - (2.0 * value.Z * value.Z); - double x = Math.Atan2(a, b); - - if (!radians) - { - x = 180.0 / Math.PI * x; - y = 180.0 / Math.PI * y; - z = 180.0 / Math.PI * z; - } - - return new Vector3d(x, y, z); + value = Quaternion.Normalize(value); + double x = Math.Atan2(2 * (value.X * value.W - value.Y * value.Z), 1 - 2 * (value.X * value.X + value.Z * value.Z)); + double y = Math.Asin(2 * (value.X * value.Z + value.Y * value.W)); + double z = Math.Atan2(2 * (value.Z * value.W - value.X * value.Y), 1 - 2 * (value.Y * value.Y + value.Z * value.Z)); + return new Vector3d(x, y, z) * (180 / Math.PI); } /// /// Converts this quaternion to a containing an Euler representation of the rotation. /// /// The quaternion to convert. - /// - /// if the resulting vector should be in radians; or if the resulting - /// vector should be in degrees. - /// /// The Euler representation of . - public static Vector3d ToEulerAnglesF(this Quaternion value, bool radians = true) + public static Vector3 ToEulerAnglesF(this Quaternion value) { - float a = (2.0f * value.Y * value.W) - (2.0f * value.X * value.Z); - float b = 1.0f - (2.0f * value.Y * value.Y) - (2.0f * value.Z * value.Z); - float y = -MathF.Atan2(a, b); - - a = 2.0f * value.X * value.Y; - b = 2.0f * value.Z * value.W; - float z = MathF.Asin(a + b); - - a = (2.0f * value.X * value.W) - (2.0f * value.Y * value.Z); - b = 1.0f - (2.0f * value.X * value.X) - (2.0f * value.Z * value.Z); - float x = MathF.Atan2(a, b); - - if (!radians) - { - x = 180.0f / MathF.PI * x; - y = 180.0f / MathF.PI * y; - z = 180.0f / MathF.PI * z; - } - - return new Vector3d(x, y, z); + value = Quaternion.Normalize(value); + float x = MathF.Atan2(2 * (value.X * value.W - value.Y * value.Z), 1 - 2 * (value.X * value.X + value.Z * value.Z)); + float y = MathF.Asin(2 * (value.X * value.Z + value.Y * value.W)); + float z = MathF.Atan2(2 * (value.Z * value.W - value.X * value.Y), 1 - 2 * (value.Y * value.Y + value.Z * value.Z)); + return new Vector3(x, y, z) * (180 / MathF.PI); } - // #pragma warning disable CA1021 /// /// Converts this quaternion to an axis/angle pair. @@ -85,13 +46,8 @@ public static class QuaternionExtensions /// public static void ToAxisAngle(this Quaternion value, out Vector3 axis, out float angle) { - angle = 2.0f * MathF.Acos(value.W); - - float x = value.X / MathF.Sqrt(1.0f - (value.W * value.W)); - float y = value.Y / MathF.Sqrt(1.0f - (value.W * value.W)); - float z = value.Z / MathF.Sqrt(1.0f - (value.W * value.W)); - - axis = new Vector3(x, y, z); + angle = 2 * MathF.Acos(value.W); + axis = Vector3.Normalize(new Vector3(value.X, value.Y, value.Z)); } /// @@ -102,13 +58,8 @@ public static class QuaternionExtensions /// The angle value. public static void ToAxisAngle(this Quaternion value, out Vector3d axis, out double angle) { - angle = 2.0 * Math.Acos(value.W); - - double x = value.X / Math.Sqrt(1.0 - (value.W * value.W)); - double y = value.Y / Math.Sqrt(1.0 - (value.W * value.W)); - double z = value.Z / Math.Sqrt(1.0 - (value.W * value.W)); - - axis = new Vector3d(x, y, z); + angle = 2 * Math.Acos(value.W); + axis = Vector3d.Normalize(new Vector3d(value.X, value.Y, value.Z)); } #pragma warning restore CA1021 }