582 lines
24 KiB
C#
582 lines
24 KiB
C#
using System;
|
|
#if UNITY_5_3_OR_NEWER
|
|
using Quaternion = UnityEngine.Quaternion;
|
|
#endif
|
|
|
|
namespace LinearAlgebra {
|
|
|
|
#if UNITY_5_3_OR_NEWER
|
|
public class QuaternionExtensions {
|
|
public static Quaternion Reflect(Quaternion q) {
|
|
return new(-q.x, -q.y, -q.z, q.w);
|
|
}
|
|
|
|
public static Matrix2 ToRotationMatrix(Quaternion q) {
|
|
float w = q.x, x = q.y, y = q.z, z = q.w;
|
|
|
|
float[,] result = new float[,]
|
|
{
|
|
{ 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
|
|
{ 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
|
|
{ 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
|
|
};
|
|
return new Matrix2(result);
|
|
}
|
|
|
|
public static Quaternion FromRotationMatrix(Matrix2 m) {
|
|
float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
|
|
float w, x, y, z;
|
|
|
|
if (trace > 0) {
|
|
float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
|
|
w = 0.25f / s;
|
|
x = (m.data[2, 1] - m.data[1, 2]) * s;
|
|
y = (m.data[0, 2] - m.data[2, 0]) * s;
|
|
z = (m.data[1, 0] - m.data[0, 1]) * s;
|
|
}
|
|
else {
|
|
if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
|
|
w = (m.data[2, 1] - m.data[1, 2]) / s;
|
|
x = 0.25f * s;
|
|
y = (m.data[0, 1] + m.data[1, 0]) / s;
|
|
z = (m.data[0, 2] + m.data[2, 0]) / s;
|
|
}
|
|
else if (m.data[1, 1] > m.data[2, 2]) {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
|
|
w = (m.data[0, 2] - m.data[2, 0]) / s;
|
|
x = (m.data[0, 1] + m.data[1, 0]) / s;
|
|
y = 0.25f * s;
|
|
z = (m.data[1, 2] + m.data[2, 1]) / s;
|
|
}
|
|
else {
|
|
float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
|
|
w = (m.data[1, 0] - m.data[0, 1]) / s;
|
|
x = (m.data[0, 2] + m.data[2, 0]) / s;
|
|
y = (m.data[1, 2] + m.data[2, 1]) / s;
|
|
z = 0.25f * s;
|
|
}
|
|
}
|
|
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
}
|
|
#else
|
|
public struct Quaternion {
|
|
public float x;
|
|
public float y;
|
|
public float z;
|
|
public float w;
|
|
|
|
/// <summary>
|
|
/// create a new quaternion with the given values
|
|
/// </summary>
|
|
/// <param name="x">x component</param>
|
|
/// <param name="y">y component</param>
|
|
/// <param name="z">z component</param>
|
|
/// <param name="w">w component</param>
|
|
public Quaternion(float x, float y, float z, float w) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
|
|
/// <summary>
|
|
/// An identity quaternion
|
|
/// </summary>
|
|
public static readonly Quaternion identity = new(0, 0, 0, 1);
|
|
|
|
private readonly Vector3Float xyz => new(x, y, z);
|
|
|
|
private readonly float magnitude => MathF.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);
|
|
|
|
private readonly float sqrMagnitude => x * x + y * y + z * z + w * w;
|
|
|
|
/// <summary>
|
|
/// Convert to unit quaternion
|
|
/// </summary>
|
|
/// This will preserve the orientation,
|
|
/// but ensures that it is a unit quaternion.
|
|
public readonly Quaternion normalized {
|
|
get {
|
|
float length = this.magnitude;
|
|
Quaternion q = new(this.x / length, this.y / length, this.z / length, this.w / length);
|
|
return q;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert to unity quaternion
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to convert</param>
|
|
/// <returns>A unit quaternion</returns>
|
|
/// This will preserve the orientation,
|
|
/// but ensures that it is a unit quaternion.
|
|
public static Quaternion Normalize(Quaternion q) {
|
|
return q.normalized;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert to euler angles
|
|
/// </summary>
|
|
/// <param name="q">The quaternion to convert</param>
|
|
/// <returns>A vector containing euler angles</returns>
|
|
/// The euler angles performed in the order: Z, X, Y
|
|
public static Vector3Float ToAngles(Quaternion q) {
|
|
// Extract Euler angles in Unity order (X = pitch, Y = yaw, Z = roll), returned in degrees as (x,pitch),(y,yaw),(z,roll)
|
|
// Handle singularities/gimbal lock
|
|
float test = 2f * (q.w * q.x - q.y * q.z);
|
|
// clamp
|
|
if (test >= 1f) test = 1f;
|
|
if (test <= -1f) test = -1f;
|
|
|
|
float pitch = MathF.Asin(test); // X
|
|
|
|
float roll = MathF.Atan2(2f * (q.w * q.z + q.x * q.y),
|
|
1f - 2f * (q.x * q.x + q.z * q.z)); // Z
|
|
|
|
float yaw = MathF.Atan2(2f * (q.w * q.y + q.x * q.z),
|
|
1f - 2f * (q.y * q.y + q.x * q.x)); // Y
|
|
|
|
const float rad2deg = 180f / MathF.PI;
|
|
return new Vector3Float(pitch * rad2deg, yaw * rad2deg, roll * rad2deg);
|
|
// float test = q.x * q.y + q.z * q.w;
|
|
// if (test > 0.499f) // singularity at north pole
|
|
// return new Vector3Float(0, 2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, 90);
|
|
|
|
// else if (test < -0.499f) // singularity at south pole
|
|
// return new Vector3Float(0, -2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, -90);
|
|
|
|
// else {
|
|
// float sqx = q.x * q.x;
|
|
// float sqy = q.y * q.y;
|
|
// float sqz = q.z * q.z;
|
|
|
|
// return new Vector3Float(
|
|
// MathF.Atan2(2 * q.x * q.w - 2 * q.y * q.z, 1 - 2 * sqx - 2 * sqz) *
|
|
// AngleFloat.Rad2Deg,
|
|
// MathF.Atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * sqy - 2 * sqz) *
|
|
// AngleFloat.Rad2Deg,
|
|
// MathF.Asin(2 * test) * AngleFloat.Rad2Deg);
|
|
// }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create a rotation from euler angles
|
|
/// </summary>
|
|
/// <param name="x">The angle around the right axis</param>
|
|
/// <param name="y">The angle around the upward axis</param>
|
|
/// <param name="z">The angle around the forward axis</param>
|
|
/// <returns>The resulting quaternion</returns>
|
|
/// Rotation are appied in the order Z, X, Y.
|
|
public static Quaternion Euler(float x, float y, float z) {
|
|
return Quaternion.Euler(new Vector3Float(x, y, z));
|
|
}
|
|
/// <summary>
|
|
/// Create a rotation from a vector containing euler angles
|
|
/// </summary>
|
|
/// <param name="eulerAngles">Vector with the euler angles</param>
|
|
/// <returns>The resulting quaternion</returns>
|
|
/// Rotation are appied in the order Z, X, Y.
|
|
public static Quaternion Euler(Vector3Float angles) {
|
|
Vector3Float euler = angles * AngleFloat.Deg2Rad;
|
|
float cx = MathF.Cos(euler.horizontal * 0.5f);
|
|
float sx = MathF.Sin(euler.horizontal * 0.5f);
|
|
float cy = MathF.Cos(euler.vertical * 0.5f);
|
|
float sy = MathF.Sin(euler.vertical * 0.5f);
|
|
float cz = MathF.Cos(euler.depth * 0.5f);
|
|
float sz = MathF.Sin(euler.depth * 0.5f);
|
|
|
|
// Unity uses intrinsic Z, then X, then Y -> q = Qy * Qx * Qz
|
|
Quaternion q;
|
|
q.w = cy * cx * cz + sy * sx * sz;
|
|
q.x = cy * sx * cz + sy * cx * sz;
|
|
q.y = sy * cx * cz - cy * sx * sz;
|
|
q.z = cy * cx * sz - sy * sx * cz;
|
|
return q;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Multiply two quaternions
|
|
/// </summary>
|
|
/// <param name="q1"></param>
|
|
/// <param name="q2"></param>
|
|
/// <returns>The resulting rotation</returns>
|
|
public static Quaternion operator *(Quaternion q1, Quaternion q2) {
|
|
return new Quaternion(
|
|
q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x,
|
|
-q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y,
|
|
q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z,
|
|
-q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotate a vector using this quaterion
|
|
/// </summary>
|
|
/// <param name="q">The rotation</param>
|
|
/// <param name="v">The vector to rotate</param>
|
|
/// <returns>The rotated vector</returns>
|
|
public static Vector3Float operator *(Quaternion q, Vector3Float v) {
|
|
float num = q.x * 2;
|
|
float num2 = q.y * 2;
|
|
float num3 = q.z * 2;
|
|
float num4 = q.x * num;
|
|
float num5 = q.y * num2;
|
|
float num6 = q.z * num3;
|
|
float num7 = q.x * num2;
|
|
float num8 = q.x * num3;
|
|
float num9 = q.y * num3;
|
|
float num10 = q.w * num;
|
|
float num11 = q.w * num2;
|
|
float num12 = q.w * num3;
|
|
|
|
float px = v.horizontal;
|
|
float py = v.vertical;
|
|
float pz = v.depth;
|
|
float rx =
|
|
(1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
|
|
float ry =
|
|
(num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
|
|
float rz =
|
|
(num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
|
|
Vector3Float result = new(rx, ry, rz);
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// The inverse of quaterion
|
|
/// </summary>
|
|
/// <param name="quaternion">The quaternion for which the inverse is
|
|
/// needed</param> <returns>The inverted quaternion</returns>
|
|
public static Quaternion Inverse(Quaternion q) {
|
|
float n = MathF.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
|
|
return new Quaternion(-q.x / n, -q.y / n, -q.z / n, q.w / n);
|
|
}
|
|
|
|
/// <summary>
|
|
/// A rotation which looks in the given direction
|
|
/// </summary>
|
|
/// <param name="forward">The look direction</param>
|
|
/// <param name="upwards">The up direction</param>
|
|
/// <returns>The look rotation</returns>
|
|
public static Quaternion LookRotation(Vector3Float forward, Vector3Float up) {
|
|
Vector3Float nForward = forward.normalized;
|
|
Vector3Float nRight = Vector3Float.Normalize(Vector3Float.Cross(up, nForward));
|
|
Vector3Float nUp = Vector3Float.Cross(nForward, nRight);
|
|
float m00 = nRight.horizontal; // x;
|
|
float m01 = nRight.vertical; // y;
|
|
float m02 = nRight.depth; // z;
|
|
float m10 = nUp.horizontal; // x;
|
|
float m11 = nUp.vertical; // y;
|
|
float m12 = nUp.depth; // z;
|
|
float m20 = nForward.horizontal; // x;
|
|
float m21 = nForward.vertical; // y;
|
|
float m22 = nForward.depth; // z;
|
|
|
|
float num8 = (m00 + m11) + m22;
|
|
float x, y, z, w;
|
|
if (num8 > 0) {
|
|
float num = MathF.Sqrt(num8 + 1);
|
|
w = num * 0.5f;
|
|
num = 0.5f / num;
|
|
x = (m12 - m21) * num;
|
|
y = (m20 - m02) * num;
|
|
z = (m01 - m10) * num;
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
if ((m00 >= m11) && (m00 >= m22)) {
|
|
float num7 = MathF.Sqrt(((1 + m00) - m11) - m22);
|
|
float num4 = 0.5F / num7;
|
|
x = 0.5f * num7;
|
|
y = (m01 + m10) * num4;
|
|
z = (m02 + m20) * num4;
|
|
w = (m12 - m21) * num4;
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
if (m11 > m22) {
|
|
float num6 = MathF.Sqrt(((1 + m11) - m00) - m22);
|
|
float num3 = 0.5F / num6;
|
|
x = (m10 + m01) * num3;
|
|
y = 0.5F * num6;
|
|
z = (m21 + m12) * num3;
|
|
w = (m20 - m02) * num3;
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
float num5 = MathF.Sqrt(((1 + m22) - m00) - m11);
|
|
float num2 = 0.5F / num5;
|
|
x = (m20 + m02) * num2;
|
|
y = (m21 + m12) * num2;
|
|
z = 0.5F * num5;
|
|
w = (m01 - m10) * num2;
|
|
return new Quaternion(x, y, z, w);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion with the given forward direction with up =
|
|
/// Vector3::up
|
|
/// </summary>
|
|
/// <param name="forward">The look direction</param>
|
|
/// <returns>The rotation for this direction</returns>
|
|
/// For the rotation, Vector::up is used for the up direction.
|
|
/// Note: if the forward direction == Vector3::up, the result is
|
|
/// Quaternion::identity
|
|
public static Quaternion LookRotation(Vector3Float forward) {
|
|
Vector3Float up = new(0, 1, 0);
|
|
return LookRotation(forward, up);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculat the rotation from on vector to another
|
|
/// </summary>
|
|
/// <param name="fromDirection">The from direction</param>
|
|
/// <param name="toDirection">The to direction</param>
|
|
/// <returns>The rotation from the first to the second vector</returns>
|
|
public static Quaternion FromToRotation(Vector3Float fromDirection, Vector3Float toDirection) {
|
|
Vector3Float axis = Vector3Float.Cross(fromDirection, toDirection);
|
|
axis = axis.normalized;
|
|
AngleFloat angle = Vector3Float.SignedAngle(fromDirection, toDirection, axis);
|
|
Quaternion rotation = AngleAxis(angle, axis);
|
|
return rotation;
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotate form one orientation to anther with a maximum amount of degrees
|
|
/// </summary>
|
|
/// <param name="from">The from rotation</param>
|
|
/// <param name="to">The destination rotation</param>
|
|
/// <param name="maxDegreesDelta">The maximum amount of degrees to
|
|
/// rotate</param> <returns>The possibly limited rotation</returns>
|
|
public static Quaternion RotateTowards(Quaternion from, Quaternion to,
|
|
float maxDegreesDelta) {
|
|
float num = Quaternion.UnsignedAngle(from, to);
|
|
if (num == 0) {
|
|
return to;
|
|
}
|
|
float t = MathF.Min(1, maxDegreesDelta / num);
|
|
return SlerpUnclamped(from, to, t);
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert an angle/axis representation to a quaternion
|
|
/// </summary>
|
|
/// <param name="angle">The angle</param>
|
|
/// <param name="axis">The axis</param>
|
|
/// <returns>The resulting quaternion</returns>
|
|
public static Quaternion AngleAxis(AngleFloat angle, Vector3Float axis) {
|
|
if (axis.sqrMagnitude == 0.0f)
|
|
return Quaternion.identity;
|
|
|
|
float radians = angle.inRadians;
|
|
radians *= 0.5f;
|
|
|
|
Vector3Float axis2 = axis * MathF.Sin(radians);
|
|
float x = axis2.horizontal; // x;
|
|
float y = axis2.vertical; // y;
|
|
float z = axis2.depth; // z;
|
|
float w = MathF.Cos(radians);
|
|
|
|
return new Quaternion(x, y, z, w).normalized;
|
|
}
|
|
/// <summary>
|
|
/// Convert this quaternion to angle/axis representation
|
|
/// </summary>
|
|
/// <param name="angle">A pointer to the angle for the result</param>
|
|
/// <param name="axis">A pointer to the axis for the result</param>
|
|
public readonly void ToAngleAxis(out AngleFloat angle, out Vector3Float axis) {
|
|
Quaternion q1 = (MathF.Abs(this.w) > 1.0f) ? this.normalized : this;
|
|
angle = AngleFloat.Radians(2.0f * MathF.Acos(q1.w)); // angle
|
|
float den = MathF.Sqrt(1.0F - q1.w * q1.w);
|
|
if (den > 0.0001f) {
|
|
axis = Vector3Float.Normalize(q1.xyz / den);
|
|
}
|
|
else {
|
|
// This occurs when the angle is zero.
|
|
// Not a problem: just set an arbitrary normalized axis.
|
|
axis = Vector3Float.right;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the angle between two orientations
|
|
/// </summary>
|
|
/// <param name="orientation1">The first orientation</param>
|
|
/// <param name="orientation2">The second orientation</param>
|
|
/// <returns>The smallest angle in degrees between the two
|
|
/// orientations</returns>
|
|
public static float UnsignedAngle(Quaternion q1, Quaternion q2) {
|
|
// float f = Dot(q1, q2);
|
|
// return MathF.Acos(MathF.Min(MathF.Abs(f), 1)) * 2 * AngleFloat.Rad2Deg;
|
|
|
|
float dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
|
|
dot = MathF.Min(MathF.Max(dot, -1f), 1f);
|
|
return 2f * MathF.Acos(MathF.Abs(dot)) * (180f / MathF.PI);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sherical lerp between two rotations
|
|
/// </summary>
|
|
/// <param name="rotation1">The first rotation</param>
|
|
/// <param name="rotation2">The second rotation</param>
|
|
/// <param name="factor">The factor between 0 and 1.</param>
|
|
/// <returns>The resulting rotation</returns>
|
|
/// A factor 0 returns rotation1, factor1 returns rotation2.
|
|
public static Quaternion Slerp(Quaternion a,
|
|
Quaternion b, float t) {
|
|
if (t > 1)
|
|
t = 1;
|
|
if (t < 0)
|
|
t = 0;
|
|
return SlerpUnclamped(a, b, t);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Unclamped sherical lerp between two rotations
|
|
/// </summary>
|
|
/// <param name="rotation1">The first rotation</param>
|
|
/// <param name="rotation2">The second rotation</param>
|
|
/// <param name="factor">The factor</param>
|
|
/// <returns>The resulting rotation</returns>
|
|
/// A factor 0 returns rotation1, factor1 returns rotation2.
|
|
/// Values outside the 0..1 range will result in extrapolated rotations
|
|
public static Quaternion SlerpUnclamped(Quaternion a,
|
|
Quaternion b, float t) {
|
|
// if either input is zero, return the other.
|
|
if (a.sqrMagnitude == 0.0f) {
|
|
if (b.sqrMagnitude == 0.0f) {
|
|
return identity;
|
|
}
|
|
return b;
|
|
}
|
|
else if (b.sqrMagnitude == 0.0f) {
|
|
return a;
|
|
}
|
|
|
|
Vector3Float axyz = a.xyz;
|
|
Vector3Float bxyz = b.xyz;
|
|
float cosHalfAngle = a.w * b.w + Vector3Float.Dot(axyz, bxyz);
|
|
|
|
Quaternion b2 = b;
|
|
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
|
|
// angle = 0.0f, so just return one input.
|
|
return a;
|
|
}
|
|
else if (cosHalfAngle < 0.0f) {
|
|
b2.x = -b.x;
|
|
b2.y = -b.y;
|
|
b2.z = -b.z;
|
|
b2.w = -b.w;
|
|
cosHalfAngle = -cosHalfAngle;
|
|
}
|
|
|
|
float blendA;
|
|
float blendB;
|
|
if (cosHalfAngle < 0.99f) {
|
|
// do proper slerp for big angles
|
|
float halfAngle = MathF.Acos(cosHalfAngle);
|
|
float sinHalfAngle = MathF.Sin(halfAngle);
|
|
float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
|
|
blendA = MathF.Sin(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
|
|
blendB = MathF.Sin(halfAngle * t) * oneOverSinHalfAngle;
|
|
}
|
|
else {
|
|
// do lerp if angle is really small.
|
|
blendA = 1.0f - t;
|
|
blendB = t;
|
|
}
|
|
Vector3Float v = axyz * blendA + b2.xyz * blendB;
|
|
Quaternion result =
|
|
new(v.horizontal, v.vertical, v.depth, blendA * a.w + blendB * b2.w);
|
|
if (result.sqrMagnitude > 0.0f)
|
|
return result.normalized;
|
|
else
|
|
return Quaternion.identity;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert this quaternion to angle/axis representation
|
|
/// </summary>
|
|
/// <param name="angle">A pointer to the angle for the result</param>
|
|
/// <param name="axis">A pointer to the axis for the result</param>
|
|
public readonly void ToAngleAxis(out float angle, out Vector3Float axis) {
|
|
ToAxisAngleRad(this, out axis, out angle);
|
|
angle *= AngleFloat.Rad2Deg;
|
|
}
|
|
private static void ToAxisAngleRad(Quaternion q,
|
|
out Vector3Float axis,
|
|
out float angle) {
|
|
Quaternion q1 = (MathF.Abs(q.w) > 1.0f) ? Quaternion.Normalize(q) : q;
|
|
angle = 2.0f * MathF.Acos(q1.w); // angle
|
|
float den = MathF.Sqrt(1.0F - q1.w * q1.w);
|
|
if (den > 0.0001f) {
|
|
axis = (q1.xyz / den).normalized;
|
|
}
|
|
else {
|
|
// This occurs when the angle is zero.
|
|
// Not a problem: just set an arbitrary normalized axis.
|
|
axis = new Vector3Float(1, 0, 0);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the angle of around the give axis for a rotation
|
|
/// </summary>
|
|
/// <param name="axis">The axis around which the angle should be
|
|
/// computed</param> <param name="rotation">The source rotation</param>
|
|
/// <returns>The signed angle around the axis</returns>
|
|
public static float GetAngleAround(Vector3Float axis, Quaternion rotation) {
|
|
Quaternion secondaryRotation = GetRotationAround(axis, rotation);
|
|
secondaryRotation.ToAngleAxis(out float rotationAngle, out Vector3Float rotationAxis);
|
|
|
|
// Do the axis point in opposite directions?
|
|
if (Vector3Float.Dot(axis, rotationAxis) < 0)
|
|
rotationAngle = -rotationAngle;
|
|
|
|
return rotationAngle;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns the rotation limited around the given axis
|
|
/// </summary>
|
|
/// <param name="axis">The axis which which the rotation should be
|
|
/// limited</param> <param name="rotation">The source rotation</param>
|
|
/// <returns>The rotation around the given axis</returns>
|
|
public static Quaternion GetRotationAround(Vector3Float axis, Quaternion rotation) {
|
|
Vector3Float ra = new(rotation.x, rotation.y, rotation.z); // rotation axis
|
|
Vector3Float p = Vector3Float.Project(
|
|
ra, axis); // return projection ra on to axis (parallel component)
|
|
Quaternion twist = new(p.horizontal, p.vertical, p.depth, rotation.w);
|
|
twist = Normalize(twist);
|
|
return twist;
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// Swing-twist decomposition of a rotation
|
|
/// </summary>
|
|
/// <param name="axis">The base direction for the decomposition</param>
|
|
/// <param name="q">The source rotation</param>
|
|
/// <param name="swing">A pointer to the quaternion for the swing
|
|
/// result</param> <param name="twist">A pointer to the quaternion for the
|
|
/// twist result</param>
|
|
static void GetSwingTwist(Vector3Float axis, Quaternion q,
|
|
out Quaternion swing, out Quaternion twist) {
|
|
twist = GetRotationAround(axis, q);
|
|
swing = q * Inverse(twist);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the dot product of two quaternions
|
|
/// </summary>
|
|
/// <param name="rotation1">The first rotation</param>
|
|
/// <param name="rotation2">The second rotation</param>
|
|
/// <returns></returns>
|
|
public static float Dot(Quaternion q1, Quaternion q2) {
|
|
return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
} |