main/src/georegression/geometry/ConvertRotation3D_F64.java
public static Rodrigues_F64 matrixToRodrigues( DMatrixRMaj R, Rodrigues_F64 rodrigues ) {
		if( rodrigues == null ) {
			rodrigues = new Rodrigues_F64();
		}
		// parts of this are from wikipedia
		// http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle

		double diagSum = ( (R.unsafe_get( 0, 0 ) + R.unsafe_get( 1, 1 ) + R.unsafe_get( 2, 2 )) - 1.0 ) / 2.0;

		double absDiagSum = Math.abs(diagSum);
		
		if( absDiagSum <= 1.0 && 1.0-absDiagSum > 10.0*GrlConstants.EPS ) {
			// if numerically stable use a faster technique
			rodrigues.theta = Math.acos(diagSum);
			double bottom = 2.0 * Math.sin(rodrigues.theta);

			// in cases where bottom is close to zero that means theta is also close to zero and the vector
			// doesn't matter that much
			rodrigues.unitAxisRotation.x = (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
			rodrigues.unitAxisRotation.y = (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
			rodrigues.unitAxisRotation.z = (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;

			// in extreme underflow situations the result can be unnormalized
			rodrigues.unitAxisRotation.normalize();

			// In theory this might be more stable
			// rotationAxis( R, rodrigues.unitAxisRotation);
		} else {

			// this handles the special case where the bottom is very very small or equal to zero
			if( diagSum >= 1.0 )
				rodrigues.theta = 0;
			else if( diagSum <= -1.0 )
				rodrigues.theta = Math.PI;
			else
				rodrigues.theta = Math.acos(diagSum);

			// compute the value of x,y,z up to a sign ambiguity
			rodrigues.unitAxisRotation.x = Math.sqrt((R.get(0, 0) + 1) / 2);
			rodrigues.unitAxisRotation.y = Math.sqrt((R.get(1, 1) + 1) / 2);
			rodrigues.unitAxisRotation.z = Math.sqrt((R.get(2, 2) + 1) / 2);

			double x = rodrigues.unitAxisRotation.x;
			double y = rodrigues.unitAxisRotation.y;
			double z = rodrigues.unitAxisRotation.z;

			if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.EPS) {
				x *= -1;
			}
			if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.EPS) {
				z *= -1;
			}
			if (Math.abs(R.get(2,1) - 2 * z * y) > GrlConstants.EPS) {
				y *= -1;
				x *= -1;
			}

			rodrigues.unitAxisRotation.x = x;
			rodrigues.unitAxisRotation.y = y;
			rodrigues.unitAxisRotation.z = z;
		}

		return rodrigues;
	}
public static Rodrigues_F64 matrixToRodrigues( DMatrixRMaj R, Rodrigues_F64 rodrigues ) {
		if( rodrigues == null ) {
			rodrigues = new Rodrigues_F64();
		}
		// parts of this are from wikipedia
		// http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle

		double diagSum = ( (R.unsafe_get( 0, 0 ) + R.unsafe_get( 1, 1 ) + R.unsafe_get( 2, 2 )) - 1.0 ) / 2.0;

		double absDiagSum = Math.abs(diagSum);
		
		if( absDiagSum <= 1.0 && 1.0-absDiagSum > 10.0*GrlConstants.EPS ) {
			// if numerically stable use a faster technique
			rodrigues.theta = Math.acos(diagSum);
			double bottom = 2.0 * Math.sin(rodrigues.theta);

			// in cases where bottom is close to zero that means theta is also close to zero and the vector
			// doesn't matter that much
			rodrigues.unitAxisRotation.x = (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
			rodrigues.unitAxisRotation.y = (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
			rodrigues.unitAxisRotation.z = (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;

			// in extreme underflow situations the result can be unnormalized
			rodrigues.unitAxisRotation.normalize();

			// In theory this might be more stable
			// rotationAxis( R, rodrigues.unitAxisRotation);
		} else {

			// this handles the special case where the bottom is very very small or equal to zero
			if( diagSum >= 1.0 )
				rodrigues.theta = 0;
			else if( diagSum <= -1.0 )
				rodrigues.theta = Math.PI;
			else
				rodrigues.theta = Math.acos(diagSum);

			// compute the value of x,y,z up to a sign ambiguity
			rodrigues.unitAxisRotation.x = Math.sqrt((R.get(0, 0) + 1) / 2);
			rodrigues.unitAxisRotation.y = Math.sqrt((R.get(1, 1) + 1) / 2);
			rodrigues.unitAxisRotation.z = Math.sqrt((R.get(2, 2) + 1) / 2);

			double x = rodrigues.unitAxisRotation.x;
			double y = rodrigues.unitAxisRotation.y;
			double z = rodrigues.unitAxisRotation.z;

			if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.EPS) {
				x *= -1;
			}
			if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.EPS) {
				z *= -1;
			}
			if (Math.abs(R.get(2,1) - 2 * z * y) > GrlConstants.EPS) {
				y *= -1;
				x *= -1;
			}

			rodrigues.unitAxisRotation.x = x;
			rodrigues.unitAxisRotation.y = y;
			rodrigues.unitAxisRotation.z = z;
		}

		return rodrigues;
	}
public static Rodrigues_F64 matrixToRodrigues ( DMatrixRMaj R , Rodrigues_F64 rodrigues ) { if ( rodrigues == null ) { rodrigues = new Rodrigues_F64 ( ) ; } // parts of this are from wikipedia // http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle double diagSum = ( ( R . unsafe_get ( 0 , 0 ) + R . unsafe_get ( 1 , 1 ) + R . unsafe_get ( 2 , 2 ) ) - 1.0 ) / 2.0 ; double absDiagSum = Math . abs ( diagSum ) ; if ( absDiagSum <= 1.0 && 1.0 - absDiagSum > 10.0 * GrlConstants . EPS ) { // if numerically stable use a faster technique rodrigues . theta = Math . acos ( diagSum ) ; double bottom = 2.0 * Math . sin ( rodrigues . theta ) ; // in cases where bottom is close to zero that means theta is also close to zero and the vector // doesn't matter that much rodrigues . unitAxisRotation . x = ( R . unsafe_get ( 2 , 1 ) - R . unsafe_get ( 1 , 2 ) ) / bottom ; rodrigues . unitAxisRotation . y = ( R . unsafe_get ( 0 , 2 ) - R . unsafe_get ( 2 , 0 ) ) / bottom ; rodrigues . unitAxisRotation . z = ( R . unsafe_get ( 1 , 0 ) - R . unsafe_get ( 0 , 1 ) ) / bottom ; // in extreme underflow situations the result can be unnormalized rodrigues . unitAxisRotation . normalize ( ) ; // In theory this might be more stable // rotationAxis( R, rodrigues.unitAxisRotation); } else { // this handles the special case where the bottom is very very small or equal to zero if ( diagSum >= 1.0 ) rodrigues . theta = 0 ; else if ( diagSum <= - 1.0 ) rodrigues . theta = Math . PI ; else rodrigues . theta = Math . acos ( diagSum ) ; // compute the value of x,y,z up to a sign ambiguity rodrigues . unitAxisRotation . x = Math . sqrt ( ( R . get ( 0 , 0 ) + 1 ) / 2 ) ; rodrigues . unitAxisRotation . y = Math . sqrt ( ( R . get ( 1 , 1 ) + 1 ) / 2 ) ; rodrigues . unitAxisRotation . z = Math . sqrt ( ( R . get ( 2 , 2 ) + 1 ) / 2 ) ; double x = rodrigues . unitAxisRotation . x ; double y = rodrigues . unitAxisRotation . y ; double z = rodrigues . unitAxisRotation . z ; if ( Math . abs ( R . get ( 1 , 0 ) - 2 * x * y ) > GrlConstants . EPS ) { x *= - 1 ; } if ( Math . abs ( R . get ( 2 , 0 ) - 2 * x * z ) > GrlConstants . EPS ) { z *= - 1 ; } if ( Math . abs ( R . get ( 2 , 1 ) - 2 * z * y ) > GrlConstants . EPS ) { y *= - 1 ; x *= - 1 ; } rodrigues . unitAxisRotation . x = x ; rodrigues . unitAxisRotation . y = y ; rodrigues . unitAxisRotation . z = z ; } return rodrigues ; }
Converts a rotation matrix into {@link georegression.struct.so.Rodrigues_F64}.

@param R Rotation matrix.
@param rodrigues Storage used for solution.  If null a new instance is declared.
@return The found axis and rotation angle.
Converts a rotation matrix into { @link georegression . struct . so . Rodrigues_F64 } .