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;
	}
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
Converts a rigid body motion into a twist coordinate . The value of theta used to generate the motion is assumed to be one .