|
public static Normalize ( |
||
| q | ||
| return | ||
public static Quaternion Normalize( Quaternion q )
{
float lenSq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
if( lenSq > IKEpsilon ) {
if( lenSq >= 1.0f - IKEpsilon && lenSq <= 1.0 + IKEpsilon ) {
return q;
} else {
float s = 1.0f / Mathf.Sqrt( lenSq );
return new Quaternion( q.x * s, q.y * s, q.z * s, q.w * s );
}
}
return q; // Failsafe.
}