clear
clc

%Solution of exercise 2b Week#5

%% Properties
E=200000;
%Member ab
l_ab=6000;
I_ab=6.36*10^8;
A_ab=2.5*10^4;
%Member bc
l_bc=4000;
I_bc=6.36*10^8;
A_bc=2.5*10^4;
%Member bd
l_bd=4000;
I_bd=8.61*10^8;
A_bd=1.76*10^4;
%Member ce
l_ce=4000;
I_ce=8.61*10^8;
A_ce=1.76*10^4;

%% Equation number
nb_el=4;
elements=[1:nb_el];
connectivity=[1 2;
    2 3;
    2 4;
    3 5];

numEq = [1 2 3 4 5 6;
    4 5 6 7 8 9;
    4 5 6 10 11 12;
    7 8 9 13 14 15];

%% Transformation matrix from local to global
%Columns
c_col = cos(pi/2);
s_col = sin(pi/2);
T_col = [ c_col,  s_col, 0, 0, 0, 0;
    -s_col,  c_col, 0, 0, 0, 0;
    0,  0, 1, 0, 0, 0;
    0,  0, 0, c_col,  s_col, 0;
    0,  0, 0, -s_col, c_col, 0;
    0,  0, 0, 0,  0, 1];
%Beams
c_beams = cos(0);
s_beams = sin(0);
T_beams = [ c_beams,  s_beams, 0, 0, 0, 0;
    -s_beams,  c_beams, 0, 0, 0, 0;
    0,  0, 1, 0, 0, 0;
    0,  0, 0, c_beams,  s_beams, 0;
    0,  0, 0, -s_beams, c_beams, 0;
    0,  0, 0, 0,  0, 1];

%% Elastic stiffness matrix in basic reference frame
%Member ab
Kbar_ab = [E*A_ab/l_ab 0 0;
    0  4*E*I_ab/l_ab 2*E*I_ab/l_ab;
    0 2*E*I_ab/l_ab 4*E*I_ab/l_ab];
%Member bc
Kbar_bc = [E*A_bc/l_bc 0 0;
    0  4*E*I_bc/l_bc 2*E*I_bc/l_bc;
    0 2*E*I_bc/l_bc 4*E*I_bc/l_bc];
%Member bd
Kbar_bd = [E*A_bd/l_bd 0 0;
    0  4*E*I_bd/l_bd 2*E*I_bd/l_bd;
    0 2*E*I_bd/l_bd 4*E*I_bd/l_bd];
%Member ce
Kbar_ce = [E*A_ce/l_ce 0 0;
    0  4*E*I_ce/l_ce 2*E*I_ce/l_ce;
    0 2*E*I_ce/l_ce 4*E*I_ce/l_ce];

%% Elastic stiffness matrix in local reference frame
%Member ab
Ke_loc_ab = [ E*A_ab/l_ab,         0,          0,        -E*A_ab/l_ab,         0,           0;
    0,   12*E*I_ab/l_ab^3,   6*E*I_ab/l_ab^2,         0, -12*E*I_ab/l_ab^3,   6*E*I_ab/l_ab^2;
    0,    6*E*I_ab/l_ab^2,    4*E*I_ab/l_ab,         0,  -6*E*I_ab/l_ab^2,    2*E*I_ab/l_ab;
    -E*A_ab/l_ab,         0,          0,         E*A_ab/l_ab,         0,           0;
    0,  -12*E*I_ab/l_ab^3,  -6*E*I_ab/l_ab^2,         0,  12*E*I_ab/l_ab^3,  -6*E*I_ab/l_ab^2;
    0,    6*E*I_ab/l_ab^2,    2*E*I_ab/l_ab,         0,  -6*E*I_ab/l_ab^2,    4*E*I_ab/l_ab];
%Member bc
Ke_loc_bc = [
    E*A_bc/l_bc,         0,          0,        -E*A_bc/l_bc,         0,           0;
    0,   12*E*I_bc/l_bc^3,   6*E*I_bc/l_bc^2,         0, -12*E*I_bc/l_bc^3,   6*E*I_bc/l_bc^2;
    0,    6*E*I_bc/l_bc^2,    4*E*I_bc/l_bc,         0,  -6*E*I_bc/l_bc^2,    2*E*I_bc/l_bc;
    -E*A_bc/l_bc,         0,          0,         E*A_bc/l_bc,         0,           0;
    0,  -12*E*I_bc/l_bc^3,  -6*E*I_bc/l_bc^2,         0,  12*E*I_bc/l_bc^3,  -6*E*I_bc/l_bc^2;
    0,    6*E*I_bc/l_bc^2,    2*E*I_bc/l_bc,         0,  -6*E*I_bc/l_bc^2,    4*E*I_bc/l_bc;
    ];
