
function v = invCayley(Q)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Computes the coords v for a given rotation matrix 
% Q using the inverse Cayley map.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

v = zeros(3,1) ;

Qskw = Q-Q' ;

trQ = Q(1,1) + Q(2,2) + Q(3,3) ;

v(1) = 2*Qskw(3,2)/(trQ+1) ;
v(2) = 2*Qskw(1,3)/(trQ+1) ;
v(3) = 2*Qskw(2,1)/(trQ+1) ;

