public static double getAngle ( Matrix4d transform ) { // Calculate angle double c = ( transform . m00 + transform . m11 + transform . m22 - 1 ) / 2.0 ; //=cos(theta) // c is sometimes slightly out of the [-1,1] range due to numerical instabilities if ( - 1 - 1e-8 < c && c < - 1 ) c = - 1 ; if ( 1 + 1e-8 > c && c > 1 ) c = 1 ; if ( - 1 > c || c > 1 ) { throw new IllegalArgumentException ( "Input matrix is not a valid rotation matrix." ) ; } return Math . acos ( c ) ; }
Quickly compute the rotation angle from a rotation matrix .