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.
///