function [] = truss2D
%------------------------------------------------------------
% 2D linear truss elements with 2 Degrees Of Freedom at each node.
% Imad Barsoum & Jonas Faleskog /KTH Solid Mechanics/2005-Feb-2/
%----------------------------------------------------------------
% Variables:
%    Problem size
%       Nn:         total number of nodes
%       Ne:         total number of elements
%       No_of_Eqns: total number of equations (degrees of freedom)
%    Vectors & Matrices:
%      node:    Columns: 1 = node #, 2 = x-coord, 3 = y-coord
%      element: Columns: 1 = element #,  2 = 1st node #, 
%                        3 = 2nd node #, 4 = spring constant
%      D:   global displacement vector
%      F:   global force vector
%------------------------------------------------------------
% 1. Read in-put data from file = truss2D.inp
%       The global point force vector, Fs, is established here!
fid = fopen('truss2D.inp','r');
[node,element,Dzero,Fs,elem_force,Nn,Ne] = read_input(fid);
fclose(fid);
%------------------------------------------------------------
% 2. Assembly of (i) global stiffness matrix of the system of trusses
%       and (ii) global force vector.
%       Loop over all elements (compute the element stiffness matrix
%       for each element in the global coordinate system and add it
%       to the system stiffness matrix).
%       Define the dimension of the matrix and set it to zero.
%
%       Element stiffnes matrix in global coordinate system
%           Ke(i,j)  dimension: 4x4 
%       System stiffnes matrix in global coordinate system
%           Ksystem(i,j)  dimension: No_of_Eqns x No_of_Eqns
%
No_of_Eqns = 2*Nn;
Fb = zeros(No_of_Eqns,1);
Ksystem = zeros(No_of_Eqns,No_of_Eqns);
for e = 1 : Ne
    Enode1 = round( element(e,1) ); Enode2 = round( element(e,2) ); 
    Eemod = element(e,3); Earea = element(e,4);
    [Ke] = ElemStiffMatrix(Eemod,Earea,node(Enode1,:),node(Enode2,:));
    [Fbe] = ElemVolumeForces(elem_force(e,:),Earea,node(Enode1,:),node(Enode2,:));
    ekv(2) = 2*Enode1;
    ekv(1) = ekv(2) - 1;
    ekv(4) = 2*Enode2;
    ekv(3) = ekv(4) - 1;
    for i = 1 : 4
        k = ekv(i);
        Fb(k) = Fb(k) + Fbe(i);
        for j = 1 : 4
            l = ekv(j);
            Ksystem(k,l) = Ksystem(k,l) + Ke(i,j);
        end
    end
end
F = Fs + Fb;
%------------------------------------------------------------
% 3. Enforce zero displacment boundary conditions by setting the
%      appropriate columns and rows to zero in the system stiffness
%      matrix. Actual reduction of the system matrix can be avoided
%      by setting the diagonal term of constrained D.O.F. to unity.
Kmod = Ksystem
for i = 1 : No_of_Eqns
    if( Dzero(i) == 1 )
        Kmod(i,:) = 0;
        Kmod(:,i) = 0;
        Kmod(i,i) = 1.0;
        F(i) = 0;
    end
end
%------------------------------------------------------------
% 4. Solve the Equation system
D = Kmod\F
%------------------------------------------------------------
% 5. Compute the reaction forces (this is simply done by multiplying
%        the displacement vector by the full system stiffness matrix)
F = Ksystem*D;
%------------------------------------------------------------
% 6. Compute the element stresses
[sigma] = elem_stress(D,element,node,Ne);
%------------------------------------------------------------
% 7. Plot the deformed and the undeformed configuration
dmag = input('Give magnification factor for displacements in plot: ');
plot_result(node,element,Nn,Ne,D,F,dmag);
%------------------------------------------------------------
% 8. Generating the output file truss2D.out where the results are listed
gen_output(node,element,sigma,D,F,Fb,Dzero);
%*************************************************************
%           FUNCTIONS     and     SUBROUTINES
%*************************************************************
% Compute the element stiffness matrix in the global coordinate
%    system for the element in question. 
function [Ke] = ElemStiffMatrix(E,A,node1,node2)
%  Retrieve coordinates of element nodes
x1 = node1(1);  x2 = node2(1);  y1 = node1(2);  y2 = node2(2);
%  Element length
le = sqrt( (x2-x1)^2 + (y2-y1)^2 );
%  Stiffness of the truss element
truss_stiffness = E*A/le;
%  Direction cosines
c = (x2-x1) / le;   s = (y2-y1) / le;
k_c2 = c*c * truss_stiffness;
k_s2 = s*s * truss_stiffness;
k_cs = c*s * truss_stiffness;
%  Element stiffness matrix transformed to global coordinate system
Ke = [ k_c2   k_cs   -k_c2   -k_cs ; ...
       k_cs   k_s2   -k_cs   -k_s2 ; ...
      -k_c2  -k_cs    k_c2    k_cs ; ...
      -k_cs  -k_s2    k_cs    k_s2 ;];
