void Angle::fromMatrix(const Matrix3d& matrix)
{
assert(matrix.x() == 3 && matrix.y() == 3);
double forward[3];
double left[3];
double up[3];
forward[0] = matrix[0][0];
forward[1] = matrix[1][0];
forward[2] = matrix[2][0];
left[0] = matrix[0][1];
left[1] = matrix[1][1];
left[2] = matrix[2][1];
up[2] = matrix[2][2];
double xyDist = sqrt(forward[0] * forward[0] + forward[1] * forward[1]);
if (xyDist > 0.0001)
{
(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
(*this)[YAW] = RAD2DEG( atan2( forward[1], forward[0] ) );
(*this)[ROLL] = RAD2DEG( atan2( left[2], up[2] ) );
}
else
{ // gimbal lock
(*this)[PITCH] = RAD2DEG( atan2( -forward[2], xyDist ) );
(*this)[YAW] = RAD2DEG( atan2( -left[0], left[1] ) );
(*this)[ROLL] = 0;
}
}
