
function Q = cayley(v)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Computes the rotation matrix Q corresponding to 
% the coordinate vector v using the Cayley map.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize variables

Q      = zeros(3,3) ;
vcross = zeros(3,3) ;

% Compute skew matrix [vx]

vcross(3,2) =  v(1) ;
vcross(2,3) = -v(1) ;
vcross(1,3) =  v(2) ;
vcross(3,1) = -v(2) ;
vcross(2,1) =  v(3) ;
vcross(1,2) = -v(3) ;

% Evaluate the Cayley map

a = 4/( 4 + v(1)^2 + v(2)^2 + v(3)^2 )  ;

Q = eye(3) + a*vcross + (a/2)*vcross*vcross ;