%*************************************************************
% Compute consistent nodal forces given a volume force acting along
%  the element on the form: Kx = q0 + q1*(x/le) + q2*(x/le)^2
%  Transform to the global coordinate system
function [Fb] = ElemVolumeForces(elem_force,A,node1,node2)
Fb = zeros(4,1);
if( elem_force(1) ~= 0 )
% Retrieve coordinates of element nodes
    x1 = node1(1);  x2 = node2(1);  y1 = node1(2);  y2 = node2(2);
% Element length
    le = sqrt( (x2-x1)^2 + (y2-y1)^2 );
% Direction cosines
    c = (x2-x1) / le;  s = (y2-y1) / le;
% Compute node forces in local coordinate system
    q0 = elem_force(2); q1 = elem_force(3); q2 = elem_force(4);
    f1 = A*le/2.0 * (q0 + q1/3.0 + q2/6.0);
    f2 = A*le/2.0 * (q0 + 2.0*q1/3.0 + q2/2.0);
% Consistent nodal forces transformed to global coordinates
    Fb = [ c*f1; s*f1; c*f2; s*f2;];
end
%*************************************************************
%  This routine generates a plot of deformed and undeformed configuration
function plot_result(node,element,Nn,Ne,D,F,dmag)
hold on
% Undeformed configuration
%  Plot nodes
for i = 1 : Nn
    x = node(i,1);  y = node(i,2);  plot(x,y,'ob');
end
%  Plot elements
for i = 1 : Ne
    Enode1 = round( element(i,1) ); Enode2 = round( element(i,2) ); 
    x = [node(Enode1,1),node(Enode2,1)];
    y = [node(Enode1,2),node(Enode2,2)];
    line(x,y)
end
% Deformed configuration
%  Plot nodes
for i = 1 : Nn
    x = node(i,1)+dmag*D(2*i-1); y = node(i,2)+dmag*D(2*i);  plot(x,y,'or');
end
%  Plot elements
for i = 1 : Ne
    Enode1 = round( element(i,1) ); Enode2 = round( element(i,2) ); 
    x = [node(Enode1,1) + dmag*D(2*Enode1-1), node(Enode2,1) + dmag*D(2*Enode2-1)];
    y = [node(Enode1,2) + dmag*D(2*Enode1),   node(Enode2,2) + dmag*D(2*Enode2)];
    line(x,y)
end
%*************************************************************
% This routine reads in-put data from the file "truss2D.inp"
function [node,element,Dzero,Fs,elem_force,Nn,Ne] = read_input(fid)
data = [];
%
% Node numbers and coordinates
data = str2num(fgetl(fid));
data = str2num(fgetl(fid));
while (length(data) > 0)
    Nnum = round(data(1));
    node(Nnum,1:2) = data(2:3);
    data = str2num(fgetl(fid));
end
Nn = length(node(:,1));
%
% Elements (connectivity, E & A)
data = str2num(fgetl(fid));
while (length(data) > 0)
    Enum = round(data(1));
    element( Enum, 1:4) = data(2:5);
    data = str2num(fgetl(fid));
end
Ne = length(element(:,1));
%
% Zero displacement Boundary Conditions
Dzero = zeros(2*Nn,1);
data = str2num(fgetl(fid));
while (length(data) > 0)
    if (data(2) == 1)
        Dzero(2*data(1)-1) = 1;
    elseif (data(2) == 2)
        Dzero(2*data(1)) = 1;
    end
    data = str2num(fgetl(fid));
end
%
% Applied point forces
Fs = zeros(2*Nn,1);
data = str2num(fgetl(fid));
while (length(data) > 0)
    if (data(2) == 1)
        Fs(2*data(1)-1)  = data(3);
    elseif (data(2) == 2)
        Fs(2*data(1)) = data(3);
    end
    data = str2num(fgetl(fid));