%Member bd
Ke_loc_bd = [
    E*A_bd/l_bd,         0,          0,        -E*A_bd/l_bd,         0,           0;
    0,   12*E*I_bd/l_bd^3,   6*E*I_bd/l_bd^2,         0, -12*E*I_bd/l_bd^3,   6*E*I_bd/l_bd^2;
    0,    6*E*I_bd/l_bd^2,    4*E*I_bd/l_bd,         0,  -6*E*I_bd/l_bd^2,    2*E*I_bd/l_bd;
    -E*A_bd/l_bd,         0,          0,         E*A_bd/l_bd,         0,           0;
    0,  -12*E*I_bd/l_bd^3,  -6*E*I_bd/l_bd^2,         0,  12*E*I_bd/l_bd^3,  -6*E*I_bd/l_bd^2;
    0,    6*E*I_bd/l_bd^2,    2*E*I_bd/l_bd,         0,  -6*E*I_bd/l_bd^2,    4*E*I_bd/l_bd;
    ];
%Member ce
Ke_loc_ce = [
    E*A_ce/l_ce,         0,          0,        -E*A_ce/l_ce,         0,           0;
    0,   12*E*I_ce/l_ce^3,   6*E*I_ce/l_ce^2,         0, -12*E*I_ce/l_ce^3,   6*E*I_ce/l_ce^2;
    0,    6*E*I_ce/l_ce^2,    4*E*I_ce/l_ce,         0,  -6*E*I_ce/l_ce^2,    2*E*I_ce/l_ce;
    -E*A_ce/l_ce,         0,          0,         E*A_ce/l_ce,         0,           0;
    0,  -12*E*I_ce/l_ce^3,  -6*E*I_ce/l_ce^2,         0,  12*E*I_ce/l_ce^3,  -6*E*I_ce/l_ce^2;
    0,    6*E*I_ce/l_ce^2,    2*E*I_ce/l_ce,         0,  -6*E*I_ce/l_ce^2,    4*E*I_ce/l_ce;
    ];

% % Test
% [L,~]=computeTransfoBasicToLocal_corot(l_ab,zeros(6,1),zeros(3,1));
% Ktest=L'*Kbar_ab*L;
% test=norm(Ktest-Ke_loc_ab);

%% Initial total structure stiffness matrix
Kg_glob_ab=zeros(6);
Kg_glob_bc=zeros(6);
Kg_glob_bd=zeros(6);
Kg_glob_ce=zeros(6);

Ktot_glob_ab=T_col'*(Ke_loc_ab+Kg_glob_ab)*T_col;
Ktot_glob_bc=T_col'*(Ke_loc_bc+Kg_glob_bc)*T_col;
Ktot_glob_bd=T_beams'*(Ke_loc_bd+Kg_glob_bd)*T_beams;
Ktot_glob_ce=T_beams'*(Ke_loc_ce+Kg_glob_ce)*T_beams;

dof_ab=numEq(1,:);
dof_bc=numEq(2,:);
dof_bd=numEq(3,:);
dof_ce=numEq(4,:);

% Assemble
Ktot=zeros(3*(size(elements, 2)+1));
Ktot(dof_ab,dof_ab)=Ktot(dof_ab,dof_ab)+Ktot_glob_ab;
Ktot(dof_bc,dof_bc)=Ktot(dof_bc,dof_bc)+Ktot_glob_bc;
Ktot(dof_bd,dof_bd)=Ktot(dof_bd,dof_bd)+Ktot_glob_bd;
Ktot(dof_ce,dof_ce)=Ktot(dof_ce,dof_ce)+Ktot_glob_ce;

%% Initialize variables
v_Vector=[];
F_int_Vector=[];
lambda_Vector=[];

F_int=zeros(3*(size(elements, 2)+1),1);
lambda=0;
v=zeros(3*(size(elements, 2)+1),1);
F_unb=zeros(3*(size(elements, 2)+1),1);
F_ext_Tot=zeros(3*(size(elements, 2)+1),1);

DeltaV_u=zeros(3*(size(elements, 2)+1),1);
DeltaV_f=zeros(3*(size(elements, 2)+1),1);

%% Definition of the boundary conditions
alpha=0.01;
F_ext=zeros(3*(size(elements, 2)+1),1);
F_ext(4)=alpha;
F_ext(7)=alpha;
F_ext(8)=-1;
allDofs=[1:3*(size(elements, 2)+1)];
fixedDofs=[1 2 11 14];
freeDOfs=setdiff(allDofs,fixedDofs);

