diff --git a/Engine.Core/Primitives/Quaternion.cs b/Engine.Core/Primitives/Quaternion.cs index 4593ee3..7a04539 100644 --- a/Engine.Core/Primitives/Quaternion.cs +++ b/Engine.Core/Primitives/Quaternion.cs @@ -217,6 +217,63 @@ public readonly struct Quaternion(float x, float y, float z, float w) return new Quaternion(axis.X * sinHalf, axis.Y * sinHalf, axis.Z * sinHalf, MathF.Cos(halfAngle)); } + /// + /// Calculates the from given yaw, pitch and roll values. + /// + /// The rotation calculated by the given parameters. + public static Quaternion FromAngles(float x, float y, float z) + { + float cosX = Math.Cos(x * .5f); + float sinX = Math.Sin(x * .5f); + float cosY = Math.Cos(y * .5f); + float sinY = Math.Sin(y * .5f); + float cozZ = Math.Cos(z * .5f); + float sinZ = Math.Sin(z * .5f); + + return new Quaternion( + x: sinX * cosY * cozZ - cosX * sinY * sinZ, + y: cosX * sinY * cozZ + sinX * cosY * sinZ, + z: cosX * cosY * sinZ - sinX * sinY * cozZ, + w: cosX * cosY * cozZ + sinX * sinY * sinZ + ); + } + + /// + /// Calculates the from given . + /// + /// The axis of the rotation in . + /// The angle in radians. + /// The rotation calculated by the given . + public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion) + { + float m00 = 1 - 2 * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z); + float m01 = 2 * (quaternion.X * quaternion.Y - quaternion.W * quaternion.Z); + float m02 = 2 * (quaternion.X * quaternion.Z + quaternion.W * quaternion.Y); + float m03 = 0; + + float m10 = 2 * (quaternion.X * quaternion.Y + quaternion.W * quaternion.Z); + float m11 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Z * quaternion.Z); + float m12 = 2 * (quaternion.Y * quaternion.Z - quaternion.W * quaternion.X); + float m13 = 0; + + float m20 = 2 * (quaternion.X * quaternion.Z - quaternion.W * quaternion.Y); + float m21 = 2 * (quaternion.Y * quaternion.Z + quaternion.W * quaternion.X); + float m22 = 1 - 2 * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y); + float m23 = 0; + + float m30 = 0; + float m31 = 0; + float m32 = 0; + float m33 = 1; + + return new( + m00, m01, m02, m03, + m10, m11, m12, m13, + m20, m21, m22, m23, + m30, m31, m32, m33 + ); + } + /// /// Checks if two s are approximately equal within a specified epsilon range. /// @@ -291,6 +348,9 @@ public static class QuaternionExtensions /// public static float Dot(this Quaternion left, Quaternion right) => Quaternion.Dot(left, right); + /// + public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(this Quaternion quaternion) => Quaternion.ToRotationMatrix4x4(quaternion); + /// public static Quaternion CreateRotation(this Vector3D axis, float angle) => Quaternion.FromAxisAngle(axis, angle);