public static Quaternion FromRotationMatrix( Matrix3 matrix )
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
Quaternion result = Quaternion.Zero;
Real trace = matrix.m00 + matrix.m11 + matrix.m22;
Real root = 0.0f;
if ( trace > 0.0f )
{
// |this.w| > 1/2, may as well choose this.w > 1/2
root = Utility.Sqrt( trace + 1.0f ); // 2w
result.w = 0.5f * root;
root = 0.5f / root; // 1/(4w)
result.x = ( matrix.m21 - matrix.m12 ) * root;
result.y = ( matrix.m02 - matrix.m20 ) * root;
result.z = ( matrix.m10 - matrix.m01 ) * root;
}
else
{
// |result.w| <= 1/2
int i = 0;
if ( matrix.m11 > matrix.m00 )
i = 1;
if ( matrix.m22 > matrix[ i, i ] )
i = 2;
int j = next[ i ];
int k = next[ j ];
root = Utility.Sqrt( matrix[ i, i ] - matrix[ j, j ] - matrix[ k, k ] + 1.0f );
unsafe
{
Real* apkQuat = &result.x;
apkQuat[ i ] = 0.5f * root;
root = 0.5f / root;
result.w = ( matrix[ k, j ] - matrix[ j, k ] ) * root;
apkQuat[ j ] = ( matrix[ j, i ] + matrix[ i, j ] ) * root;
apkQuat[ k ] = ( matrix[ k, i ] + matrix[ i, k ] ) * root;
}
}
return result;
}