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

		double w_norm = twist.w.norm();

		if( w_norm == 0.0 ) {
			CommonOps_DDRM.setIdentity(motion.R);
			motion.T.x = twist.v.x*theta;
			motion.T.y = twist.v.y*theta;
			motion.T.z = twist.v.z*theta;
			return motion;
		}

		DMatrixRMaj R = motion.getR();

		// First handle the SO region.  This Rodrigues equation
		double wx = twist.w.x/w_norm, wy = twist.w.y/w_norm, wz = twist.w.z/w_norm;

		ConvertRotation3D_F64.rodriguesToMatrix(wx,wy,wz, theta*w_norm, R);

		theta *= w_norm;

		// Now compute the translational component
		// (I - SO)*(w cross v) + w*w'*v*theta
		double vx = twist.v.x, vy = twist.v.y, vz = twist.v.z;

		double wv_x = wy*vz - wz*vy;
		double wv_y = wz*vx - wx*vz;
		double wv_z = wx*vy - wy*vx;

		double left_x = (1 - R.data[0])*wv_x -      R.data[1]*wv_y  -      R.data[2]*wv_z;
		double left_y =     -R.data[3]*wv_x  + (1 - R.data[4])*wv_y -      R.data[5]*wv_z;
		double left_z =     -R.data[6]*wv_x  -      R.data[7]*wv_y  + (1 - R.data[8])*wv_z;

		double right_x = (wx*wx*vx + wx*wy*vy + wx*wz*vz)*theta;
		double right_y = (wy*wx*vx + wy*wy*vy + wy*wz*vz)*theta;
		double right_z = (wz*wx*vx + wz*wy*vy + wz*wz*vz)*theta;

		motion.T.x = (double)left_x + right_x;
		motion.T.y = (double)left_y + right_y;
		motion.T.z = (double)left_z + right_z;
		motion.T.divide(w_norm);

		return motion;
	}
public static Se3_F64 exponential(TwistCoordinate_F64 twist , double theta , Se3_F64 motion ) {
		if( motion == null ) {
			motion = new Se3_F64();
		}

		double w_norm = twist.w.norm();

		if( w_norm == 0.0 ) {
			CommonOps_DDRM.setIdentity(motion.R);
			motion.T.x = twist.v.x*theta;
			motion.T.y = twist.v.y*theta;
			motion.T.z = twist.v.z*theta;
			return motion;
		}

		DMatrixRMaj R = motion.getR();

		// First handle the SO region.  This Rodrigues equation
		double wx = twist.w.x/w_norm, wy = twist.w.y/w_norm, wz = twist.w.z/w_norm;

		ConvertRotation3D_F64.rodriguesToMatrix(wx,wy,wz, theta*w_norm, R);

		theta *= w_norm;

		// Now compute the translational component
		// (I - SO)*(w cross v) + w*w'*v*theta
		double vx = twist.v.x, vy = twist.v.y, vz = twist.v.z;

		double wv_x = wy*vz - wz*vy;
		double wv_y = wz*vx - wx*vz;
		double wv_z = wx*vy - wy*vx;

		double left_x = (1 - R.data[0])*wv_x -      R.data[1]*wv_y  -      R.data[2]*wv_z;
		double left_y =     -R.data[3]*wv_x  + (1 - R.data[4])*wv_y -      R.data[5]*wv_z;
		double left_z =     -R.data[6]*wv_x  -      R.data[7]*wv_y  + (1 - R.data[8])*wv_z;

		double right_x = (wx*wx*vx + wx*wy*vy + wx*wz*vz)*theta;
		double right_y = (wy*wx*vx + wy*wy*vy + wy*wz*vz)*theta;
		double right_z = (wz*wx*vx + wz*wy*vy + wz*wz*vz)*theta;

		motion.T.x = (double)left_x + right_x;
		motion.T.y = (double)left_y + right_y;
		motion.T.z = (double)left_z + right_z;
		motion.T.divide(w_norm);

		return motion;
	}
public static Se3_F64 exponential ( TwistCoordinate_F64 twist , double theta , Se3_F64 motion ) { if ( motion == null ) { motion = new Se3_F64 ( ) ; } double w_norm = twist . w . norm ( ) ; if ( w_norm == 0.0 ) { CommonOps_DDRM . setIdentity ( motion . R ) ; motion . T . x = twist . v . x * theta ; motion . T . y = twist . v . y * theta ; motion . T . z = twist . v . z * theta ; return motion ; } DMatrixRMaj R = motion . getR ( ) ; // First handle the SO region.  This Rodrigues equation double wx = twist . w . x / w_norm , wy = twist . w . y / w_norm , wz = twist . w . z / w_norm ; ConvertRotation3D_F64 . rodriguesToMatrix ( wx , wy , wz , theta * w_norm , R ) ; theta *= w_norm ; // Now compute the translational component // (I - SO)*(w cross v) + w*w'*v*theta double vx = twist . v . x , vy = twist . v . y , vz = twist . v . z ; double wv_x = wy * vz - wz * vy ; double wv_y = wz * vx - wx * vz ; double wv_z = wx * vy - wy * vx ; double left_x = ( 1 - R . data [ 0 ] ) * wv_x - R . data [ 1 ] * wv_y - R . data [ 2 ] * wv_z ; double left_y = - R . data [ 3 ] * wv_x + ( 1 - R . data [ 4 ] ) * wv_y - R . data [ 5 ] * wv_z ; double left_z = - R . data [ 6 ] * wv_x - R . data [ 7 ] * wv_y + ( 1 - R . data [ 8 ] ) * wv_z ; double right_x = ( wx * wx * vx + wx * wy * vy + wx * wz * vz ) * theta ; double right_y = ( wy * wx * vx + wy * wy * vy + wy * wz * vz ) * theta ; double right_z = ( wz * wx * vx + wz * wy * vy + wz * wz * vz ) * theta ; motion . T . x = ( double ) left_x + right_x ; motion . T . y = ( double ) left_y + right_y ; motion . T . z = ( double ) left_z + right_z ; motion . T . divide ( w_norm ) ; return motion ; }
<p>
Computes the exponential map for a twist:<br>
exp(hat(xi)*theta) = [ SO , (I-SO)*(w cross v) + w*w<sup>T</sup>v&Theta; 0 , 1]<br>
SO = exp(hat(w)*theta)
</p>

See page 42 in R. Murray, et. al. "A Mathematical Introduction to ROBOTIC MANIPULATION" 1994

@param twist Twist coordinate
@param theta Magnitude of rotation
@param motion Storage for SE(3).  If null a new instance will be returned.
@return The transformation.
<p > Computes the exponential map for a twist : <br > exp ( hat ( xi ) * theta ) = [ SO ( I - SO ) * ( w cross v ) + w * w<sup > T< / sup > v&Theta ; 0 1 ] <br > SO = exp ( hat ( w ) * theta ) < / p >