%% Test 3 example
% Please read the notes carefully. There was an unclear explenation of the 
% process on the exercise...

%% 1) Consider a rotation with axis of rotation generated by vector: 
v = [1/3 2/3 2/3]'; % axis of rotation
u1 = [0 0 9]'; % which map the vector u1
u2 = [4 -1 8]'; % to the vector u2

u1v = (u1'*v)*v; % projection of u1 onto subspace of v (rotation axis)
u2v = (u2'*v)*v; % projection of u2 onto subspace of v

% p is a subspace orthogonal to v (rotation plane)
% u1 can be written as u1 = u1p + u1v, same for u2
% then u1p = u1-u1v is the projection of u1 onto the rotation plane, same for u2
u1p = u1-u1v; % projection of u1 onto subspace p
u2p = u2-u2v; % projection of u2 onto subspace p

% the angle of rotation is the angle between u1p and u2p 
% (also angle: k x 2pi + angle, where k e N, k not equal 0)
angle = acos(u1p'*u2p/norm(u1p)/norm(u2p))
% !!! We will get an absolute value and not the direction of the angle!!!
% => we should check if the rotation about this angle transform u1 to u2
% => if not we should use the rotation about -angle 

% rotation matrix
v_x = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
R = eye(3)+sin(angle)*v_x+(1-cos(angle))*v_x*v_x

% check the rotation about the angle 
u2 - R*u1   % should be close to zero
% !!! If this doesn't hold we have to assume the rotation about -angle 
% or that the axis of rotation with oposite sign 

% quaternion from angle and axis
q1 = [cos(angle/2); sin(angle/2) * v]
q2 = -q1


%% 2) Find the axis and the angle of rotation for the following quaternions:
% a) q1 = [1 0 0 0]'
angle1 = 2*acos(1)
axis1 = [0 0 0]

% b) q2 = [0 0 -1 0]'
angle2 = 2*acos(0)
axis2 = [0 -1 0]

% c) q3 = 1/sqrt(2) * [1 1 1 1]'
% First of all, q3 is not quaternion => you should end here.
% If we assume norm(q3)=1 which look like the author goal ... 
angle3 = 2*acos(sqrt(2)/2)
axis3 = [sqrt(2)/2 sqrt(2)/2 sqrt(2)/2]/sin(angle/2)

%% 3) Find all rotation matrices R e R^{2x2} for which there hold true RR = R'
% a) The angle should be either theta=0 and R = eye(2) 
%    or +-2pi = angle(R*R*R) => +-2pi = 3*theta => theta = +-2pi/3
%    => R = [cos(theta) -sin(theta); sin(theta) cos(theta)]
% 
%    e.g. only for +-120 deg rotations holds: rotating twice by 120deg 
%    is the same as rotating 120deg in opposite direction

% b) Algebradic solution
a = sym('a','real');
R = [cos(a) -sin(a); sin(a) cos(a)];
RR = R*R;

% RR(1) must equal R(1)
eq = RR(1)-R(1)
solve(eq)

% but also RR(2) must equal R(3) (R'(2))
eq = RR(2)-R(3)
solve(eq)

% we see that the possible solutions are 
% 0 => R = I
% -(2*pi)/3 => R =  [       -1/2, 3^(1/2)/2]
%                   [ -3^(1/2)/2,      -1/2]
% (2*pi)/3 => R =   [      -1/2, -3^(1/2)/2]
%                   [ 3^(1/2)/2,       -1/2]


%% 4) Consider the rotation with axis of rotation generated by vector
v = 1/sqrt(3)*[1 1 1]'; % axis of rotation
u1 = [1 0 0]'; % which map the vector u1
u2 = [0 0 1]'; % to the vector u2

% a) We can use the same approach as in 1) OR it is easier here
%    if we imagine rotation around [1 1 1] which brings the z axis to the 
%    x axis ([0 0 1]=>[1 0 0]) then the same rotation must bring 
%    [1 0 0] => [0 1 0] and [0 1 0]=> [0 0 1] and we have a matrix equation 
%    R* [z x y] = [x y z] which is actually just a permutation!
%    => R = [0 0 1; 1 0 0; 0 1 0];
%
% b) Let's compare it with the previous method
u1v = (u1'*v)*v; % projection of u1 onto subspace of v (rotation axis)
u2v = (u2'*v)*v; % projection of u2 onto subspace of v
 
u1p = u1-u1v; % projection of u1 onto subspace p
u2p = u2-u2v; % projection of u2 onto subspace p

% the angle of rotation is simply the angle between u1p and u2p
angle = acos(u1p'*u2p/norm(u1p)/norm(u2p))
% !!! We will get an absolute value and not the direction of the angle!!!
% => we should check if the rotation about this angle transform u1 to u2
% => if not we should use the rotation about -angle 

% check the rotation about the angle 
u2 - R*u1   % should be close to zero
% It is ok in this case. If we have to compute u1 = R*u2 we whould have
% chage the sign of the angle or the sign of the axis of rotation 
% (i.e. the normalization). 

% This also holds for the rotation about 2pi*k + angle, where k e N/{0}
% but we have the bounds in the assignment so there will be just one
% solution for v = 1/sqrt(3) * [1 1 1]' and one solution for 
% v = -1/sqrt(3) * [1 1 1]' which we can choose arbitrary (e.g. ||v|| = 1)

% rotation matrix
v_x = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
R = eye(3)+sin(angle)*v_x+(1-cos(angle))*v_x*v_x

% the eigen values 
[V, lambda] = eig(R)


%% 5 Consider the quaternion:
q = 1/3 * [0 -1 -2 -2]';

% Find all pairs [theta, v] which correspond to q:
% => first, we compute the solution according the standard approach:
theta1 = acos(0)
v1 = 1/3*[-1 -2 -2]'
% thus, we get just angle = pi in the bound -pi < theta <= pi
% BUT!!! we must find all the pairs which correspond q and therfore:
theta2 = pi
v2 = 1/3*[1 2 2]'
% also correspond the same rotation as q and therfore correspond q
% => [(pi,1/3*[-1 -2 -2]); (pi,1/3*[1 2 2])]

x_ = @(v) [0 -v2(3) v2(2); v2(3) 0 -v2(1); -v2(2) v2(1) 0];
R1 = eye(3)+sin(theta1)*x_(v1)+(1-cos(theta1))*x_(v1)*x_(v1)
R2 = eye(3)+sin(theta2)*x_(v2)+(1-cos(theta2))*x_(v2)*x_(v2)

%CHECK
R1-R2
% so there are in total 2 pairs (pi,1/3*[-1 -2 -2]) and (pi,-1/3*[-1 -2 -2])


%% 6 Consider rotation:
R = [0 1 0; 0 0 1; 1 0 0];
angle = acos((trace(R)-1)/2)
axis = 1/(2*sin(angle))*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)]
% we also know ||v|| = 1 and it is enough to normalize axis instead 
% of multiplication with 1/(2*sin(angle))

% there will be 2 quaternions
q1 = [cos(angle/2) sin(angle/2)*axis']
q2 = -q1


% This is similar case as in 4 but now it is a different permutation
% x=>z, z => y , y => x so it is the opposite rotation - angle = 120 deg
% but axis = -1/sqrt(3)*[ 1 1 1] 



