Misaki
b914716225
All checks were successful
Publish NuGet Packages / publish (push) Successful in 1m47s
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(u_sign >> 31);
|
|
var t_mask = new uint4(asuint(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>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(u_sign >> 31);
|
|
var t_mask = new uint4(asuint(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(float4.unitW, new quaternion(new float3x3(t, cross(forward, t), forward)).value, 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(Mathematics.quaternion.identity.value, x * rsqrt(len), 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(defaultvalue.value, x * rsqrt(len), 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(new float3(1f), rcp(det), 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);
|
|
}
|
|
} |