dcm2rot_v000

%dcm (Cba) to rotation vector (r^a)
%Cba: B to A transformation matrix
%r^a=Rotation vector defined in A. (Rotate A around r with mag(r) to obtain B)
%the magnitude of the returning rotation vector will be between [0 and PI]

%This is the implementaion of savage(3-31).
function rvec=dcm2rot(dcm)

sinPHI=0.5*sqrt((dcm(3,2)-dcm(2,3))^2+(dcm(1,3)-dcm(3,1))^2+(dcm(2,1)-dcm(1,2))^2);
cosPHI=0.5*(dcm(1,1)+dcm(2,2)+dcm(3,3)-1);

PHI=atan2(sinPHI,cosPHI);

if (cosPHI>=0)
    if (sinPHI==0)
        F=1;
    else
        F=PHI/sinPHI;
    end
    rvec=0.5*F*[dcm(3,2)-dcm(2,3);dcm(1,3)-dcm(3,1);dcm(2,1)-dcm(1,2)];
else
    sr_a=1-cosPHI;
    mu1=sqrt((dcm(1,1)-1)/sr_a+1);
    mu2=sqrt((dcm(2,2)-1)/sr_a+1);
    mu3=sqrt((dcm(3,3)-1)/sr_a+1);
    if (mu1>=mu2 && mu1>=mu3)
       u1=mu1*sign(dcm(3,2)-dcm(2,3));
       u2=1/2/u1*(dcm(1,2)+dcm(2,1))/sr_a;
       u3=1/2/u1*(dcm(1,3)+dcm(3,1))/sr_a;
    elseif (mu2>=mu3)
       u2=mu2*sign(dcm(1,3)-dcm(3,1));
       u3=1/2/u2*(dcm(2,3)+dcm(3,2))/sr_a;
       u1=1/2/u2*(dcm(1,2)+dcm(2,1))/sr_a; 
    else
       u3=mu3*sign(dcm(2,1)-dcm(1,2));
       u1=1/2/u3*(dcm(1,3)+dcm(3,1))/sr_a;
       u2=1/2/u3*(dcm(2,3)+dcm(3,2))/sr_a;
    end
    rvec=PHI*[u1;u2;u3];
end