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