end
%
% Applied volume forces
elem_force = zeros(Ne,4);
data = str2num(fgetl(fid));
while (length(data) > 0)
    elem_force(data(1),:) = data;
    data = str2num(fgetl(fid));
end
return
%*************************************************************
% This routine calculates the stress in each element
function [sigma] = elem_stress(D,element,node,Ne)
sigma = zeros(Ne,1);
%  Retrieve coordinates of element nodes
for e = 1 : Ne
    x1 = node(element(e,1),1);  x2 = node(element(e,2),1);  
    y1 = node(element(e,1),2);  y2 = node(element(e,2),2);
%  Element length
    le = sqrt( (x2-x1)^2 + (y2-y1)^2 );
    B = [-1/le 1/le];
%  Young's modulus
    E = element(e,3);
%  Direction cosines
    c = (x2-x1)/le;  s = (y2-y1)/le;  T = [c s 0 0 ; 0 0 c s];
    De = [D(2*element(e,1)-1); D(2*element(e,1)); ...
          D(2*element(e,2)-1); D(2*element(e,2))];
% Stress in e:th element
    sigma(e) = E*B*T*De;
end
%*************************************************************
% This routine generates an output file where the results are listed
function gen_output(node,element,sigma,D,F,Fb,Dzero)
fid = fopen('truss2D.out','w+');
t = clock;
fprintf(fid,'%s\n','****************************************************');
fprintf(fid,'%s%4.2f\n',['File: truss2D.out   Date: ',date,'   Time: '], t(4) + .01*t(5));
fprintf(fid,'%s\n','****************************************************');
fprintf(fid,'%s\n','------------- External Load Vectors ----------------');
fprintf(fid,'%s\n','Node           Fx                  Fy');
for i = 1 : length(node(:,1))
    if ( Dzero(2*i-1)==0 & Dzero(2*i)==0 ) 
       fprintf(fid,'%u%s%16.6e%s%16.6e\n',i,'    ',F(2*i-1),'    ',F(2*i));
    elseif  ( Dzero(2*i-1)==0 & Dzero(2*i)==1 )
       fprintf(fid,'%u%s%16.6e%s\n',i,'    ',F(2*i-1),'              - ');
    elseif  ( Dzero(2*i-1)==1 & Dzero(2*i)==0 )
       fprintf(fid,'%u%s%16.6e\n',i,'              -         ',F(2*i));
    elseif  ( Dzero(2*i-1)==1 & Dzero(2*i)==1 )
       fprintf(fid,'%u%s\n',i,'              -                   -');
    end
end
fprintf(fid,'%s\n','------------- Nodal Displacements ------------------');
fprintf(fid,'%s\n','Node           Dx                  Dy');
for i = 1 : length(node(:,1))
    fprintf(fid,'%u%s%16.6e%s%16.6e\n',i,'    ',D(2*i-1),'    ',D(2*i));
end
fprintf(fid,'%s\n','------------- Reaction Forces ----------------------');
fprintf(fid,'%s\n','Node           Rx                  Ry');
R=F-Fb;
for i = 1 : length(node(:,1))
    if ( Dzero(2*i-1)==1 & Dzero(2*i)==1 ) 
       fprintf(fid,'%u%s%16.6e%s%16.6e\n',i,'    ',R(2*i-1),'    ',R(2*i));
    elseif  ( Dzero(2*i-1)==1 & Dzero(2*i)==0 )
       fprintf(fid,'%u%s%16.6e%s\n',i,'    ',R(2*i-1),'              - ');
    elseif  ( Dzero(2*i-1)==0 & Dzero(2*i)==1 )
       fprintf(fid,'%u%s%16.6e\n',i,'              -         ',R(2*i));
    elseif  ( Dzero(2*i-1)==0 & Dzero(2*i)==0 )
       fprintf(fid,'%u%s\n',i,'              -                   -');
    end
end
fprintf(fid,'%s\n','------------- Element Stresses ---------------------');
fprintf(fid,'%s\n','Element           S');
for i = 1 : length(sigma(:,1))
    fprintf(fid,'%u%s%16.6e\n',i,'    ',sigma(i));
end
fclose(fid);
disp(' *** The result is listed in the output file truss2D.out ***');
%*************************************************************