%#########################################################################
%%%%% Version 2.0 #######  MOHAMED ABDOU MAHRAN KASEM ##################
% Ph.D. Aerospace Engineering Department, Cairo Uiniversity
% Egypt (17th Oct. 2015)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%% Finite Element Program %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%    Truss Structure     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear, clc, close all
% the main objective of this program is to solve general truss using the FEM
% to obtain the truss reactions and displacements

%% Preprocessing
tic
%% $$$$$$$$$$$$$$$$$$$$$ Pre-Processing $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
%-------------------------------------------------------------------------

%----------------------
% Variables Description
%----------------------
%-----------------------------------------------------------------------
%  Variable      |                     Description
%-----------------------------------------------------------------------
%   coord        |                  nodes coordinates matrix
%    nnod        |                   number of nodes
%    elem        |                 element conectivity matrix
%     nel        |                 number of elements
%     E          |                  Elastic Modulus
%     nu         |                   Poison ratio
%     k          |     element stiffness matrix in global coordinates
%     F          |                 Global force vector
%     D          |               Global displacement vector
%     B          |             the strain-displacement matrix
%     Ds         |             the stress-displacement matrix
%     del        |            the element nodal displacement matrix
%     fel        |            the element nodal reaction matrix
%     eps        |            the strain on the elements (3,iel)
%    sigma       |            the element stress (3,iel)
%    nfix        |               number of fixed dof
%    dof         |                    all dofs
%     Le         |                 element length
%    theta       |                element orientation angle
%    fixn        |                     fixed nodes
%-----------------------------------------------------------------------

%-----------------------------------
% Input data for control parameters
%-----------------------------------
% Inputs: Coord, elem, E, nu, A, fixn
L = 1;
coord = [0 0;L 0;L L];  % nodes' coordinates [x y]
elem  = [1 2; 2 3; 1 3]; % element connectivity [node(1) node(2)]
nel   = length(elem);
nnod  = length(coord);

% Material Properties - Steel
E     = 100e9;   %[Pa]
nu    = 0.29;
rho   = 8000;    %[kg/m3]
% cross section area
A     = 0.0005*ones(1, nel); %[m2]

nnel  = 2;           % number of nodes per element
ndof  = 2;           % number of dofs per node
sdof  = nnod*ndof;   % total system dof
dof   = 1:1:sdof;
edof  = nnel*ndof;   % total element dofs

%-----------------------------------
% Input data for Boundary conditions
%-----------------------------------
fixn    = [1 2];
fixdof  = [fixn*2 - 1   fixn*2];
nfix    = length(fixdof);
freedof = setdiff(dof, fixdof);
%---------------------------------------
% Initialization of matrices and vectors
%---------------------------------------
F     = zeros(sdof,1);
D     = zeros(sdof,1);
%--------------
% applied force
%--------------
F(5) = 100000; %[N]
F(6) = -200000; %[N]

%% Processing

%----------------------------
% The Global stiffness matrix
%----------------------------
[K, Vel]   = truss_stiff(elem,coord,E,A, nel, edof, sdof);

%-------------------------------------
% global stiffness matrix partitioning
%-------------------------------------
% nf = freedof         nd = fixdof
%  -    -     -                  -   -     -
% | Fnf  |   |  Knf,und , Knf,nd  | |  Dund  |
% |      | = |                    | |        |
% | Funf |   |  Kunf,und, Kunf,nd | |   Dnd  |
%  -    -     -                  -   -     -
Fn     = F(freedof);               % the known forces (Load)
Kun_n  = K(fixdof,fixdof);         % the stiffness matrix that relates the unknown forces with the known displacement
Kun_un = K(fixdof,freedof);        % the stiffness matrix that relate the unknown forces with the unknown displacement
Kn_n   = K(freedof,fixdof);        % the stiffness matrix that relate the known forces with the known displacement
Kn_un  = K(freedof, freedof);      % the stiffness matrix that relate the unknown forces with the known displacement
Dn     = D(fixdof);                % the given constraints

%----------------
% system solution
%----------------
%  -  -      -     -   -  -     -     -   -   -
% | Fn | =  |  Kn,n | | Dn | + | Kn,un | | Dun |
%  -  -      -     -   -  -     -     -   -   -
dd  = Fn - Kn_n*Dn;
Dun = Kn_un\dd;              % calculate the unknown displacement
% -   -      -     -    -  -      -     -   -   -
%| Fun | =  | Kun,un | | Dun | + | Kun,n | | Dn  |
% -   -      -     -    -  -      -     -   -   -
Fun = Kun_n*Dn + Kun_un*Dun;       % calculate the unknown forces
F(fixdof)  = Fun;
D(freedof) = Dun;
%-------------------------------------
% the element forces and displacements
%-------------------------------------
[del, fel_local, sigma] = dis_truss(D, E,A, nel, elem, edof, coord);
% Structural Weight[Ib]
W = rho*sum(Vel);

%% $$$$$$$$$$$$$$$$$$$$$$$ Post-Processing $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
%--------------------------------------------------------------------------

figure(1);hold on;
for i = 1:nel
    line(coord(elem(i,:),1),coord(elem(i,:),2),'linewidth',2);
    line(coord(elem(i,:),1) + 10*del([1,3],i),coord(elem(i,:),2) + 10*del([2,4],i),...
        'linewidth',2,'color','r');   % the deformation multiplied by a factor to be clear on the screen
end
% nodes numbering
for j = 1:nnod
    text(coord(j,1), coord(j,2), num2str(j));
end
axis equal; axis off
title('Truss Structure','linewidth',3)
disp('The reaction forces vector[Ib]: ')
disp(F')
disp('The displacement vector[m]: ')
disp(D')
disp('The maximum displacement[in]: ')
[Dmax, index] = max(D);
disp(Dmax)

time = toc