main/src/georegression/geometry/ConvertRotation3D_F64.java
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;
	}
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;
	}
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.

@param R (Input) rotation matrix
@param quat (Output) Optional storage for quaternion.  If null a new class will be used.
@return unit quaternion representation of the rotation matrix.
Extracts quaternions from the provided rotation matrix .