/// <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); }