% Copyright (c) 2017, Domenico L. Gatti
% All rights reserved.
% 
% Redistribution and use in source and binary forms, with or without 
% modification, are permitted provided that the following conditions are 
% met:
% 
%     * Redistributions of source code must retain the above copyright 
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright 
%       notice, this list of conditions and the following disclaimer in 
%       the documentation and/or other materials provided with the 
%       distribution
%       
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%% General dependencies
% We always start from the CODE directory and we add to the path
% subdirectories containing various tools described in the book chapters.
addpath(genpath('../GENERAL_SCRIPTS_FUNCTIONS'));
% addpath(genpath('../DATABASE'));
% addpath(genpath('../TOOLBOXES'));

%% CHAPTER 2. Change of basis.

format short
clear
clc

% Identity matrix:

a = [3;2;1]
I = eye(3)
a_v = I*a

u1 = [2;3;1], u2 = [0;1;1], u3 = [2 1 1]'
U = [2 0 2;3 1 1;1 1 1]
U = [u1 u2 u3]

% Change of basis
a_u = [0.5 -0.5 1]'
U*a_u

% Gauss-Jordan elimination
UI = [U I]	
UI(2,:) = UI(2,:)-1.5*UI(1,:)
UI(3,:) = UI(3,:)-0.5*UI(1,:)
UI(3,:) = UI(3,:)-1*UI(2,:)
UI(1,:) = UI(1,:)-1*UI(3,:)
UI(2,:) = UI(2,:)+1*UI(3,:)
UI([1 3],:) = UI([1 3],:)/2

% Gaussian Elimination step by step
[x,Up] = gauss_elim_step_by_step(U,a_v)

% Row reduced echelon form - Inverse
UI = rref([U I])
invU = UI(:,4:6) , invU = UI(:,[4 5 6]), invU = inv(U), invU = U^-1
U*invU , invU*U

% Change of basis
a_u = invU*a_v , a_v = U*a_u

% Solution of systems of linear equations
a_u = inv(U)*a_v		
a_u = mldivide(U,a_v)
a_u = U\a_v		

tic, a_u = inv(U)*a_v, toc		
tic, a_u = mldivide(U,a_v), toc
tic, a_u = U\a_v, toc

tic, a_u = linsolve(U,a_v), toc

% Transformation matrices
a_v = [3 2]'
theta = pi/6
Q = [cos(theta) sin(theta);-sin(theta) cos(theta)]
a_u = Q*a_v

U = inv(Q)
U = Q'

% Orthogonal matrices
a = [3 5 1]
b = [2 0 -6]
c = cross(a,b)
u1 = a/norm(a)
u2 = b/norm(b)
u3 = c/norm(c)
I = eye(3);
U = [u1;u2;u3]'
U'*U, U*U'
Q = inv(U)
Q'*Q, Q*Q'


norm(Q(:,1)) , norm(Q(:,2)) , norm(Q(:,3))
norm(Q(1,:)) , norm(Q(2,:)) , norm(Q(3,:))
Q(:,1)' *Q(:,2) , Q(:,1)' *Q(:,3) , Q(:,2)' *Q(:,3)
Q(1,:)*Q(2,:)' , Q(1,:)*Q(3,:)' , Q(2,:)*Q(3,:)'
Q(1,:)*Q(1,:)' , Q(2,:)*Q(2,:)' , Q(3,:)*Q(3,:)'
% or simply
Q'*Q, Q*Q'

a = [3 2 1]';b = [1 2 3]';
a'*b
(Q*a)'*(Q*b)

Q' , inv(Q)
Q'*Q, Q*Q'

det(Q)

% Linear Transformations
T = [2 0;3 1;1 1];
v1 = [0.5 0.5]';
v2 = [2 3]'
c = 2; d = 3;
% 1st condition
T*(v1 + v2)
T*v1 + T*v2
% 2nd condition
T*(c*v1)
c*(T*v1)
% 3rd condition
T*(c*v1+d*v2)
c*(T*v1)+d*(T*v2)

% Similarity Transformations
a = [3 6]';
R_v = [cos(pi/6) sin(pi/6);-sin(pi/6) cos(pi/6)];
b = R_v*a
show_frame('new',eye(3))
hold on
show_vector([a;0],'magenta','a')
show_vector([b;0],'yellow','b')
hold off

U = [5 3;1 9]
R_u = inv(U)*R_v*U
R_u = U\R_v*U  % using the 'backslash' operator
b = U*R_u*inv(U)*a
b = U*R_u/U*a  % using the 'backslash' operator

%% SPECIAL TOPIC: Intrinsic and extrinsic rotations
a = [3 5 1]
b = [2 0 -6]
c = cross(a,b)
u1 = a/norm(a)
u2 = b/norm(b)
u3 = c/norm(c)
I = eye(3);
U = [u1;u2;u3]'
U'*U, U*U'
Q = inv(U)

