main/src/georegression/transform/twist/TwistOps_F64.java
public static TwistCoordinate_F64 twist( Se3_F64 motion , TwistCoordinate_F64 twist ) {
		if( twist == null )
			twist = new TwistCoordinate_F64();

		if(MatrixFeatures_DDRM.isIdentity(motion.R, GrlConstants.TEST_F64)) {
			twist.w.set(0,0,0);
			twist.v.set(motion.T);
		} else {
			Rodrigues_F64 rod = new Rodrigues_F64();
			ConvertRotation3D_F64.matrixToRodrigues(motion.R,rod);

			twist.w.set(rod.unitAxisRotation);
			double theta = rod.theta;

			// A = (I-SO)*hat(w) + w*w'*theta
			DMatrixRMaj A = CommonOps_DDRM.identity(3);
			CommonOps_DDRM.subtract(A,motion.R, A);

			DMatrixRMaj w_hat = GeometryMath_F64.crossMatrix(twist.w,null);
			DMatrixRMaj tmp = A.copy();
			CommonOps_DDRM.mult(tmp,w_hat,A);

			Vector3D_F64 w = twist.w;
			A.data[0] += w.x*w.x*theta; A.data[1] += w.x*w.y*theta; A.data[2] += w.x*w.z*theta;
			A.data[3] += w.y*w.x*theta; A.data[4] += w.y*w.y*theta; A.data[5] += w.y*w.z*theta;
			A.data[6] += w.z*w.x*theta; A.data[7] += w.z*w.y*theta; A.data[8] += w.z*w.z*theta;

			DMatrixRMaj y = new DMatrixRMaj(3,1);
			y.data[0] = motion.T.x;
			y.data[1] = motion.T.y;
			y.data[2] = motion.T.z;

			DMatrixRMaj x = new DMatrixRMaj(3,1);

			CommonOps_DDRM.solve(A,y,x);

			twist.w.scale(rod.theta);
			twist.v.x = (double) x.data[0];
			twist.v.y = (double) x.data[1];
			twist.v.z = (double) x.data[2];
			twist.v.scale(rod.theta);
		}
		return twist;
	}
public static TwistCoordinate_F64 twist( Se3_F64 motion , TwistCoordinate_F64 twist ) {
		if( twist == null )
			twist = new TwistCoordinate_F64();

		if(MatrixFeatures_DDRM.isIdentity(motion.R, GrlConstants.TEST_F64)) {
			twist.w.set(0,0,0);
			twist.v.set(motion.T);
		} else {
			Rodrigues_F64 rod = new Rodrigues_F64();
			ConvertRotation3D_F64.matrixToRodrigues(motion.R,rod);

			twist.w.set(rod.unitAxisRotation);
			double theta = rod.theta;

			// A = (I-SO)*hat(w) + w*w'*theta
			DMatrixRMaj A = CommonOps_DDRM.identity(3);
			CommonOps_DDRM.subtract(A,motion.R, A);

			DMatrixRMaj w_hat = GeometryMath_F64.crossMatrix(twist.w,null);
			DMatrixRMaj tmp = A.copy();
			CommonOps_DDRM.mult(tmp,w_hat,A);

			Vector3D_F64 w = twist.w;
			A.data[0] += w.x*w.x*theta; A.data[1] += w.x*w.y*theta; A.data[2] += w.x*w.z*theta;
			A.data[3] += w.y*w.x*theta; A.data[4] += w.y*w.y*theta; A.data[5] += w.y*w.z*theta;
			A.data[6] += w.z*w.x*theta; A.data[7] += w.z*w.y*theta; A.data[8] += w.z*w.z*theta;

			DMatrixRMaj y = new DMatrixRMaj(3,1);
			y.data[0] = motion.T.x;
			y.data[1] = motion.T.y;
			y.data[2] = motion.T.z;

			DMatrixRMaj x = new DMatrixRMaj(3,1);

			CommonOps_DDRM.solve(A,y,x);

			twist.w.scale(rod.theta);
			twist.v.x = (double) x.data[0];
			twist.v.y = (double) x.data[1];
			twist.v.z = (double) x.data[2];
			twist.v.scale(rod.theta);
		}
		return twist;
	}
Converts a rigid body motion into a twist coordinate.  The value of theta used to generate the motion
is assumed to be one.

@param motion (Input) The SE(3) transformation
@param twist (Output) Storage for twist.
@return magnitude of the motion