function [Kmod,M] = frame2D
%------------------------------------------------------------
% % Imad Barsoum & Jonas Faleskog /KTH Solid Mechanics/2006-Feb-8/
%----------------------------------------------------------------
% 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
%      element: Columns: 1 = 1st node #, 2 = 2nd node #, 3 = E modulus,
%                        4 = Area, 5 = momen of intertia Iz
%      D:   global displacement vector
%      F:   global force vector
%------------------------------------------------------------
% 1. Read in-put data from file = frame2D.inp
%       The global force vector is established here!
fid = fopen('frame2D.inp','r');
[node,element,Dzero,Fs,elem_axial_force,elem_perp_force,Nn,Ne] = read_input(fid);
fclose(fid);
%------------------------------------------------------------
% 2. Assembly of (i) global stiffness matrix of the frame
%       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: 6x6 
%       System stiffnes matrix in global coordinate system
%           Ksystem(i,j)  dimension: No_of_Eqns x No_of_Eqns
%
No_of_Eqns = 3*Nn;
Fbd = 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); EIz = element(e,5);
    [Ke] = ElemStiffMatrix(Eemod,Earea,EIz,node(Enode1,:),node(Enode2,:));
    [Fbde] = ElemDistForces(elem_axial_force(e,:),elem_perp_force(e,:),node(Enode1,:),node(Enode2,:));
    ekv(3) = 3*Enode1;
    ekv(2) = ekv(3) - 1;
    ekv(1) = ekv(2) - 1;
    ekv(6) = 3*Enode2;
    ekv(5) = ekv(6) - 1;
    ekv(4) = ekv(5) - 1;
    for i = 1 : 6
        k = ekv(i);
        Fbd(k) = Fbd(k) + Fbde(i);
        for j = 1 : 6
            l = ekv(j);
            Ksystem(k,l) = Ksystem(k,l) + Ke(i,j);
        end
    end
end
F = Fs + Fbd;
%------------------------------------------------------------
% 3. Enforce zero displacment/rotation 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/moments (this is simply done by multiplying
%    the displacement vector by the full system stiffness matrix)
F = Ksystem*D
%------------------------------------------------------------
% 6. Plot the deformed and the undeformed configuration
dmag = input('Give magnification factor for displacements in plot: ');
[M] = plot_result(node,element,Nn,Ne,D,F,dmag);
%------------------------------------------------------------
% 7. Generating the output file frame2D.out where the results are listed
gen_output(node,element,D,F,Fbd,Dzero);
%*************************************************************
%           FUNCTIONS     and     SUBROUTINES
%*************************************************************
% Compute the element stiffness matrix 
function [Ke] = ElemStiffMatrix(E,A,Iz,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);  a = le/2;
%  Stiffness of the beam/truss
kb = E*Iz/(2*a^3);
kt = E*A/le;
%  Element stiffness matrix
Ke = [ kt        0          0         -kt        0          0        ;
        0        3*kb       3*a*kb      0       -3*kb       3*a*kb   ;
        0        3*a*kb     4*a^2*kb    0       -3*a*kb     2*a^2*kb ;
      -kt        0          0          kt        0          0        ;
        0       -3*kb      -3*a*kb      0        3*kb      -3*a*kb   ;
        0        3*a*kb     2*a^2*kb    0       -3*a*kb     4*a^2*kb ;];
%  Direction cosines
T = eye(6,6);
c = (x2-x1)/le;   s = (y2-y1)/le;
T2 = [c s ; -s c];
T(1:2,1:2) = T2;  T(4:5,4:5) = T2;
%  Global element stiffness matrix
Ke = T'*Ke*T;
%*************************************************************
% Compute consistent nodal forces given a distributed force acting along
%  the element on the form: fd = q0 + q1*(x/le) + q2*(x/le)^2
function [Fbde] = ElemDistForces(elem_axial_force,elem_perp_force,node1,node2)
fperp  = zeros(6,1);
faxial = zeros(6,1);
T = eye(6,6);
% 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);  a = le/2;
%  Direction cosines
c = (x2-x1)/le;   s = (y2-y1)/le;
T2 = [c s ; -s c];
T(1:2,1:2) = T2;  T(4:5,4:5) = T2;
%
% Element distributed forces (Perpendicular)
if (elem_perp_force(1) ~= 0)
% Compute node forces
    qi_perp = elem_perp_force(2); qj_perp = elem_perp_force(3); 
    fperp_i = a*qi_perp * [0.0 ; 0.7 ; 0.2*a  ; 0.0 ; 0.3 ; -2*a/15];
    fperp_j = a*qj_perp * [0.0 ; 0.3 ; 2*a/15 ; 0.0 ; 0.7 ;  -0.2*a];
    fperp =  fperp_i + fperp_j;
