% Construct a rotation matrix from a vector containing the lower triangular part
% of a skew symmetric matrix using the matrix exponential.
%
% For 1 input parameter,  we obtain the usual rotation matrix
% [cos(rot),-sin(rot); sin(rot),cos(rot)].
%
% For 3 input parameters, we obtain the axis angle parametrisation from
% Rodrigues' rotation formula. For very small rotations (in the order of 1e-2),
% the parametrisation is approximately the same as the yaw, pitch, roll
% parametrisation with angles alpha=rot(1), beta=rot(2) and gamma=rot(3).
% The order of the multiplication of the individual rotation matrices Rx, Ry and
% Rz does not matter since they commute for infinitesimal rotations.
%
%   See also DROTMAT.M.
%
% (c) by Hannes Nickisch, MPI for Biological Cybernetics, 2012 January 05

function R = rotmat(rot,varargin)
  N = numel(rot);                           % number of parameters in the vector
  d = fix(1/2 + sqrt(1/4+2*N));              % size of the skew symmetric matrix
  if     nargin>1 && d==2
    R = [cos(rot) -sin(rot); sin(rot) cos(rot)];
  elseif nargin>1 && d==3
    a = norm(rot); c = cos(a); s = sin(a); r = rot(:)/a;
    R = r*(1-c)*r' + [c,-r(3)*s, r(2)*s; r(3)*s,c,-r(1)*s; -r(2)*s,r(1)*s,c];
  else
    sgn = cos(pi*(0:N-1)');                                  % alternating signs
    X = zeros(d); X(tril(ones(d),-1)>0)=flipud(rot(:)).*sgn;  % construct matrix
    R = expm(X-X');                  % take exponential of skew symmetric matrix
  end