% test 3 example
%% 1
% R*u1 = u2
v = [1/3 2/3 2/3]'; % axis of rotation
u1 = [0 0 9]';
u2 = [4 -1 8]';
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 simply the angle between u1p and u2p
angle = acos(u1p'*u2p/norm(u1p)/norm(u2p))
% check if the computed angle should be positive or negative (correct the direction)
if(cross(u1p,u2p)'*v<0)
   angle = -angle; % you can check that this works by running this for v = -[1/3 2/3 2/3]';
end
% 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
u2-R*u1
%% 2 
% a) R = I
angle = 2*acos(1)
axis = [0 0 0]
% b) sin(angle/2) = 1
angle = 2*acos(0)
axis = [0 -1 0]
% c)
angle = 2*acos(sqrt(2)/2)
axis = [sqrt(2)/2 sqrt(2)/2 sqrt(2)/2]/sin(angle/2)
%% 3
% only +-120 deg rotations
% rotating twice by 120deg is the same as rotating 120deg in opposite
% direction
% proof
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
% 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];
%
% NOW compare with the previous method
%
% R*u1 = u2
v = 1/sqrt(3)*[1 1 1]'; % axis of rotation
u1 = [0 0 1]';
u2 = [1 0 0]';
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 simply the angle between u1p and u2p
angle = acos(u1p'*u2p/norm(u1p)/norm(u2p))
% check if the computed angle should be positive or negative (correct the direction)
if(cross(u1p,u2p)'*v<0)
   angle = -angle; % you can check that this works by running this for v = -[1/3 2/3 2/3]';
end
% 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
u2-R*u1
% eigenvalues
eig(R)
%% 5
% since cos(angle/2) = 0 there are 2 possible angles => +-pi
% but only pi fulfils the condition -pi < angle <= pi
% therefore that gives one pair => (pi,1/3*[-1 -2 -2]) corresponding to this
% quaternion
v1 = 1/3*[-1 -2 -2]';
angle = pi;
v_x = [0 -v1(3) v1(2); v1(3) 0 -v1(1); -v1(2) v1(1) 0];
R1 = eye(3)+sin(angle)*v_x+(1-cos(angle))*v_x*v_x
%% 6
% 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] 
% OR use the formulas
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)]
% there will be 2 quaternions
q1 = [cos(angle/2) sin(angle/2)*axis']
q2 = -q1
