Misaki
b08662b77d
Fixed conditional selection logic in quaternion and SVD math functions by swapping select argument order for correctness. Fixed LookRotationSafe and normalizesafe to return valid quaternions. Corrected SVD helper functions for proper value swapping and safe reciprocal. Added unit tests for matrix, reflection, projection, refraction, quaternion normalization, LookRotationSafe, and SVD operations. Incremented project version to 1.3.3. Minor formatting and using directive updates.
786 lines
37 KiB
C#
786 lines
37 KiB
C#
using System.Runtime.CompilerServices;
|
|
|
|
using static Misaki.HighPerformance.Mathematics.math;
|
|
|
|
namespace Misaki.HighPerformance.Mathematics;
|
|
|
|
#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters. Such names may become reserved for the language.
|
|
public partial struct quaternion : IEquatable<quaternion>
|
|
#pragma warning restore CS8981 // The type name only contains lower-cased ascii characters. Such names may become reserved for the language.
|
|
{
|
|
/// <summary>
|
|
/// The quaternion component values.
|
|
/// </summary>
|
|
public float4 value;
|
|
|
|
/// <summary>A quaternion representing the identity transform.</summary>
|
|
public static readonly quaternion identity = new quaternion(0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
/// <summary>Constructs a quaternion from four float values.</summary>
|
|
/// <param name="x">The quaternion x component.</param>
|
|
/// <param name="y">The quaternion y component.</param>
|
|
/// <param name="z">The quaternion z component.</param>
|
|
/// <param name="w">The quaternion w component.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public quaternion(float x, float y, float z, float w)
|
|
{
|
|
value.x = x;
|
|
value.y = y;
|
|
value.z = z;
|
|
value.w = w;
|
|
}
|
|
|
|
/// <summary>Constructs a quaternion from float4 vector.</summary>
|
|
/// <param name="value">The quaternion xyzw component values.</param>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public quaternion(float4 value)
|
|
{
|
|
this.value = value;
|
|
}
|
|
|
|
/// <summary>Implicitly converts a float4 vector to a quaternion.</summary>
|
|
/// <param name="v">The quaternion xyzw component values.</param>
|
|
/// <returns>The quaternion constructed from a float4 vector.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static implicit operator quaternion(float4 v)
|
|
{
|
|
return new quaternion(v);
|
|
}
|
|
|
|
/// <summary>Constructs a unit quaternion from a float3x3 rotation matrix. The matrix must be orthonormal.</summary>
|
|
/// <param name="m">The float3x3 orthonormal rotation matrix.</param>
|
|
public quaternion(float3x3 m)
|
|
{
|
|
var u = m.c0;
|
|
var v = m.c1;
|
|
var w = m.c2;
|
|
|
|
var u_sign = (asuint(u.x) & 0x80000000);
|
|
var t = v.y + asfloat(asuint(w.z) ^ u_sign);
|
|
var u_mask = new uint4((int)u_sign >> 31);
|
|
var t_mask = new uint4(asint(t) >> 31);
|
|
|
|
var tr = 1.0f + abs(u.x);
|
|
|
|
var sign_flips = new uint4(0x00000000, 0x80000000, 0x80000000, 0x80000000) ^ (u_mask & new uint4(0x00000000, 0x80000000, 0x00000000, 0x80000000)) ^ (t_mask & new uint4(0x80000000, 0x80000000, 0x80000000, 0x00000000));
|
|
|
|
value = float4(tr, u.y, w.x, v.z) + asfloat(asuint(float4(t, v.x, u.z, w.y)) ^ sign_flips); // +---, +++-, ++-+, +-++
|
|
|
|
value = asfloat((asuint(value) & ~u_mask) | (asuint(value.zwxy) & u_mask));
|
|
value = asfloat((asuint(value.wzyx) & ~t_mask) | (asuint(value) & t_mask));
|
|
value = normalize(value);
|
|
}
|
|
|
|
/// <summary>Constructs a unit quaternion from an orthonormal float4x4 matrix.</summary>
|
|
/// <param name="m">The float4x4 orthonormal rotation matrix.</param>
|
|
public quaternion(float4x4 m)
|
|
{
|
|
var u = m.c0;
|
|
var v = m.c1;
|
|
var w = m.c2;
|
|
|
|
var u_sign = (asuint(u.x) & 0x80000000);
|
|
var t = v.y + asfloat(asuint(w.z) ^ u_sign);
|
|
var u_mask = new uint4((int)u_sign >> 31);
|
|
var t_mask = new uint4(asint(t) >> 31);
|
|
|
|
var tr = 1.0f + abs(u.x);
|
|
|
|
var sign_flips = new uint4(0x00000000, 0x80000000, 0x80000000, 0x80000000) ^ (u_mask & new uint4(0x00000000, 0x80000000, 0x00000000, 0x80000000)) ^ (t_mask & new uint4(0x80000000, 0x80000000, 0x80000000, 0x00000000));
|
|
|
|
value = new float4(tr, u.y, w.x, v.z) + asfloat(asuint(new float4(t, v.x, u.z, w.y)) ^ sign_flips); // +---, +++-, ++-+, +-++
|
|
|
|
value = asfloat((asuint(value) & ~u_mask) | (asuint(value.zwxy) & u_mask));
|
|
value = asfloat((asuint(value.wzyx) & ~t_mask) | (asuint(value) & t_mask));
|
|
|
|
value = normalize(value);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion representing a rotation around a unit axis by an angle in radians.
|
|
/// The rotation direction is clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="axis">The axis of rotation.</param>
|
|
/// <param name="angle">The angle of rotation in radians.</param>
|
|
/// <returns>The quaternion representing a rotation around an axis.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion AxisAngle(float3 axis, float angle)
|
|
{
|
|
sincos(0.5f * angle, out var s, out var c);
|
|
return new quaternion(new float4(axis * s, c));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the y-axis and finally the z-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in x-y-z order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerXYZ(float3 xyz)
|
|
{
|
|
// return mul(rotateZ(xyz.z), mul(rotateY(xyz.y), rotateX(xyz.x)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z - s.y * s.z * c.x,
|
|
// s.y * c.x * c.z + s.x * s.z * c.y,
|
|
// s.z * c.x * c.y - s.x * s.y * c.z,
|
|
// c.x * c.y * c.z + s.y * s.z * s.x
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(-1.0f, 1.0f, -1.0f, 1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the z-axis and finally the y-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in x-z-y order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerXZY(float3 xyz)
|
|
{
|
|
// return mul(rotateY(xyz.y), mul(rotateZ(xyz.z), rotateX(xyz.x)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z + s.y * s.z * c.x,
|
|
// s.y * c.x * c.z + s.x * s.z * c.y,
|
|
// s.z * c.x * c.y - s.x * s.y * c.z,
|
|
// c.x * c.y * c.z - s.y * s.z * s.x
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(1.0f, 1.0f, -1.0f, -1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the x-axis and finally the z-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in y-x-z order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerYXZ(float3 xyz)
|
|
{
|
|
// return mul(rotateZ(xyz.z), mul(rotateX(xyz.x), rotateY(xyz.y)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z - s.y * s.z * c.x,
|
|
// s.y * c.x * c.z + s.x * s.z * c.y,
|
|
// s.z * c.x * c.y + s.x * s.y * c.z,
|
|
// c.x * c.y * c.z - s.y * s.z * s.x
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(-1.0f, 1.0f, 1.0f, -1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the z-axis and finally the x-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in y-z-x order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerYZX(float3 xyz)
|
|
{
|
|
// return mul(rotateX(xyz.x), mul(rotateZ(xyz.z), rotateY(xyz.y)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z - s.y * s.z * c.x,
|
|
// s.y * c.x * c.z - s.x * s.z * c.y,
|
|
// s.z * c.x * c.y + s.x * s.y * c.z,
|
|
// c.x * c.y * c.z + s.y * s.z * s.x
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(-1.0f, -1.0f, 1.0f, 1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the x-axis and finally the y-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// This is the default order rotation order in Unity.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in z-x-y order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerZXY(float3 xyz)
|
|
{
|
|
// return mul(rotateY(xyz.y), mul(rotateX(xyz.x), rotateZ(xyz.z)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z + s.y * s.z * c.x,
|
|
// s.y * c.x * c.z - s.x * s.z * c.y,
|
|
// s.z * c.x * c.y - s.x * s.y * c.z,
|
|
// c.x * c.y * c.z + s.y * s.z * s.x
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(1.0f, -1.0f, -1.0f, 1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the y-axis and finally the x-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in z-y-x order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerZYX(float3 xyz)
|
|
{
|
|
// return mul(rotateX(xyz.x), mul(rotateY(xyz.y), rotateZ(xyz.z)));
|
|
sincos(0.5f * xyz, out var s, out var c);
|
|
return new quaternion(
|
|
// s.x * c.y * c.z + s.y * s.z * c.x,
|
|
// s.y * c.x * c.z - s.x * s.z * c.y,
|
|
// s.z * c.x * c.y + s.x * s.y * c.z,
|
|
// c.x * c.y * c.z - s.y * s.x * s.z
|
|
new float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * new float4(c.xyz, s.x) * new float4(1.0f, -1.0f, 1.0f, -1.0f));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the y-axis and finally the z-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in x-y-z order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerXYZ(float x, float y, float z)
|
|
{
|
|
return EulerXYZ(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the z-axis and finally the y-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in x-z-y order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerXZY(float x, float y, float z)
|
|
{
|
|
return EulerXZY(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the x-axis and finally the z-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in y-x-z order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerYXZ(float x, float y, float z)
|
|
{
|
|
return EulerYXZ(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the z-axis and finally the x-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in y-z-x order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerYZX(float x, float y, float z)
|
|
{
|
|
return EulerYZX(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the x-axis and finally the y-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// This is the default order rotation order in Unity.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in z-x-y order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerZXY(float x, float y, float z)
|
|
{
|
|
return EulerZXY(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the y-axis and finally the x-axis.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in z-y-x order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion EulerZYX(float x, float y, float z)
|
|
{
|
|
return EulerZYX(new float3(x, y, z));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing 3 rotations around the principal axes in a given order.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// When the rotation order is known at compile time, it is recommended for performance reasons to use specific
|
|
/// Euler rotation constructors such as EulerZXY(...).
|
|
/// </summary>
|
|
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
|
|
/// <param name="order">The order in which the rotations are applied.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in the specified order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion Euler(float3 xyz, RotationOrder order = RotationOrder.ZXY)
|
|
{
|
|
switch (order)
|
|
{
|
|
case RotationOrder.XYZ:
|
|
return EulerXYZ(xyz);
|
|
case RotationOrder.XZY:
|
|
return EulerXZY(xyz);
|
|
case RotationOrder.YXZ:
|
|
return EulerYXZ(xyz);
|
|
case RotationOrder.YZX:
|
|
return EulerYZX(xyz);
|
|
case RotationOrder.ZXY:
|
|
return EulerZXY(xyz);
|
|
case RotationOrder.ZYX:
|
|
return EulerZYX(xyz);
|
|
default:
|
|
return identity;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion constructed by first performing 3 rotations around the principal axes in a given order.
|
|
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
|
|
/// When the rotation order is known at compile time, it is recommended for performance reasons to use specific
|
|
/// Euler rotation constructors such as EulerZXY(...).
|
|
/// </summary>
|
|
/// <param name="x">The rotation angle around the x-axis in radians.</param>
|
|
/// <param name="y">The rotation angle around the y-axis in radians.</param>
|
|
/// <param name="z">The rotation angle around the z-axis in radians.</param>
|
|
/// <param name="order">The order in which the rotations are applied.</param>
|
|
/// <returns>The quaternion representing the Euler angle rotation in the specified order.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion Euler(float x, float y, float z, RotationOrder order = RotationOrder.Default)
|
|
{
|
|
return Euler(new float3(x, y, z), order);
|
|
}
|
|
|
|
/// <summary>Returns a quaternion that rotates around the x-axis by a given number of radians.</summary>
|
|
/// <param name="angle">The clockwise rotation angle when looking along the x-axis towards the origin in radians.</param>
|
|
/// <returns>The quaternion representing a rotation around the x-axis.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion RotateX(float angle)
|
|
{
|
|
sincos(0.5f * angle, out var s, out var c);
|
|
return new quaternion(s, 0.0f, 0.0f, c);
|
|
}
|
|
|
|
/// <summary>Returns a quaternion that rotates around the y-axis by a given number of radians.</summary>
|
|
/// <param name="angle">The clockwise rotation angle when looking along the y-axis towards the origin in radians.</param>
|
|
/// <returns>The quaternion representing a rotation around the y-axis.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion RotateY(float angle)
|
|
{
|
|
sincos(0.5f * angle, out var s, out var c);
|
|
return new quaternion(0.0f, s, 0.0f, c);
|
|
}
|
|
|
|
/// <summary>Returns a quaternion that rotates around the z-axis by a given number of radians.</summary>
|
|
/// <param name="angle">The clockwise rotation angle when looking along the z-axis towards the origin in radians.</param>
|
|
/// <returns>The quaternion representing a rotation around the z-axis.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion RotateZ(float angle)
|
|
{
|
|
sincos(0.5f * angle, out var s, out var c);
|
|
return new quaternion(0.0f, 0.0f, s, c);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion view rotation given a unit length forward vector and a unit length up vector.
|
|
/// The two input vectors are assumed to be unit length and not collinear.
|
|
/// If these assumptions are not met use float3x3.LookRotationSafe instead.
|
|
/// </summary>
|
|
/// <param name="forward">The view forward direction.</param>
|
|
/// <param name="up">The view up direction.</param>
|
|
/// <returns>The quaternion view rotation.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion LookRotation(float3 forward, float3 up)
|
|
{
|
|
var t = normalize(cross(up, forward));
|
|
return new quaternion(new float3x3(t, cross(forward, t), forward));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a quaternion view rotation given a forward vector and an up vector.
|
|
/// The two input vectors are not assumed to be unit length.
|
|
/// If the magnitude of either of the vectors is so extreme that the calculation cannot be carried out reliably or the vectors are collinear,
|
|
/// the identity will be returned instead.
|
|
/// </summary>
|
|
/// <param name="forward">The view forward direction.</param>
|
|
/// <param name="up">The view up direction.</param>
|
|
/// <returns>The quaternion view rotation or the identity quaternion.</returns>
|
|
public static quaternion LookRotationSafe(float3 forward, float3 up)
|
|
{
|
|
var forwardLengthSq = dot(forward, forward);
|
|
var upLengthSq = dot(up, up);
|
|
|
|
forward *= rsqrt(forwardLengthSq);
|
|
up *= rsqrt(upLengthSq);
|
|
|
|
var t = cross(up, forward);
|
|
var tLengthSq = dot(t, t);
|
|
t *= rsqrt(tLengthSq);
|
|
|
|
var mn = min(min(forwardLengthSq, upLengthSq), tLengthSq);
|
|
var mx = max(max(forwardLengthSq, upLengthSq), tLengthSq);
|
|
|
|
var accept = mn > 1e-35f && mx < 1e35f && isfinite(forwardLengthSq) && isfinite(upLengthSq) && isfinite(tLengthSq);
|
|
return new quaternion(select(new quaternion(new float3x3(t, cross(forward, t), forward)).value, float4.unitW, accept));
|
|
}
|
|
|
|
/// <summary>Returns true if the quaternion is equal to a given quaternion, false otherwise.</summary>
|
|
/// <param name="x">The quaternion to compare with.</param>
|
|
/// <returns>True if the quaternion is equal to the input, false otherwise.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public bool Equals(quaternion x)
|
|
{
|
|
return value.x == x.value.x && value.y == x.value.y && value.z == x.value.z && value.w == x.value.w;
|
|
}
|
|
|
|
/// <summary>Returns whether true if the quaternion is equal to a given quaternion, false otherwise.</summary>
|
|
/// <param name="x">The object to compare with.</param>
|
|
/// <returns>True if the quaternion is equal to the input, false otherwise.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override bool Equals(object? x)
|
|
{
|
|
return x is quaternion converted && Equals(converted);
|
|
}
|
|
|
|
/// <summary>Returns a hash code for the quaternion.</summary>
|
|
/// <returns>The hash code of the quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override int GetHashCode()
|
|
{
|
|
return (int)hash(this);
|
|
}
|
|
|
|
/// <summary>Returns a string representation of the quaternion.</summary>
|
|
/// <returns>The string representation of the quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override string ToString()
|
|
{
|
|
return string.Format("quaternion({0}f, {1}f, {2}f, {3}f)", value.x, value.y, value.z, value.w);
|
|
}
|
|
}
|
|
|
|
#pragma warning disable CS8981 // The type name only contains lower-cased ascii characters. Such names may become reserved for the language.
|
|
public static partial class math
|
|
#pragma warning restore CS8981 // The type name only contains lower-cased ascii characters. Such names may become reserved for the language.
|
|
{
|
|
/// <summary>Returns a quaternion constructed from four float values.</summary>
|
|
/// <param name="x">The x component of the quaternion.</param>
|
|
/// <param name="y">The y component of the quaternion.</param>
|
|
/// <param name="z">The z component of the quaternion.</param>
|
|
/// <param name="w">The w component of the quaternion.</param>
|
|
/// <returns>The quaternion constructed from individual components.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion quaternion(float x, float y, float z, float w)
|
|
{
|
|
return new quaternion(x, y, z, w);
|
|
}
|
|
|
|
/// <summary>Returns a quaternion constructed from a float4 vector.</summary>
|
|
/// <param name="value">The float4 containing the components of the quaternion.</param>
|
|
/// <returns>The quaternion constructed from a float4.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion quaternion(float4 value)
|
|
{
|
|
return new quaternion(value);
|
|
}
|
|
|
|
/// <summary>Returns the conjugate of a quaternion value.</summary>
|
|
/// <param name="q">The quaternion to conjugate.</param>
|
|
/// <returns>The conjugate of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion conjugate(quaternion q)
|
|
{
|
|
return new quaternion(q.value * new float4(-1.0f, -1.0f, -1.0f, 1.0f));
|
|
}
|
|
|
|
/// <summary>Returns the inverse of a quaternion value.</summary>
|
|
/// <param name="q">The quaternion to invert.</param>
|
|
/// <returns>The quaternion inverse of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion inverse(quaternion q)
|
|
{
|
|
var x = q.value;
|
|
return quaternion(rcp(dot(x, x)) * x * new float4(-1.0f, -1.0f, -1.0f, 1.0f));
|
|
}
|
|
|
|
/// <summary>Returns the dot product of two quaternions.</summary>
|
|
/// <param name="a">The first quaternion.</param>
|
|
/// <param name="b">The second quaternion.</param>
|
|
/// <returns>The dot product of two quaternions.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float dot(quaternion a, quaternion b)
|
|
{
|
|
return dot(a.value, b.value);
|
|
}
|
|
|
|
/// <summary>Returns the length of a quaternion.</summary>
|
|
/// <param name="q">The input quaternion.</param>
|
|
/// <returns>The length of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float length(quaternion q)
|
|
{
|
|
return sqrt(dot(q.value, q.value));
|
|
}
|
|
|
|
/// <summary>Returns the squared length of a quaternion.</summary>
|
|
/// <param name="q">The input quaternion.</param>
|
|
/// <returns>The length squared of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float lengthsq(quaternion q)
|
|
{
|
|
return dot(q.value, q.value);
|
|
}
|
|
|
|
/// <summary>Returns a normalized version of a quaternion q by scaling it by 1 / length(q).</summary>
|
|
/// <param name="q">The quaternion to normalize.</param>
|
|
/// <returns>The normalized quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion normalize(quaternion q)
|
|
{
|
|
var x = q.value;
|
|
return quaternion(rsqrt(dot(x, x)) * x);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a safe normalized version of the q by scaling it by 1 / length(q).
|
|
/// Returns the identity when 1 / length(q) does not produce a finite number.
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to normalize.</param>
|
|
/// <returns>The normalized quaternion or the identity quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion normalizesafe(quaternion q)
|
|
{
|
|
var x = q.value;
|
|
var len = dot(x, x);
|
|
return quaternion(select(x * rsqrt(len), Mathematics.quaternion.identity.value, len > FLT_MIN_NORMAL));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a safe normalized version of the q by scaling it by 1 / length(q).
|
|
/// Returns the given default value when 1 / length(q) does not produce a finite number.
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to normalize.</param>
|
|
/// <param name="defaultvalue">The default value.</param>
|
|
/// <returns>The normalized quaternion or the default value.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion normalizesafe(quaternion q, quaternion defaultvalue)
|
|
{
|
|
var x = q.value;
|
|
var len = dot(x, x);
|
|
return quaternion(select(x * rsqrt(len), defaultvalue.value, len > FLT_MIN_NORMAL));
|
|
}
|
|
|
|
/// <summary>Returns the natural exponent of a quaternion. Assumes w is zero.</summary>
|
|
/// <param name="q">The quaternion with w component equal to zero.</param>
|
|
/// <returns>The natural exponent of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion unitexp(quaternion q)
|
|
{
|
|
var v_rcp_len = rsqrt(dot(q.value.xyz, q.value.xyz));
|
|
var v_len = rcp(v_rcp_len);
|
|
sincos(v_len, out var s, out var c);
|
|
return quaternion(new float4(q.value.xyz * v_rcp_len * s, c));
|
|
}
|
|
|
|
/// <summary>Returns the natural exponent of a quaternion.</summary>
|
|
/// <param name="q">The quaternion.</param>
|
|
/// <returns>The natural exponent of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion exp(quaternion q)
|
|
{
|
|
var v_rcp_len = rsqrt(dot(q.value.xyz, q.value.xyz));
|
|
var v_len = rcp(v_rcp_len);
|
|
sincos(v_len, out var s, out var c);
|
|
return quaternion(new float4(q.value.xyz * v_rcp_len * s, c) * exp(q.value.w));
|
|
}
|
|
|
|
/// <summary>Returns the natural logarithm of a unit length quaternion.</summary>
|
|
/// <param name="q">The unit length quaternion.</param>
|
|
/// <returns>The natural logarithm of the unit length quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion unitlog(quaternion q)
|
|
{
|
|
var w = clamp(q.value.w, -1.0f, 1.0f);
|
|
var s = acos(w) * rsqrt(1.0f - w * w);
|
|
return quaternion(new float4(q.value.xyz * s, 0.0f));
|
|
}
|
|
|
|
/// <summary>Returns the natural logarithm of a quaternion.</summary>
|
|
/// <param name="q">The quaternion.</param>
|
|
/// <returns>The natural logarithm of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion log(quaternion q)
|
|
{
|
|
var v_len_sq = dot(q.value.xyz, q.value.xyz);
|
|
var q_len_sq = v_len_sq + q.value.w * q.value.w;
|
|
|
|
var s = acos(clamp(q.value.w * rsqrt(q_len_sq), -1.0f, 1.0f)) * rsqrt(v_len_sq);
|
|
return quaternion(new float4(q.value.xyz * s, 0.5f * log(q_len_sq)));
|
|
}
|
|
|
|
/// <summary>Returns the result of transforming the quaternion b by the quaternion a.</summary>
|
|
/// <param name="a">The quaternion on the left.</param>
|
|
/// <param name="b">The quaternion on the right.</param>
|
|
/// <returns>The result of transforming quaternion b by the quaternion a.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion mul(quaternion a, quaternion b)
|
|
{
|
|
return quaternion(a.value.wwww * b.value + (a.value.xyzx * b.value.wwwx + a.value.yzxy * b.value.zxyy) * new float4(1.0f, 1.0f, 1.0f, -1.0f) - a.value.zxyz * b.value.yzxz);
|
|
}
|
|
|
|
/// <summary>Returns the result of transforming a vector by a quaternion.</summary>
|
|
/// <param name="q">The quaternion transformation.</param>
|
|
/// <param name="v">The vector to transform.</param>
|
|
/// <returns>The transformation of vector v by quaternion q.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float3 mul(quaternion q, float3 v)
|
|
{
|
|
var t = 2 * cross(q.value.xyz, v);
|
|
return v + q.value.w * t + cross(q.value.xyz, t);
|
|
}
|
|
|
|
/// <summary>Returns the result of rotating a vector by a unit quaternion.</summary>
|
|
/// <param name="q">The quaternion rotation.</param>
|
|
/// <param name="v">The vector to rotate.</param>
|
|
/// <returns>The rotation of vector v by quaternion q.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float3 rotate(quaternion q, float3 v)
|
|
{
|
|
var t = 2 * cross(q.value.xyz, v);
|
|
return v + q.value.w * t + cross(q.value.xyz, t);
|
|
}
|
|
|
|
/// <summary>Returns the result of a normalized linear interpolation between two quaternions q1 and a2 using an interpolation parameter t.</summary>
|
|
/// <remarks>
|
|
/// Prefer to use this over slerp() when you know the distance between q1 and q2 is small. This can be much
|
|
/// higher performance due to avoiding trigonometric function evaluations that occur in slerp().
|
|
/// </remarks>
|
|
/// <param name="q1">The first quaternion.</param>
|
|
/// <param name="q2">The second quaternion.</param>
|
|
/// <param name="t">The interpolation parameter.</param>
|
|
/// <returns>The normalized linear interpolation of two quaternions.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion nlerp(quaternion q1, quaternion q2, float t)
|
|
{
|
|
return normalize(q1.value + t * (chgsign(q2.value, dot(q1, q2)) - q1.value));
|
|
}
|
|
|
|
/// <summary>Returns the result of a spherical interpolation between two quaternions q1 and a2 using an interpolation parameter t.</summary>
|
|
/// <param name="q1">The first quaternion.</param>
|
|
/// <param name="q2">The second quaternion.</param>
|
|
/// <param name="t">The interpolation parameter.</param>
|
|
/// <returns>The spherical linear interpolation of two quaternions.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion slerp(quaternion q1, quaternion q2, float t)
|
|
{
|
|
var dt = dot(q1, q2);
|
|
if (dt < 0.0f)
|
|
{
|
|
dt = -dt;
|
|
q2.value = -q2.value;
|
|
}
|
|
|
|
if (dt < 0.9995f)
|
|
{
|
|
var angle = acos(dt);
|
|
var s = rsqrt(1.0f - dt * dt); // 1.0f / sin(angle)
|
|
var w1 = sin(angle * (1.0f - t)) * s;
|
|
var w2 = sin(angle * t) * s;
|
|
return quaternion(q1.value * w1 + q2.value * w2);
|
|
}
|
|
else
|
|
{
|
|
// if the angle is small, use linear interpolation
|
|
return nlerp(q1, q2, t);
|
|
}
|
|
}
|
|
|
|
/// <summary>Returns the angle in radians between two unit quaternions.</summary>
|
|
/// <param name="q1">The first quaternion.</param>
|
|
/// <param name="q2">The second quaternion.</param>
|
|
/// <returns>The angle between two unit quaternions.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static float angle(quaternion q1, quaternion q2)
|
|
{
|
|
var diff = asin(length(normalize(mul(conjugate(q1), q2)).value.xyz));
|
|
return diff + diff;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Extracts the rotation from a matrix.
|
|
/// </summary>
|
|
/// <remarks>This method supports any type of rotation matrix: if the matrix has a non uniform scale you should use this method.</remarks>
|
|
/// <param name="m">Matrix to extract rotation from</param>
|
|
/// <returns>Extracted rotation</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static quaternion rotation(float3x3 m)
|
|
{
|
|
var det = determinant(m);
|
|
if (abs(1f - det) < svd.EPSILON_DETERMINANT)
|
|
{
|
|
return new quaternion(m);
|
|
}
|
|
|
|
if (abs(det) > svd.EPSILON_DETERMINANT)
|
|
{
|
|
var tmp = mulScale(m, rsqrt(new float3(lengthsq(m.c0), lengthsq(m.c1), lengthsq(m.c2))));
|
|
if (abs(1f - determinant(tmp)) < svd.EPSILON_DETERMINANT)
|
|
{
|
|
return new quaternion(tmp);
|
|
}
|
|
}
|
|
|
|
return svd.svdRotation(m);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
private static float3x3 adj(float3x3 m, out float det)
|
|
{
|
|
float3x3 adjT;
|
|
adjT.c0 = cross(m.c1, m.c2);
|
|
adjT.c1 = cross(m.c2, m.c0);
|
|
adjT.c2 = cross(m.c0, m.c1);
|
|
det = dot(m.c0, adjT.c0);
|
|
|
|
return transpose(adjT);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
private static bool adjInverse(float3x3 m, out float3x3 i, float epsilon = svd.EPSILON_NORMAL)
|
|
{
|
|
i = adj(m, out var det);
|
|
var c = abs(det) > epsilon;
|
|
var detInv = select(rcp(det), new float3(1f), c);
|
|
i = scaleMul(detInv, i);
|
|
return c;
|
|
}
|
|
|
|
/// <summary>Returns a uint hash code of a quaternion.</summary>
|
|
/// <param name="q">The quaternion to hash.</param>
|
|
/// <returns>The hash code for the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static uint hash(quaternion q)
|
|
{
|
|
return hash(q.value);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a uint4 vector hash code of a quaternion.
|
|
/// When multiple elements are to be hashes together, it can more efficient to calculate and combine wide hash
|
|
/// that are only reduced to a narrow uint hash at the very end instead of at every step.
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to hash.</param>
|
|
/// <returns>The uint4 vector hash code of the input quaternion.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static uint4 hashwide(quaternion q)
|
|
{
|
|
return hashwide(q.value);
|
|
}
|
|
} |