show_frame('new',Q)
show_frame('new',Q')

% Euler intrinsic rotations
a_v = [3 2 1]'
phi = pi/6;theta = pi/3;psi = pi/10;
Q1 = [1 0 0; 0 cos(phi) sin(phi);0 -sin(phi) cos(phi)]
Q2 = [cos(theta) 0 -sin(theta);0 1 0;sin(theta) 0 cos(theta)]
Q3 = [cos(psi) sin(psi) 0;-sin(psi) cos(psi) 0;0 0 1]
a_u = Q3*Q2*Q1* a_v
Q = Q3*Q2*Q1
a_u = Q*a_v

% Euler extrinsic rotations
a_v = [3 2 1]'
phi = 30; theta = 60; psi = 18;
Q1 = [1 0 0; 0 cosd(phi) sind(phi);0 -sind(phi) cosd(phi)];
Q2 = [cosd(theta) 0 -sind(theta);0 1 0;sind(theta) 0 cosd(theta)];
Q3 = [cosd(psi) sind(psi) 0;-sind(psi) cosd(psi) 0;0 0 1];
Q = Q3*Q2*Q1;
R = Q'; 
R = Q1'*Q2'*Q3'; % transposition rule
R1 = Q1'; R2 = Q2'; R3 = Q3';
R = R1*R2*R3;
rot_a_v = R1*R2*R3*a_v
rot_a_v = R*a_v


%%
a_v = [3 2 1]'
phi = 13; theta = 18; psi = -30;
Q1 = [1 0 0; 0 cosd(phi) sind(phi);0 -sind(phi) cosd(phi)];
Q2 = [cosd(theta) 0 -sind(theta);0 1 0;sind(theta) 0 cosd(theta)];
Q3 = [cosd(psi) sind(psi) 0;-sind(psi) cosd(psi) 0;0 0 1];
Q = Q3*Q2*Q1;
R = Q'

R = Q1'*Q2'*Q3'; % transposition rule
R1 = Q1'; R2 = Q2'; R3 = Q3';
R = R1*R2*R3;
rot_a_v = R1*R2*R3*a_v
rot_a_v = R*a_v

rot_a_v_t = (R*a_v)'
rot_a_v_t = a_v'*R3'*R2'*R1'
rot_a_v_t = a_v'*Q3*Q2*Q1

a_u = Q3*Q2*Q1*a_v
a_u_t = a_v'*R1*R2*R3

% Reflection matrices
phi = pi/2
R_rot = [cos(phi) -sin(phi);sin(phi) cos(phi)]
R_ref = [cos(phi) sin(phi);sin(phi) -cos(phi)]
b = [1 0.5]'
b = b/norm(b)
b_rot = R_rot*b
b_ref = R_ref*b
show_frame('new',eye(3))
hold on
show_vector([b;0],'k','b')
show_vector([b_rot;0],'m','b\_rot')
show_vector([b_ref;0],'y','b\_ref')
det(R_ref)

%% SPECIAL TOPIC: Covariant and contravariant bases

F = [3 1 0]' 
a = [1.5 2 0]'
Fa = F'*a

g1 = rand(3,1)
g2 = rand(3,1)
g3 = rand(3,1)
g1 = g1/norm(g1)
g2 = g2/norm(g2)
g3 = g3/norm(g3)
G = [g1 g2 g3]

inv_G = inv(G) 
F_g = inv_G*F
a_g = inv_G*a
Fa_g = F_g'*a_g

% General definition of dot product valid for any reference frame.
Fa_g = (F_g(1)*g1 + F_g(2)*g2 + F_g(3)*g3)' * (a_g(1)*g1 + a_g(2)*g2 + a_g(3)*g3)

% Contravariant basis
cvg1 = cross(g2,g3)/(g1'*cross(g2,g3))
cvg2 = cross(g3,g1)/(g2'*cross(g3,g1))
cvg3 = cross(g1,g2)/(g3'*cross(g1,g2))
cvg1 = cvg1/norm(cvg1)
cvg2 = cvg2/norm(cvg2)
cvg3 = cvg3/norm(cvg3)
cvG = [cvg1 cvg2 cvg3]
G_cv = inv(G')
G_cv = inv_G'

show_frame('new',eye(3))
show_frame('add',G,'--r','--g','--b','g1','g2','g3')
show_frame('add',G_cv,':r',':g',':b','g1\_cv','g2\_cv','g3\_cv')

F_g_cv = [g1'*F g2'*F g3'*F]'
a_g_cv = [g1'*a g2'*a g3'*a]'
F_g_cv = G'*F
a_g_cv = G'*a


Fa = F'*a
Fa = F_g'*a_g_cv
Fa = F_g_cv'*a_g

