diff --git a/Engine.Core/Primitives/Quaternion.cs b/Engine.Core/Primitives/Quaternion.cs index 47c02f6..5ecb722 100644 --- a/Engine.Core/Primitives/Quaternion.cs +++ b/Engine.Core/Primitives/Quaternion.cs @@ -156,6 +156,27 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab /// The normalized . public static Quaternion Normalize(Quaternion quaternion) => quaternion / Length(quaternion); + public static Quaternion LookAt(Vector3D origin, Vector3D target, Vector3D up) => LookAt(target - origin, up); + public static Quaternion LookAt(Vector3D target, Vector3D up) + { + Vector3D forward = target.Normalized; + + if (forward.LengthSquared() < 1e-6f) + return Identity; + + Vector3D right = up.Cross(forward).Normalized; + Vector3D newUp = forward.Cross(right); + + System.Numerics.Matrix4x4 rot = new( + right.X, right.Y, right.Z, 0f, + newUp.X, newUp.Y, newUp.Z, 0f, + forward.X, forward.Y, forward.Z, 0f, + 0f, 0f, 0f, 1f + ); + + return FromRotationMatrix4x4(rot); + } + /// /// Rotates a around a axis by the specified angle (in radians). /// @@ -212,8 +233,8 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab dot = -dot; } - if (dot > 0.9995f) - return Lerp(from, to, t); + if (dot > 0.999999f) + return to; float angle = Math.Acos(dot); float sinAngle = Math.Sin(angle); @@ -278,8 +299,7 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab /// /// Calculates the from given . /// - /// The axis of the rotation in . - /// The angle in radians. + /// The rotation . /// The rotation calculated by the given . public static System.Numerics.Matrix4x4 ToRotationMatrix4x4(Quaternion quaternion) { @@ -311,6 +331,52 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab ); } + /// + /// Calculates the from given . + /// + /// The rotation . + /// The rotation calculated by the given . + public static Quaternion FromRotationMatrix4x4(System.Numerics.Matrix4x4 martix) + { + float trace = martix.M11 + martix.M22 + martix.M33; + float w, x, y, z; + + if (trace > 0) + { + float s = Math.Sqrt(trace + 1.0f) * 2f; + w = .25f * s; + x = (martix.M23 - martix.M32) / s; + y = (martix.M31 - martix.M13) / s; + z = (martix.M12 - martix.M21) / s; + } + else if ((martix.M11 > martix.M22) && (martix.M11 > martix.M33)) + { + float s = Math.Sqrt(1.0f + martix.M11 - martix.M22 - martix.M33) * 2f; + w = (martix.M23 - martix.M32) / s; + x = .25f * s; + y = (martix.M12 + martix.M21) / s; + z = (martix.M31 + martix.M13) / s; + } + else if (martix.M22 > martix.M33) + { + float s = Math.Sqrt(1.0f + martix.M22 - martix.M11 - martix.M33) * 2f; + w = (martix.M31 - martix.M13) / s; + x = (martix.M12 + martix.M21) / s; + y = .25f * s; + z = (martix.M23 + martix.M32) / s; + } + else + { + float s = Math.Sqrt(1.0f + martix.M33 - martix.M11 - martix.M22) * 2f; + w = (martix.M12 - martix.M21) / s; + x = (martix.M31 + martix.M13) / s; + y = (martix.M23 + martix.M32) / s; + z = .25f * s; + } + + return new(x, y, z, w); + } + /// /// Checks if two s are approximately equal within a specified epsilon range. ///