function  out = rv( in )

% rv  3D rotation vector <--> 3x3 coordinate rotation matrix
% E=rv(v) and v=rv(E) convert between a rotation vector v, whose magnitude
% and direction describe the angle and axis of rotation of a coordinate
% frame B relative to frame A, and the 3x3 coordinate rotation matrix E
% that transforms from A to B coordinates.  For example, if v=[theta;0;0]
% then rv(v) produces the same matrix as rx(theta).  If the argument is a
% 3x3 matrix then it is assumed to be E, otherwise it is assumed to be v.
% rv(E) expects E to be accurately orthonormal, and returns a column vector
% with a magnitude in the range [0,pi].  If the magnitude is exactly pi
% then the sign of the return value is unpredictable, since pi*u and -pi*u,
% where u is any unit vector, both represent the same rotation.  rv(v) will
% accept a row or column vector of any magnitude.

if all(size(in)==[3 3])
  out = Etov( in );
else
  out = vtoE( [in(1);in(2);in(3)] );
end


function  E = vtoE( v )

theta = norm(v);
if theta == 0
  E = eye(3);
else
  s = sin(theta);
  c = cos(theta);
  c1 = 2 * sin(theta/2)^2;		% 1-cos(h) == 2sin^2(h/2)
  u = v/theta;
  E = c*eye(3) - s*skew(u) + c1*u*u';
end


function  v = Etov( E )

% This function begins by extracting the skew-symmetric component of E,
% which, as can be seen from the previous function, is -s*skew(v/theta).
% For angles sufficiently far from pi, v is then calculated directly from
% this quantity.  However, for angles close to pi, E is almost symmetric,
% and so extracting the skew-symmetric component becomes numerically
% ill-conditioned, and provides an increasingly inaccurate value for the
% direction of v.  Therefore, the component c1*u*u' is extracted as well,
% and used to get an accurate value for the direction of v.  If the angle
% is exactly pi then the sign of v will be indeterminate, since +v and -v
% both represent the same rotation, but the direction will still be
% accurate.

w = -skew(E);				% w == s/theta * v
s = norm(w);
c = (trace(E)-1)/2;
theta = atan2(s,c);

if s == 0
  v = [0;0;0];
elseif theta < 0.9*pi			% a somewhat arbitrary threshold
  v = theta/s * w;
else					% extract v*v' component from E and
  E = E - c * eye(3);			% pick biggest column (best chance
  E = E + E';				% to get sign right)
  if E(1,1) >= E(2,2) && E(1,1) >= E(3,3)
    if w(1) >= 0
      v = E(:,1);
    else
      v = -E(:,1);
    end
  elseif E(2,2) >= E(3,3)
    if w(2) >= 0
      v = E(:,2);
    else
      v = -E(:,2);
    end
  else
    if w(3) >= 0
      v = E(:,3);
    else
      v = -E(:,3);
    end
  end
  v = theta/norm(v) * v;
end