/// <summary>
/// Causes a rotation <paramref name="angle"/> degs around <paramref name="axis"/>
/// </summary>
/// <param name="angle">Angle to rotate</param>
/// <param name="axis">Rotation axis</param>
/// <returns>A rotated Quaternion</returns>
public static THQuaternion AngleAxis(float angle, ref THVector3 axis)
{
if ((axis.magnitude * axis.magnitude) == 0)
{
return(identity);
}
var result = identity;
var rads = angle * degToRad;
rads *= 0.5f;
axis.Normalize();
axis = axis * (float)Sin(rads);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = (float)Cos(rads);
return(Normalize(result));
}
// 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);
}
