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 .