/* From Quaternion to Matrix and Back
http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
*/
void
RQuatRotationMatrix( RQuat* out, const struct RMtx4* mtx )
{
#define I( r, c ) RMtx4GetElement( mtx, (r), (c) )
/* assumes:
- mtx represents rotation (contains no scaling factor)
- I(3,3) == 1.0f
*/
rmReal diag00 = I( 0, 0 );
rmReal diag11 = I( 1, 1 );
rmReal diag22 = I( 2, 2 );
if ( diag00 + diag11 + diag22 > 0.0f )
{
rmReal t = diag00 + diag11 + diag22 + 1.0f;
rmReal s = 1.0f / ( rmSqrt( t ) * 2.0f );
RQuatSetW( out, s * t );
RQuatSetZ( out, (I(1,0) - I(0,1)) * s );
RQuatSetY( out, (I(0,2) - I(2,0)) * s );
RQuatSetX( out, (I(2,1) - I(1,2)) * s );
}
else if ( diag00 > diag11 && diag00 > diag22 )
{
rmReal t = diag00 - diag11 - diag22 + 1.0f;
rmReal s = 1.0f / ( rmSqrt( t ) * 2.0f );
RQuatSetX( out, s * t );
RQuatSetY( out, (I(1,0) + I(0,1)) * s );
RQuatSetZ( out, (I(0,2) + I(2,0)) * s );
RQuatSetW( out, (I(2,1) - I(1,2)) * s );
}
else if ( diag11 > diag22 )
{
rmReal t = -diag00 + diag11 - diag22 + 1.0f;
rmReal s = 1.0f / ( rmSqrt( t ) * 2.0f );
RQuatSetY( out, s * t );
RQuatSetX( out, (I(1,0) + I(0,1)) * s );
RQuatSetW( out, (I(0,2) - I(2,0)) * s );
RQuatSetZ( out, (I(2,1) + I(1,2)) * s );
}
else
{
rmReal t = -diag00 - diag11 + diag22 + 1.0f;
rmReal s = 1.0f / ( rmSqrt( t ) * 2.0f );
RQuatSetZ( out, s * t );
RQuatSetW( out, (I(1,0) - I(0,1)) * s );
RQuatSetX( out, (I(0,2) + I(2,0)) * s );
RQuatSetY( out, (I(2,1) + I(1,2)) * s );
}
#undef I
}
rmReal
RVec4Length( const RVec4* in )
{
return rmSqrt( in->x*in->x + in->y*in->y + in->z*in->z + in->w*in->w );
}
rmReal
RQuatLength( const RQuat* in )
{
return rmSqrt( in->x*in->x + in->y*in->y + in->z*in->z + in->w*in->w );
}
rmReal
RVec2Length( const RVec2* in )
{
return rmSqrt( in->x*in->x + in->y*in->y );
}