Syntriax/Syntriax.Engine
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: Quaternion.SLerp snapping issue

Browse Source
This commit is contained in:
Syntriax
2025-10-23 12:50:38 +03:00
parent 4bdd32b808
commit 981732ff75
1 changed files with 70 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
Engine.Core/Primitives/Quaternion.cs
View File

@@ -156,6 +156,27 @@ public readonly struct Quaternion(float x, float y, float z, float w) : IEquatab
/// <returns>The normalized <see cref="Quaternion"/>.</returns>
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);
}
/// <summary>
/// Rotates a <see cref="Quaternion"/> around a axis by the specified angle (in radians).
/// </summary>
@@ -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
/// <summary>
/// Calculates the <see cref="System.Numerics.Matrix4x4"/> from given <see cref="Quaternion"/>.
/// </summary>
/// <param name="axis">The axis of the rotation in <see cref="Vector3D"/>.</param>
/// <param name="angle">The angle in radians.</param>
/// <param name="quaternion">The rotation <see cref="Quaternion"/>.</param>
/// <returns>The rotation <see cref="System.Numerics.Matrix4x4"/> calculated by the given <see cref="Quaternion"/>.</returns>
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
);
}
/// <summary>
/// Calculates the <see cref="Quaternion"/> from given <see cref="System.Numerics.Matrix4x4"/>.
/// </summary>
/// <param name="martix">The rotation <see cref="System.Numerics.Matrix4x4"/>.</param>
/// <returns>The rotation <see cref="Quaternion"/> calculated by the given <see cref="System.Numerics.Matrix4x4"/>.</returns>
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);
}
/// <summary>
/// Checks if two <see cref="Quaternion"/>s are approximately equal within a specified epsilon range.
/// </summary>