// from http://stackoverflow.com/questions/12088610/conversion-between-euler-quaternion-like-in-unity3d-engine
private static THVector3 ToEulerRad(THQuaternion rotation)
{
float sqw = rotation.w * rotation.w;
float sqx = rotation.x * rotation.x;
float sqy = rotation.y * rotation.y;
float sqz = rotation.z * rotation.z;
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
float test = rotation.x * rotation.w - rotation.y * rotation.z;
THVector3 v;
if (test > 0.4995f * unit)
{ // singularity at north pole
v.y = 2f * Atan2(rotation.y, rotation.x);
v.x = UnityEngine.Mathf.PI / 2;
v.z = 0;
return(NormalizeAngles(v * Rad2Deg));
}
if (test < -0.4995f * unit)
{ // singularity at south pole
v.y = -2f * Atan2(rotation.y, rotation.x);
v.x = -UnityEngine.Mathf.PI / 2;
v.z = 0;
return(NormalizeAngles(v * Rad2Deg));
}
THQuaternion q = new THQuaternion(rotation.w, rotation.z, rotation.x, rotation.y);
v.y = (float)System.Math.Atan2(2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
v.x = (float)System.Math.Asin(2f * (q.x * q.z - q.w * q.y)); // Pitch
v.z = (float)System.Math.Atan2(2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
return(NormalizeAngles(v * Rad2Deg));
}
private static THQuaternion SlerpUnclamped(ref THQuaternion a, ref THQuaternion b, float t)
{
// if either input is zero, return the other.
if (a.lengthSquared == 0.0f)
{
if (b.lengthSquared == 0.0f)
{
return(identity);
}
return(b);
}
else if (b.lengthSquared == 0.0f)
{
return(a);
}
float cosHalfAngle = a.w * b.w + THVector3.Dot(a.xyz, b.xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(a);
}
else if (cosHalfAngle < 0.0f)
{
b.xyz = -b.xyz;
b.w = -b.w;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
float halfAngle = (float)Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Math.Sin(halfAngle * (1.0f - t)) * oneOverSinHalfAngle;
blendB = (float)Math.Sin(halfAngle * t) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - t;
blendB = t;
}
THQuaternion result = new THQuaternion(a.xyz * blendA + b.xyz * blendB, blendA * a.w + blendB * b.w);
if (result.lengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(identity);
}
}
/// <summary>
/// Set Quaternion values<para />
/// XYZ must be in degs
/// </summary>
/// <param name="xyz">XYZ Angles</param>
public static THQuaternion CreateAndSetFromVec(THVector3 xyz)
{
var q = new THQuaternion();
q.Set(xyz);
return(q);
}
/// <summary>
/// Normalize a Quaternion
/// </summary>
/// <param name="quaternion"></param>
/// <returns></returns>
public static THQuaternion Normalize(THQuaternion quaternion)
{
var scale = 1.0f / quaternion.length;
quaternion.xyz *= scale;
quaternion.w *= scale;
return(quaternion);
}
public override bool Equals(object other)
{
if (!(other is THQuaternion))
{
return(false);
}
THQuaternion quaternion = (THQuaternion)other;
return(this.x.Equals(quaternion.x) && this.y.Equals(quaternion.y) && this.z.Equals(quaternion.z) && this.w.Equals(quaternion.w));
}
/// <summary>
/// Retuens the invenrse of a <paramref name="rotation"/>
/// </summary>
/// <param name="rotation">Rotation</param>
/// <returns>Inverted rotation</returns>
public static THQuaternion Inverse(THQuaternion rotation)
{
float lengthSq = rotation.lengthSquared;
if (lengthSq != 0.0)
{
float i = 1.0f / lengthSq;
return(new THQuaternion(rotation.xyz * -i, rotation.w * i));
}
return(rotation);
}
/// <summary>
/// Interpolates between <paramref name="a"/> and <paramref name="b"/> by <paramref name="t"/> then Normalizes the result. <paramref name="t"/> is clampled from 0-1
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">Second Quaternion</param>
/// <param name="t">Interpolation steps</param>
/// <returns>Lerped Quaternion</returns>
public static THQuaternion Lerp(THQuaternion a, THQuaternion b, float t)
{
if (t > 1)
{
t = 1;
}
if (t < 0)
{
t = 0;
}
return(Slerp(ref a, ref b, t)); // TODO: use lerp not slerp, "Because quaternion works in 4D. Rotation in 4D are linear" ???
}
/// <summary>
/// Rotates from <paramref name="a"/> towards <paramref name="b"/> by a maximum angle <paramref name="maxDegreesDelta"/>
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">Second Quaternion</param>
/// <param name="maxDegreesDelta">Max change in angle</param>
public static THQuaternion RotateTowards(THQuaternion a, THQuaternion b, float maxDegreesDelta)
{
float num = THQuaternion.Angle(a, b);
if (num == 0f)
{
return(b);
}
float t = Math.Min(1f, maxDegreesDelta / num);
return(THQuaternion.SlerpUnclamped(a, b, t));
}
private static void ToAxisAngleRad(THQuaternion q, out THVector3 axis, out float angle)
{
if (Math.Abs(q.w) > 1.0f)
{
q.Normalize();
}
angle = 2.0f * (float)Math.Acos(q.w); // angle
float den = (float)Sqrt(1.0 - q.w * q.w);
if (den > 0.0001f)
{
axis = q.xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
axis = new THVector3(1, 0, 0);
}
}
public bool Equals(THQuaternion other)
{
return(this.x.Equals(other.x) && this.y.Equals(other.y) && this.z.Equals(other.z) && this.w.Equals(other.w));
}
/// <summary>
/// Produce a normalized version of an input Quaternion
/// </summary>
/// <param name="q">Input Quaternion</param>
/// <param name="result">Output Quaternion</param>
public static void Normalize(ref THQuaternion q, out THQuaternion result)
{
var scale = 1f / q.length;
result = new THQuaternion(q.xyz * scale, q.w * scale);
}
/// <summary>
/// Produce a Dot procude of the Quaternion
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">Second Quaternion</param>
/// <returns></returns>
public static float Dot(THQuaternion a, THQuaternion b)
{
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}
/// <summary>
/// Returns the angle in degrees between two rotations <paramref name="a"/> and <paramref name="b"/>
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">First Quaternion</param>
public static float Angle(THQuaternion a, THQuaternion b)
{
float f = THQuaternion.Dot(a, b);
return(Acos(UnityEngine.Mathf.Min(UnityEngine.Mathf.Abs(f), 1f)) * 2f * radToDeg);
}
// from http://answers.unity3d.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
/// <summary>
/// Creates a rotation with the specified <paramref name="forward"/> and <paramref name="up"/> directions
/// </summary>
/// <param name="forward">Forward direction</param>
/// <param name="up">Upward direction</param>
/// <returns></returns>
private static THQuaternion LookRotation(ref THVector3 forward, ref THVector3 up)
{
// Magic
forward = THVector3.Normalize(forward);
THVector3 right = THVector3.Normalize(THVector3.Cross(up, forward));
up = THVector3.Cross(forward, right);
var m00 = right.x;
var m01 = right.y;
var m02 = right.z;
var m10 = up.x;
var m11 = up.y;
var m12 = up.z;
var m20 = forward.x;
var m21 = forward.y;
var m22 = forward.z;
float num8 = (m00 + m11) + m22;
var quaternion = new THQuaternion();
if (num8 > 0f)
{
var num = (float)System.Math.Sqrt(num8 + 1f);
quaternion.w = num * 0.5f;
num = 0.5f / num;
quaternion.x = (m12 - m21) * num;
quaternion.y = (m20 - m02) * num;
quaternion.z = (m01 - m10) * num;
return(quaternion);
}
if ((m00 >= m11) && (m00 >= m22))
{
var num7 = (float)System.Math.Sqrt(((1f + m00) - m11) - m22);
var num4 = 0.5f / num7;
quaternion.x = 0.5f * num7;
quaternion.y = (m01 + m10) * num4;
quaternion.z = (m02 + m20) * num4;
quaternion.w = (m12 - m21) * num4;
return(quaternion);
}
if (m11 > m22)
{
var num6 = (float)System.Math.Sqrt(((1f + m11) - m00) - m22);
var num3 = 0.5f / num6;
quaternion.x = (m10 + m01) * num3;
quaternion.y = 0.5f * num6;
quaternion.z = (m21 + m12) * num3;
quaternion.w = (m20 - m02) * num3;
return(quaternion);
}
var num5 = (float)System.Math.Sqrt(((1f + m22) - m00) - m11);
var num2 = 0.5f / num5;
quaternion.x = (m20 + m02) * num2;
quaternion.y = (m21 + m12) * num2;
quaternion.z = 0.5f * num5;
quaternion.w = (m01 - m10) * num2;
return(quaternion);
}
/// <summary>
/// Interpolates between <paramref name="a"/> and <paramref name="b"/> by <paramref name="t"/> and normalizes the result afterwards. The parameter <paramref name="t"/> is not clamped
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">Second Quaternion</param>
/// <param name="t">Interpolation steps</param>
/// <remarks>Lerped Quaternion</remarks>
public static THQuaternion LerpUnclamped(THQuaternion a, THQuaternion b, float t)
{
return(Slerp(ref a, ref b, t));
}
/// <summary>
/// Spherically interpolates between <paramref name="a"/> and <paramref name="b"/> by <paramref name="t"/>. The parameter <paramref name="t"/> is clamped to the range [0, 1].</para>
/// </summary>
/// <param name="a">First Quaternion</param>
/// <param name="b">Second Quaternion</param>
/// <param name="t">Steps</param>
/// <returns>Slerped Quaternion</returns>
public static THQuaternion Slerp(THQuaternion a, THQuaternion b, float t)
{
return(Slerp(ref a, ref b, t));
}
private static THQuaternion Slerp(ref THQuaternion a, ref THQuaternion b, float t)
{
return(SlerpUnclamped(ref a, ref b, Clamp01(t)));
}
