Matlab Spatial Vector Source Code
(c) 2004 Roy Featherstone
Cut and paste the source code below into files as indicated

----------- Xrotx.m ----------------------------------------------------------

function X = Xrotx( h )
% Xrotx  MM6 coordinate transform from X-axis rotation.
% Xrotx(h) calculates the MM6 coordinate transform matrix (for motion
% vectors) induced by a rotation about the +X axis by an angle h (in radians).
% Positive rotation is anticlockwise: +Y axis rotates towards +Z axis.

c = cos(h);  s = sin(h);

X = [ 1  0  0  0  0  0 ;
      0  c  s  0  0  0 ;
      0 -s  c  0  0  0 ;
      0  0  0  1  0  0 ;
      0  0  0  0  c  s ;
      0  0  0  0 -s  c
    ];

----------- Xroty.m ----------------------------------------------------------

function X = Xroty( h )
% Xroty  MM6 coordinate transform from Y-axis rotation.
% Xroty(h) calculates the MM6 coordinate transform matrix (for motion
% vectors) induced by a rotation about the +Y axis by an angle h (in radians).
% Positive rotation is anticlockwise: +Z axis rotates towards +X axis.

c = cos(h);  s = sin(h);

X = [ c  0 -s  0  0  0 ;
      0  1  0  0  0  0 ;
      s  0  c  0  0  0 ;
      0  0  0  c  0 -s ;
      0  0  0  0  1  0 ;
      0  0  0  s  0  c
    ];

----------- Xrotz.m ----------------------------------------------------------

function X = Xrotz( h )
% Xrotz  MM6 coordinate transform from Z-axis rotation.
% Xrotz(h) calculates the MM6 coordinate transform matrix (for motion
% vectors) induced by a rotation about the +Z axis by an angle h (in radians).
% Positive rotation is anticlockwise: +X axis rotates towards +Y axis.

c = cos(h);  s = sin(h);

X = [  c  s  0  0  0  0 ;
      -s  c  0  0  0  0 ;
       0  0  1  0  0  0 ;
       0  0  0  c  s  0 ;
       0  0  0 -s  c  0 ;
       0  0  0  0  0  1
    ];

----------- Xtrans.m ---------------------------------------------------------

function X = Xtrans( r )
% Xtrans  MM6 coordinate transform from 3D translation vector.
% Xtrans(r) calculates the MM6 coordinate transform matrix (for motion
% vectors) induced by a shift of origin specified by the 3D vector r, which
% contains the x, y and z coordinates of the new location of the origin
% relative to the old.

X = [  1     0     0    0  0  0 ;
       0     1     0    0  0  0 ;
       0     0     1    0  0  0 ;
       0     r(3) -r(2) 1  0  0 ;
      -r(3)  0     r(1) 0  1  0 ;
       r(2) -r(1)  0    0  0  1
    ];

----------- RBmci.m ----------------------------------------------------------

function rbi = RBmci( m, c, I )
% RBmci  Calculate RBI from mass, CoM and rotational inertia.
% RBmci(m,c,I) calculate MF6 rigid-body inertia tensor for a body with
% mass m, centre of mass at c, and (3x3) rotational inertia about CoM of I.

C = [ 0, -c(3), c(2); c(3), 0, -c(1); -c(2), c(1), 0 ];

rbi = [ I + m*C*C', m*C; m*C', m*eye(3) ];

----------- crossF.m ---------------------------------------------------------

function vcross = crossF( v )
% crossF  FF6 cross-product tensor from M6 vector.
% crossF(v) calculates the FF6 cross-product tensor of motion vector v
% such that crossF(v) * f = v X f (cross-product of v and f) where f is any
% force vector or any matrix or tensor mapping to F6.

vcross = -crossM(v)';

----------- crossM.m ---------------------------------------------------------

function vcross = crossM( v )
% crossM  MM6 cross-product tensor from M6 vector.
% crossM(v) calculates the MM6 cross-product tensor of motion vector v
% such that crossM(v) * m = v X m (cross-product of v and m) where m is any
% motion vector or any matrix or tensor mapping to M6.

vcross = [  0    -v(3)  v(2)   0     0     0    ;
	    v(3)  0    -v(1)   0     0     0    ;
	   -v(2)  v(1)  0      0     0     0    ;
	    0    -v(6)  v(5)   0    -v(3)  v(2) ;
	    v(6)  0    -v(4)   v(3)  0    -v(1) ;
	   -v(5)  v(4)  0     -v(2)  v(1)  0
	 ];