dVec3 dMatrix::getHPR()
{
dMatrix mat( *this );
mat( 3, 0 ) = 0.0f;
mat( 3, 1 ) = 0.0f;
mat( 3, 2 ) = 0.0f;
dVec3 hpr;
dVec3 diry = mat * dVec3( 0.0f, 1.0f, 0.0f ), dirz = mat * dVec3( 0.0f, 0.0f, 1.0f );
diry.normalize();
dirz.normalize();
if ( ABS( diry.x() ) < 1.0e-6 && ABS( diry.y() ) < 1.0e-6 )
{
hpr.h() = 0.0f;
hpr.p() = 90.0f * diry.z();
}
else
{
hpr.h() = RAD2DEG( atan2( diry.x(), diry.y() ) );
float length = sqrt( diry.x() * diry.x() + diry.y() * diry.y() );
if ( length < 1.0e-6 )
hpr.p() = RAD2DEG( atan2( diry.z(), length ) );
else
hpr.p() = 90.0f * diry.z();
}
dVec3 temp = diry ^ dVec3( 0.0f, 0.0f, 1.0f );//x'' = y' X z ( cross product )
temp.normalize();
dVec3 temp2 = temp ^ diry;//z'' = x'' X y' ( cross product )
temp.normalize();
float dotprod = temp2 * dirz;//dot product
if ( dotprod > 1.0f )
dotprod = 1.0f;
else if ( dotprod < -1.0f )
dotprod = -1.0f;
hpr.r() = RAD2DEG( acos( dotprod ) );
return hpr;
}