end
%
% Element distributed forces (Axial)
if (elem_axial_force(1) ~= 0)
% Compute node forces
    pi_axial = elem_axial_force(2); pj_axial = elem_axial_force(3);
    faxial_i = a*(2*pi_axial +   pj_axial)/3;
    faxial_j = a*(  pi_axial + 2*pj_axial)/3;
% Consistent nodal forces transformed to global coordinates
    faxial = [faxial_i; 0 ; 0 ; faxial_j ;  0  ;  0 ;];
end
%  Global force vector
Fbde = T'*(fperp + faxial);
%*************************************************************
%  This routine generates a plot of deformed and undeformed configuration
function [M] = plot_result(node,element,Nn,Ne,D,F,dmag)
% Undeformed configuration
%  Plot nodes
for i = 1 : Nn
    x = node(i,1);  y = node(i,2); figure(1); plot(x,y,'ob'); hold on
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
xi = linspace(-1,1,100);  T = eye(6,6);
u = zeros(1,length(xi));  w = zeros(1,length(xi));  M = []; 
for i = 1 : Ne
    Emod = element(i,3);  Iz = element(i,5);
    Enode1 = round(element(i,1));  Enode2 = round(element(i,2)); 
    % Retrieve coordinates of element nodes
    x1 = node(Enode1,1);  x2 = node(Enode2,1);  y1 = node(Enode1,2);  y2 = node(Enode2,2);
    xe = linspace(x1,x2,length(xi));   
    ye = linspace(y1,y2,length(xi));   
    % Element length
    le = sqrt((x2-x1)^2+(y2-y1)^2);  a = le/2;
    %  Direction cosines
    c = (x2-x1)/le;   s = (y2-y1)/le;    T2 = [c  s ; -s  c]; 
    T(1:2,1:2) = T2;   T(4:5,4:5) = T2;
    De = [D(3*Enode1-2 : 3*Enode1) ; D(3*Enode2-2 : 3*Enode2)];
    de = T*De;
    for j = 1 : length(xi)
        % Determine the Displacement w
        Nb1 = (2 - 3*xi(j) + xi(j)^3)./4;
        Nb2 = (1 - xi(j) - xi(j)^2 + xi(j)^3)*a./4;
        Nb3 = (2 + 3*xi(j) - xi(j)^3)./4;
        Nb4 = (-1 - xi(j) + xi(j)^2 + xi(j)^3)*a./4;
        % Determine the Displacement u
        Nd1 = (1 - xi(j))/2;
        Nd2 = (1 + xi(j))/2;
        % Global displacements U & W
        N = [Nd1    0    0   Nd2     0     0;
               0  Nb1  Nb2     0   Nb3   Nb4;];
        UW = T2'*N*de;    u(j) = UW(1);    w(j) = UW(2);
        xe_def = xe + dmag*u;
        ye_def = ye + dmag*w;
        % Determine the moments
        N1_2 =  3*xi(j)./2;
        N2_2 =  a*(3*xi(j)-1)./2;
        N3_2 = -3*xi(j)./2;
        N4_2 =  a*(1+3*xi(j))./2;
        N_biss = [0 N1_2 N2_2 0 N3_2 N4_2];  M_xi(j) = -Emod*Iz*N_biss*de;

    end
    plot(xe_def,ye_def,'-.b'); title('Undeformed and deformed configuration'); hold on;
    M = [M M_xi];
end
% axis([min(x_coord) max(x_coord) min(v_disp) max(v_disp)]);
%*************************************************************
% This routine reads in-put data from the file "frame2D.inp"
function [node,element,Dzero,Fs,e_axialdistforce,e_perpdistforce,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:5) = data(2:6);
    data = str2num(fgetl(fid));
end
Ne = length(element(:,1));
%
% Zero disp./rotation Boundary Conditions
Dzero = zeros(3*Nn,1);
data = str2num(fgetl(fid));
while (length(data) > 0)
    if (data(2) == 1)
        Dzero(3*data(1)-2) = 1;
    elseif (data(2) == 2)
        Dzero(3*data(1)-1) = 1;
    elseif (data(2) == 3)
        Dzero(3*data(1)) = 1;
    end
    data = str2num(fgetl(fid));