%% Force control
lambda_max=4000000;
nIncrements=100;
DeltaLambdaBar=lambda_max/nIncrements;
tol=1e-4;
nIterMax=1000;

%% Solve the system
n=1;
while n<nIncrements
    converged=0;
    i=1;
    
    % Iterations
    while converged==0
        
        if i==1
            % Determine DeltaV_a and DeltaV_b
            DeltaV_u(freeDOfs)=Ktot(freeDOfs,freeDOfs)\-F_unb(freeDOfs);
            DeltaV_f(freeDOfs)=Ktot(freeDOfs,freeDOfs)\F_ext(freeDOfs);
            % Determine in load multiplier DeltaLambda
            DeltaLambda=DeltaLambdaBar;
        else
            % Determine DeltaV_a and DeltaV_b
            DeltaV_u(freeDOfs)=Ktot(freeDOfs,freeDOfs)\-F_unb(freeDOfs);
            DeltaV_f(freeDOfs)=Ktot(freeDOfs,freeDOfs)\F_ext(freeDOfs);
            % Determine in load multiplier DeltaLambda
            DeltaLambda=0;
        end
        
        %Update of load and displacements
        lambda=lambda+DeltaLambda;
        DeltaV=DeltaV_u+DeltaLambda*DeltaV_f;
        v=v+DeltaV;
        
        % Compute stiffness matrix and resisting force vector
        %Member ab
        u_ab=T_col*v(dof_ab);
        [uBar_ab]=computeTransfoLocalToBasic_corot(l_ab,u_ab);
% [uBar_ab]=computeTransfoLocalToBasic_linear(l_ab,u_ab);
        qBar_ab=Kbar_ab*uBar_ab;
        [L_ab,Kg_ab]=computeTransfoBasicToLocal_corot(l_ab,u_ab,qBar_ab);
% [L_ab,Kg_ab]=computeTransfoBasicToLocal_linear(l_ab);
        %Member bc
        u_bc=T_col*v(dof_bc);
        [uBar_bc]=computeTransfoLocalToBasic_corot(l_bc,u_bc);
% [uBar_bc]=computeTransfoLocalToBasic_linear(l_bc,u_bc);
        qBar_bc=Kbar_bc*uBar_bc;
        [L_bc,Kg_bc]=computeTransfoBasicToLocal_corot(l_bc,u_bc,qBar_bc);
% [L_bc,Kg_bc]=computeTransfoBasicToLocal_linear(l_bc);
        %Member bd
        u_bd=T_beams*v(dof_bd);
        [uBar_bd]=computeTransfoLocalToBasic_corot(l_bd,u_bd);
% [uBar_bd]=computeTransfoLocalToBasic_linear(l_bd,u_bd);
        qBar_bd=Kbar_bd*uBar_bd;
        [L_bd,Kg_bd]=computeTransfoBasicToLocal_corot(l_bd,u_bd,qBar_bd);
% [L_bd,Kg_bd]=computeTransfoBasicToLocal_linear(l_bd);
        %Member ce
        u_ce=T_beams*v(dof_ce);
        [uBar_ce]=computeTransfoLocalToBasic_corot(l_ce,u_ce);
% [uBar_ce]=computeTransfoLocalToBasic_linear(l_ce,u_ce);
        qBar_ce=Kbar_ce*uBar_ce;
        [L_ce,Kg_ce]=computeTransfoBasicToLocal_corot(l_ce,u_ce,qBar_ce);
% [L_ce,Kg_ce]=computeTransfoBasicToLocal_linear(l_ce);
        
        % Assemble globa stiffness matrix
        Ktot=zeros(3*(size(elements, 2)+1));
        Ktot(dof_ab,dof_ab)=Ktot(dof_ab,dof_ab)+T_col'*(L_ab'*Kbar_ab*L_ab+Kg_ab)*T_col;
        Ktot(dof_bc,dof_bc)=Ktot(dof_bc,dof_bc)+T_col'*(L_bc'*Kbar_bc*L_bc+Kg_bc)*T_col;
        Ktot(dof_bd,dof_bd)=Ktot(dof_bd,dof_bd)+T_beams'*(L_bd'*Kbar_bd*L_bd+Kg_bd)*T_beams;
        Ktot(dof_ce,dof_ce)=Ktot(dof_ce,dof_ce)+T_beams'*(L_ce'*Kbar_ce*L_ce+Kg_ce)*T_beams;
        
        
        % Assemble resisting load vector for verification
        F_int=zeros(3*(size(elements, 2)+1),1);
        F_int(dof_ab)=F_int(dof_ab)+T_col'*(L_ab'*qBar_ab);
        F_int(dof_bc)=F_int(dof_bc)+T_col'*(L_bc'*qBar_bc);
        F_int(dof_bd)=F_int(dof_bd)+T_beams'*(L_bd'*qBar_bd);
        F_int(dof_ce)=F_int(dof_ce)+T_beams'*(L_ce'*qBar_ce);
        
        
        % Compute unbalanced force vector
        F_ext_Tot=F_ext_Tot+DeltaLambda*F_ext;
        F_unb=F_int-F_ext_Tot;
        
        % Check for convergence
        test=norm(F_unb(freeDOfs));
        if test<tol %we have converged
            converged=1;
            v_Vector(n,:)=v;
            F_int_Vector(n,:)=F_int;
            lambda_Vector(n)=lambda;
        end
        
        if i==nIterMax && converged==0
            error('Failled to converge')
        end
        i=i+1;
    end
    
    n=n+1;
