% Introduction of Finite Elements in Engineering Mechanics 
% Dr. X. Frank Xu, Stevens Institute of Technology

clear
%----------------------
%Mesh Information
%----------------------
NumNP = 4; %number of nodal points
NumEL = 2; % number of elements
NP=[1 3 2;1 4 3]; %Nodes in elements from 1 to NumEL
XORD(1:NumNP,1)=[0,0,20,20]'; % X-coordinate of nodal points
YORD(1:NumNP,1)=[0,10,10,0]'; % Y-coordinate of nodal points

%----------------------
%Specify Boundary Conditions
%----------------------
NumEQ = 2*NumNP;
U=zeros(NumEQ,1);
F=zeros(NumEQ,1);
NPBC=zeros(NumEQ,1);
NPBC(1)=1; %X-Displacement of Node 1 is constrained
NPBC(2)=1; %Y-Displacement of Node 1 is constrained
NPBC(2*2-1)=1; %X-Displacement of Node 2 is constrained
NPBC(2*2)=1; %Y-Displacement of Node 2 is constrained
U(1)=0; % Specified X-displacement for Node 1 
U(2)=0; % Specified Y-displacement for Node 1 
U(2*2-1)=0; % Specified X-displacement for Node 2 
U(2*2)=0; % Specified Y-displacement for Node 2 

F(2*3-1)=5000; % Specified X-force for Node 3 (unit:lb)
F(2*4-1)=5000; % Specified X-force for Node 4 (unit:lb)
%----------------------
%Specify Material Properties
%----------------------
E=30e6; % Young's modulus (unit:Pa)
MU=.3; % Poisson'a ratio 
D=E/(1-MU^2)*[1 MU 0;MU 1 0;0 0 (1-MU)/2]; %plane stress
%----------------------
% Form Stiffness Matrix
%----------------------
K=zeros(NumEQ, NumEQ); 
for I=1:NumEL
    Ke=zeros(4,4);
    XJ=XORD(NP(I,2))-XORD(NP(I,1));
    YJ=YORD(NP(I,2))-YORD(NP(I,1));
    XK=XORD(NP(I,3))-XORD(NP(I,1));
    YK=YORD(NP(I,3))-YORD(NP(I,1));
    XC=XORD(NP(I,1))+(XJ+XK)/3.0;
    YC=YORD(NP(I,1))+(YJ+YK)/3.0;
    
    VOL=(XJ*YK-XK*YJ)/2.0 ;
    
    COMM=1.0/(2.0*VOL);

    DNDX(1)=-(YK-YJ)*COMM; 
    DNDX(2)=+(YK   )*COMM;
    DNDX(3)=-(YJ   )*COMM;
    DNDY(1)=+(XK-XJ)*COMM;
    DNDY(2)=-(XK   )*COMM;
    DNDY(3)=+(XJ   )*COMM;
    B=[DNDX(1) 0 DNDX(2) 0 DNDX(3) 0;0 DNDY(1) 0 DNDY(2) 0 DNDY(3);DNDY(1) DNDX(1) DNDY(2) DNDX(2) DNDY(3) DNDX(3)];
    Ke = B'*D*B*VOL;
  
    pos=[2*NP(I,1)-1,2*NP(I,1),2*NP(I,2)-1,2*NP(I,2),2*NP(I,3)-1,2*NP(I,3)];
    K(pos,pos)=K(pos,pos)+Ke; % Global matrix
    
end
K0=K;
%--------------------------------------
% IMPLEMENTATION OF BOUNDARY CONDITIONS
%--------------------------------------
for I=1:NumEQ
    if NPBC(I)==1 
       K(I,I)=K(I,I)*1E9; % Big number approach
       F(I)=K(I,I)*U(I);
    end
end

%----------------------
% Solve displacement
%----------------------
U = K\F;

%-----------------------------------------
% Post-processing to obtain stress 
%-----------------------------------------
EP=zeros(3,NumEL);
S=zeros(3,NumEL);
for I=1:NumEL
    pos=[2*NP(I,1)-1,2*NP(I,1),2*NP(I,2)-1,2*NP(I,2),2*NP(I,3)-1,2*NP(I,3)];
    XJ=XORD(NP(I,2))-XORD(NP(I,1));
    YJ=YORD(NP(I,2))-YORD(NP(I,1));
    XK=XORD(NP(I,3))-XORD(NP(I,1));
    YK=YORD(NP(I,3))-YORD(NP(I,1));
    XC=XORD(NP(I,1))+(XJ+XK)/3.0;
    YC=YORD(NP(I,1))+(YJ+YK)/3.0;
    
    VOL=(XJ*YK-XK*YJ)/2.0 ;
    
    COMM=1.0/(2.0*VOL);

    DNDX(1)=-(YK-YJ)*COMM; 
    DNDX(2)=+(YK   )*COMM;
    DNDX(3)=-(YJ   )*COMM;
    DNDY(1)=+(XK-XJ)*COMM;
    DNDY(2)=-(XK   )*COMM;
    DNDY(3)=+(XJ   )*COMM;
    B=[DNDX(1) 0 DNDX(2) 0 DNDX(3) 0;0 DNDY(1) 0 DNDY(2) 0 DNDY(3);DNDY(1) DNDX(1) DNDY(2) DNDX(2) DNDY(3) DNDX(3)];
    EP(:,I)=B*U(pos);
    S(:,I)=D*EP(:,I);
end