% ==============================
% Exercise 3.1.a:
function R = getRotationMatrixFromVector(w)
  length_w = norm(w);
  w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];
  R = eye(3,3) + w_hat/length_w * sin(length_w) ...
               +(w_hat^2)/(length_w^2) * (1 - cos(length_w));
end

% Exercise 3.1.b:  (compare Lecture-Slides 2, Slide 13)
function w = getVectorFromRotationMatrix(R)
  length_w = acos((trace(R)-1)/2);
  if (length_w == 0)
      w = [0 0 0]';
  else
      w = 1/(2*sin(length_w))*[R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]'*length_w;
  end
end


% Exercise 3.1.c:
%               [ exp[w_hat]   (I - exp[w_hat]) * w_hat + w*w^T)/|w|)*v ]
% exp[xi_hat] = [                                                       ]
%               [    0^T                         1                      ]

function V = getTransformMatrixFromTwistCoord(twCoord)
  v = twCoord(1:3);
  w = twCoord(4:6);
  length_w = norm(w);
  w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];
  R = getRotationMatrixFromVector(w);
  T = ((eye(3,3) - R) * w_hat + w * w') * v / length_w;
  V = [R, T; zeros(1,3), 1];
end


%      [ v=(I-exp[w_hat])*w_hat+w*w^T)^{-1}*|w|*T ]
% xi = [                                          ]
%      [         w = hat^{-1}(log(w))             ]

function xi = getTwistCoordFromTransformMatrix(g)
  R = g(1:3,1:3);                                           % extract rotation matrix
  T = g(1:3,4);                                             % extract translation part
  w = getVectorFromRotationMatrix(R);                       % calc corresponding w
  %testR = getRotationMatrixFromVector(w);
  w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];       % we need w_hat as well
  v = inv((eye(3,3) - R) * w_hat + w * w') * norm(w) * T;   % calc the translation
  xi = [v; w];                                              % concatenate into one vector
end


% ==============================
% Exercise 3.2.a:
R = [0.1729 -0.1468 0.9739 ; 0.9739 0.1729 -0.1468 ; -0.1468 0.9739 0.1729];
a            = getVectorFromRotationMatrix(R);  % [1.0472 1.0472 1.0472]'
rotationaxis = a / norm(a);                     % [0.5774 0.5774 0.5774]'
angle        = norm(a);                         % 1.8137


% Exercise 3.2.b:
[V,D] = eig(R); 
%  [-0.2406 + 0.9706i ; -0.2406 - 0.9706i ; 1]
%  -0.2887 + 0.5000i  -0.2887 - 0.5000i   0.5774          
%   0.5774             0.5774             0.5774          
%  -0.2887 - 0.5000i  -0.2887 + 0.5000i   0.5774          


% ==============================
% Exercise 3.3:
function W = rotate_around_ray(V,a,b,alpha)
    % Initialize W:
    nVertices = size(V,1)
    W = zeros(nVertices,3);
    
    % Translation matrix (translates the model to the point (0,0,0)):
    T = [eye(3,3), -a; zeros(1,3), 1];
    
    % Translation back to the center of V:
    Tback = [eye(3,3), a; zeros(1,3), 1];
    
    % Rotation around b by alpha
    % b = rotation axis = unit vector
    % a = rotation angle 
    % Rodrigues formula takes arbitrary vector and decomposes the vector
    % into its components (unit vector, length) itself
    %
    % => input = b * alpha
    R1 = getRotationMatrixFromVector(b * alpha)
    
    % Rotation matrices in homegeneuous coordinates:
    R = [R1, zeros(3,1) ; zeros(1,3), 1]
    
    % Overall transformation matrix:
    G = Tback * R * T;
    
    % Homogeneous coordinates of V:
    Vh = [V,ones(nVertices,1)];
    
    % Rotate and translate the vertices:
    Wh_t = G * Vh';
    Wh = Wh_t';
    
    % Back transform from homogenous to 3D coordinates:
    W = Wh(:,1:3);
end