end



%% Plot the results
h1=figure;
plot(v_Vector(:,7),lambda_Vector/1000,'--k','LineWidth',1.2)
xlabel('u_b [mm]')
ylabel('P [kN]')
grid on

% plot_settings_ASCE(h1)


%% Compare linear vs corotational 
resLinear=importdata('results_week5Exo2_linear.txt');
resCorot=importdata('results_week5Exo2_corot.txt');

h2=figure;
plot(resLinear(:,1),resLinear(:,2)/1000,'-k')
hold on
plot(resCorot(:,1),resCorot(:,2)/1000,'--r','LineWidth',1.2)
legend('Linear','Corotational','Location','best')
xlabel('u_c [mm]')
ylabel('P [kN]')
grid on

plot_settings_ASCE(h2)




%% Export results
% writematrix([v_Vector(:,7),lambda_Vector'], 'results_week5Exo2_linear.txt');




%% Compute linear transformation
function [uBar]=computeTransfoLocalToBasic_linear(l,u)
L = [
    -1, 0, 0, 1, 0, 0;
    0, 1/l, 1, 0, -1/l, 0;
    0, 1/l, 0, 0, -1/l, 1
    ];

uBar=L*u;

end


function [l,Kg]=computeTransfoBasicToLocal_linear(l)
l = [
    -1, 0, 0, 1, 0, 0;
    0, 1/l, 1, 0, -1/l, 0;
    0, 1/l, 0, 0, -1/l, 1
    ];

Kg=zeros(6);

end


%% Compute corotational transformation
function [uBar]=computeTransfoLocalToBasic_corot(l,u)
DeltaUx=u(4)-u(1);
DeltaUy=u(5)-u(2);
Ln=sqrt((l+DeltaUx)^2+(DeltaUy)^2);
beta=atan(DeltaUy/(l+DeltaUx));

u1Bar=Ln-l;
u2Bar=u(3)-beta;
u3Bar=u(6)-beta;
uBar=[u1Bar,u2Bar,u3Bar]';

end


function [L,Kg]=computeTransfoBasicToLocal_corot(l,u,qBar)
DeltaUx=u(4)-u(1);
DeltaUy=u(5)-u(2);
Ln=sqrt((l+DeltaUx)^2+(DeltaUy)^2);
beta=atan(DeltaUy/(l+DeltaUx));

c=cos(beta);
s=sin(beta);

L = [
    -c, -s, 0, c, s, 0;
    -s/Ln, c/Ln, 1, s/Ln, -c/Ln, 0;
    -s/Ln, c/Ln, 0, s/Ln, -c/Ln, 1
    ];

term1 = (qBar(1) / Ln) * [
    s^2, -c*s, 0, -s^2, c*s, 0;
    -c*s, c^2, 0, c*s, -c^2, 0;
    0, 0, 0, 0, 0, 0;
    -s^2, c*s, 0, s^2, -c*s, 0;
    c*s, -c^2, 0, -c*s, c^2, 0;
    0, 0, 0, 0, 0, 0
    ];

term2 = ((qBar(2) + qBar(3)) / Ln^2) * [
    -2*s*c, c^2 - s^2, 0, 2*s*c, -c^2 + s^2, 0;
    c^2 - s^2, 2*c*s, 0, -c^2 + s^2, -2*c*s, 0;
    0, 0, 0, 0, 0, 0;
    2*s*c, -c^2 + s^2, 0, -2*s*c, c^2 - s^2, 0;
    -c^2 + s^2, -2*c*s, 0, c^2 - s^2, 2*c*s, 0;
    0, 0, 0, 0, 0, 0
    ];
Kg=term1+term2;

end