% Compute the derivatives of a rotation matrix w.r.t. to its parameters.
%
% We use the spectral representation given in theorem 4.5 of [1]. For inputs of
% reasonable size, the computation is stable although differences of
% exponentials have to be computed.
%
% [1] Igor Najfeld and Timothy F. Havel. Derivatives of the matrix exponential
%     and their computation. Advances in Applied Mathematics, 16:321-375, 1995.
%
%   See also ROTMAT.M.
%
% (c) by Hannes Nickisch, MPI for Biological Cybernetics, 2012 January 05

function varargout = drotmat(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
  varargout = cell(nargout, 1);  % allocate the right number of output arguments
  if     nargin>1 && d==2
    varargout{1} = [-sin(rot) -cos(rot); cos(rot) -sin(rot)];
  elseif nargin>1 && d==3
    a = norm(rot); c = cos(a); s = sin(a); a4 = a*a*a*a; 
    x = rot(1);    y = rot(2); z = rot(3); R  = zeros(3,3);
  
    R(1,1) = (x*z^2+x*y^2)*(2-2*c-a*s)/a4;
    R(1,2) = (a*s*(x*z+x^2*y) + c*(-x*z^3-y*z^2+(-x*y^2-x^3)*z-y^3+x^2*y)+y*z^2+y^3-x^2*y)/a4;
    R(1,3) = (a*s*(x^2*z-x*y) + c*(-z^3+x*y*z^2+(x^2-y^2)*z+x*y^3+x^3*y)+z^3+(y^2-x^2)*z)/a4;
    R(2,1) = -1*(a*s*(x*z-x^2*y) + c*(-x*z^3+y*z^2+(-x*y^2-x^3)*z+y^3-x^2*y)-y*z^2-y^3+x^2*y)/a4;
    R(2,2) = -1*(a*(x*z^2+x^3)*s-2*x*y^2*(c-1))/a4;
    R(2,3) = -1*((z^4-x*y*z^3+(2*y^2+x^2)*z^2+(-x*y^3-x^3*y)*z+y^4+x^2*y^2)*s + a*((x^2*z^2-2*x*y*z+x^2*y^2+x^4)*c + 2*x*y*z))/(a4*a);
    R(3,1) = (a*s*(x^2*z+x*y) + c*(-z^3-x*y*z^2+(x^2-y^2)*z-x*y^3-x^3*y)+z^3+(y^2-x^2)*z)/a4;
    R(3,2) = ((z^4+x*y*z^3+(2*y^2+x^2)*z^2+(x*y^3+x^3*y)*z+y^4+x^2*y^2)*s + a*((x^2*z^2+2*x*y*z+x^2*y^2+x^4)*c - 2*x*y*z))/(a4*a);
    R(3,3) = -1*(a*(x*y^2+x^3)*s-2*x*z^2*(c-1))/a4;
    varargout{1} = R;
  
    R(1,1) = -1*(a*s*(y*z^2+y^3)-2*x^2*y*c+2*x^2*y)/a4;
    R(1,2) = (a*s*(y*z+x*y^2) + c*(-y*z^3-x*z^2+(-x^2*y-y^3)*z-x^3+x*y^2)+x*z^2+x^3-x*y^2)/a4;
    R(1,3) = ((z^4+x*y*z^3+(y^2+2*x^2)*z^2+(x*y^3+x^3*y)*z+x^4+x^2*y^2)*s + a*((y^2*z^2+2*x*y*z+x^2*y^2+y^4)*c - 2*x*y*z))/(a4*a);
    R(2,1) = -1*(a*s*(y*z-x*y^2) + c*(-y*z^3+x*z^2+(-x^2*y-y^3)*z+x^3-x*y^2)-x*z^2-x^3+x*y^2)/a4;
    R(2,2) = -1*(a*(y*z^2+x^2*y)*s+(2*y*z^2+2*x^2*y)*(c-1))/a4;
    R(2,3) = (a*s*(y^2*z+x*y) + c*(-z^3-x*y*z^2+(y^2-x^2)*z-x*y^3-x^3*y)+z^3+(x^2-y^2)*z)/a4;
    R(3,1) = -1*((z^4-x*y*z^3+(y^2+2*x^2)*z^2+(-x*y^3-x^3*y)*z+x^4+x^2*y^2)*s + a*((y^2*z^2-2*x*y*z+x^2*y^2+y^4)*c + 2*x*y*z))/(a4*a);
    R(3,2) = (a*s*(y^2*z-x*y) + c*(-z^3+x*y*z^2+(y^2-x^2)*z+x*y^3+x^3*y)+z^3+(x^2-y^2)*z)/a4;
    R(3,3) = -1*(a*(x^2*y+y^3)*s-2*y*z^2*(c-1))/a4;
    varargout{2} = R;  

    R(1,1) = -1*(a*s*(z*y^2+z^3)-2*x^2*z*c+2*x^2*z)/a4;
    R(1,2) = -1*((y^4-x*y*z^3+(y^2+x^2)*z^2+(-x*y^3-x^3*y)*z+x^4+2*x^2*y^2)*s + a*((z^4+(y^2+x^2)*z^2-2*x*y*z)*c + 2*x*y*z))/(a4*a);
    R(1,3) = (a*s*(-y*z+x*z^2) + c*(y*z^3+x*z^2+(x^2*y+y^3)*z-x^3-x*y^2)-x*z^2+x^3+x*y^2)/a4;
    R(2,1) = ((y^4+x*y*z^3+(y^2+x^2)*z^2+(x*y^3+x^3*y)*z+x^4+2*x^2*y^2)*s + a*((z^4+(y^2+x^2)*z^2+2*x*y*z)*c - 2*x*y*z))/(a4*a);
    R(2,2) = -1*(a*(z^3+x^2*z)*s-(2*y^2*z)*(c-1))/a4;
    R(2,3) = (a*s*(z^2*y+x*z) + c*(-x*z^3+y*z^2+(-x*y^2-x^3)*z-y^3-x^2*y)+y^3+(x^2-z^2)*y)/a4;
    R(3,1) = (a*s*(y*z+x*z^2) + c*(-y*z^3+x*z^2+(-x^2*y-y^3)*z-x^3-x*y^2)-x*z^2+x^3+x*y^2)/a4;
    R(3,2) = (a*s*(z^2*y-x*z) + c*(x*z^3+y*z^2+(x*y^2+x^3)*z-y^3-x^2*y)+y^3+(x^2-z^2)*y)/a4;
    R(3,3) = -1*(a*(y^2+x^2)*z*s+(2*y^2+2*x^2)*z*(c-1))/a4;
    varargout{3} = R; 
  
  else
    sgn = cos(pi*(0:N-1)');                                  % alternating signs
    X = zeros(d); X(tril(ones(d),-1)>0)=flipud(rot(:)).*sgn;  % construct matrix
    [U,L] = eig(X-X'); l = diag(L);            % compute spectral representation
    l_diff = l*ones(1,d)-ones(d,1)*l.';              % difference of eigenvalues
    l_eq   = abs(l_diff)<1e-10;                             % determine equality
    Phi_eq = exp(l)*ones(1,d);                 % Phi if eigenvalues are the same
    Phi = (exp(l)*ones(1,d)-ones(d,1)*exp(l.'))./l_diff;      % and if different
    Phi(l_eq) = Phi_eq(l_eq);

    [i,j] = find(tril(ones(d),-1)>0);
    for n=1:nargout
      if n>N
        varargout{n} = zeros(d);
      else
        D = zeros(d); D(i(N-n+1),j(N-n+1)) = sgn(N-n+1); D = D-D';   % direction
        varargout{n} = real(U*((U'*D*U).*Phi)*U');      % directional derivative
      end
    end
  end