end
%
% Applied point forces/moments
Fs = zeros(3*Nn,1);
data = str2num(fgetl(fid));
while (length(data) > 0)
    if (data(2) == 1)
        Fs(3*data(1)-2)  = data(3);
    elseif (data(2) == 2)
        Fs(3*data(1)-1) = data(3);
    elseif (data(2) == 3)
        Fs(3*data(1)) = data(3);
    end
    data = str2num(fgetl(fid));
end
%
% Distributed element forces (Perpendicular)
e_perpdistforce = zeros(Ne,3);
data = str2num(fgetl(fid));
while (length(data) > 0)
    e_perpdistforce(data(1),:) = data;
    data = str2num(fgetl(fid));
end
% Distributed element forces (Axial)
e_axialdistforce = zeros(Ne,3);
data = str2num(fgetl(fid));
while (length(data) > 0)
    e_axialdistforce(data(1),:) = data;
    data = str2num(fgetl(fid));
end
return
%*************************************************************
% This routine generates an output file where the results are listed
function gen_output(node,element,D,F,Fbd,Dzero)
fid = fopen('frame2D.out','w+');
t = clock;
fprintf(fid,'%s\n','****************************************************');
fprintf(fid,'%s%4.2f\n',['File: frame2D.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                  Fz                  My');
for i = 1 : length(node(:,1))
    if ( Dzero(3*i-2)==0 & Dzero(3*i-1)==0 & Dzero(3*i)==0 ) 
        fprintf(fid,'%u%s%18.10g%s%18.10g%s%18.10g\n',i,'    ',F(3*i-2),'    ',F(3*i-1),'    ',F(3*i));
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==0 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%18.10g%s%18.10g%s\n',i,'    ',F(3*i-2),'    ',F(3*i-1),'              - ');
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==1 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%18.10g%s%s%18.10g\n',i,'    ',F(3*i-2),'              - ','    ',F(3*i));
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==0 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%s%18.10g%s%18.10g\n',i,'              - ','    ',F(3*i-1),'    ',F(3*i));
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==1 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%s%s%18.10g\n',i,'              - ','              - ','    ',F(3*i));
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==0 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%s%18.10g%s%s\n',i,'              - ','    ',F(3*i-1),'    ','              - ');
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==1 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%18.10g%s%s%s\n',i,'    ',F(3*i-2),'    ','              - ','    ','              - ');
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==1 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%s%s\n',i,'              - ','              - ','              - ');
    end
end
fprintf(fid,'%s\n','------------- Nodal DOF ------------------');
fprintf(fid,'%s\n','Node           Dx                  Dz                  Thetay');
for i = 1 : length(node(:,1))
    fprintf(fid,'%u%s%18.10g%s%18.10g%s%18.10g\n',i,'    ',D(3*i-2),'    ',D(3*i-1),'    ',D(3*i));
end

R = F-Fbd;
fprintf(fid,'%s\n','------------- Reaction Forces/Moments ----');
fprintf(fid,'%s\n','Node           Rx                  Rz                  My');
for i = 1 : length(node(:,1))
    if ( Dzero(3*i-2)==1 & Dzero(3*i-1)==1 & Dzero(3*i)==1 ) 
        fprintf(fid,'%u%s%18.10g%s%18.10g%s%18.10g\n',i,'    ',R(3*i-2),'    ',R(3*i-1),'    ',R(3*i));
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==1 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%18.10g%s%18.10g%s\n',i,'    ',R(3*i-2),'    ',R(3*i-1),'              - ');
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==0 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%18.10g%s%s%18.10g\n',i,'    ',R(3*i-2),'              - ','    ',R(3*i));
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==1 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%s%18.10g%s%18.10g\n',i,'              - ','    ',R(3*i-1),'    ',R(3*i));
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==0 & Dzero(3*i)==1 )
        fprintf(fid,'%u%s%s%s%18.10g\n',i,'              - ','              - ','    ',R(3*i));
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==1 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%s%18.10g%s%s\n',i,'              - ','    ',R(3*i-1),'    ','              - ');
    elseif  ( Dzero(3*i-2)==1 & Dzero(3*i-1)==0 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%18.10g%s%s%s\n',i,'    ',R(3*i-2),'    ','              - ','    ','              - ');
    elseif  ( Dzero(3*i-2)==0 & Dzero(3*i-1)==0 & Dzero(3*i)==0 )
        fprintf(fid,'%u%s%s%s\n',i,'              - ','              - ','              - ');
    end
end
fclose(fid);
disp(' *** The result is listed in the output file frame2D.out ***');
%*************************************************************