function [theta,w]=Quaternion2Euler(q);

ERR=10^-7;
if abs(norm(q)-1)<ERR   
 theta=2*acos(q(4));
 w=sin(theta/2)^-1*q(1:3);
 if theta>pi
  w=-w;
  theta=2*pi-theta;
 end;
else
 theta=[];
 w=[];
 fprintf('Error Euler2Quaternion: q is not a vlaid quaternion \n');
end;
