public static Quaternion_F64 matrixToQuaternion ( DMatrixRMaj R , Quaternion_F64 quat ) { if ( quat == null ) quat = new Quaternion_F64 ( ) ; // algorithm from: // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ // // Designed to minimize numerical error by not dividing by very small numbers double m00 = R . unsafe_get ( 0 , 0 ) ; double m01 = R . unsafe_get ( 0 , 1 ) ; double m02 = R . unsafe_get ( 0 , 2 ) ; double m10 = R . unsafe_get ( 1 , 0 ) ; double m11 = R . unsafe_get ( 1 , 1 ) ; double m12 = R . unsafe_get ( 1 , 2 ) ; double m20 = R . unsafe_get ( 2 , 0 ) ; double m21 = R . unsafe_get ( 2 , 1 ) ; double m22 = R . unsafe_get ( 2 , 2 ) ; double trace = m00 + m11 + m22 ; if ( trace > 0 ) { double S = Math . sqrt ( trace + 1.0 ) * 2 ; // S=4*qw quat . w = 0.25 * S ; quat . x = ( m21 - m12 ) / S ; quat . y = ( m02 - m20 ) / S ; quat . z = ( m10 - m01 ) / S ; } else if ( ( m00 > m11 ) & ( m00 > m22 ) ) { double S = Math . sqrt ( 1.0 + m00 - m11 - m22 ) * 2 ; // S=4*qx quat . w = ( m21 - m12 ) / S ; quat . x = 0.25 * S ; quat . y = ( m01 + m10 ) / S ; quat . z = ( m02 + m20 ) / S ; } else if ( m11 > m22 ) { double S = Math . sqrt ( 1.0 + m11 - m00 - m22 ) * 2 ; // S=4*qy quat . w = ( m02 - m20 ) / S ; quat . x = ( m01 + m10 ) / S ; quat . y = 0.25 * S ; quat . z = ( m12 + m21 ) / S ; } else { double S = Math . sqrt ( 1.0 + m22 - m00 - m11 ) * 2 ; // S=4*qz quat . w = ( m10 - m01 ) / S ; quat . x = ( m02 + m20 ) / S ; quat . y = ( m12 + m21 ) / S ; quat . z = 0.25 * S ; } return quat ; }
Extracts quaternions from the provided rotation matrix .