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; } }