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

clear
%----------------------
%Mesh Information
%----------------------
NumNP = 3; %number of nodal points
NumEL = 2; % number of elements
NP=[1 2;2 3]; %Nodes in elements from 1 to NumEL
XORD(1:NumNP,1)=[0,4,4+1E-6]'; % X-coordinate of nodal points
YORD(1:NumNP,1)=[0,3,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*(3-1)+1)=1; %X-Displacement of Node 3 is constrained
NPBC(2*(3-1)+2)=1; %Y-Displacement of Node 3 is constrained
U(1)=0; % Specified X-displacement for Node 1 
U(2)=0; % Specified Y-displacement for Node 1 
U(2*(3-1)+1)=0; % Specified X-displacement for Node 3 
U(2*(3-1)+2)=0; % Specified Y-displacement for Node 3 

F(2*(2-1)+1)=1000; % Specified X-force for Node 2 (unit:N)

%----------------------
%Specify Material Properties
%----------------------
E(1:NumEL,1)=1e11; % Young's modulus (unit:Pa)
A(1:NumEL,1)=1e-4; % Cross section area(unit:m2) 

%----------------------
% Form Stiffness Matrix
%----------------------
K=zeros(NumEQ, NumEQ); 
for I=1:NumEL
    Ke=zeros(4,4);
    L(I)=((XORD(NP(I,1))-XORD(NP(I,2)))^2+(YORD(NP(I,1))-YORD(NP(I,2)))^2)^0.5;
    BarAngle(I)=atan2((YORD(NP(I,2))-YORD(NP(I,1))),(XORD(NP(I,2))-XORD(NP(I,1))));
    C=cos(BarAngle(I));
    S=sin(BarAngle(I));
    Ke=A(I)*E(I)/L(I)*[C*C C*S -C*C -C*S;C*S S*S -S*C -S*S;-C*C -C*S C*C C*S;-C*S -S*S S*C S*S]; %Element stiffness matrix
    for J=1:2  
        PosK(J*2-1)=NP(I,J)*2-1;% Position of Ke in global stiffness matrix K 
        PosK(2*J)=NP(I,J)*2; % Position of Ke in global stiffness matrix K 
    end
    K(PosK,PosK)=K(PosK,PosK)+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 and reaction forces
%-----------------------------------------
for I=1:NumEL
    npx1=2*NP(I,1)-1;
    npy1=2*NP(I,1);
    npx2=2*NP(I,2)-1;
    npy2=2*NP(I,2);
    C=cos(BarAngle(I));
    S=sin(BarAngle(I));
    stress(I)=E(I)/L(I)*[-C -S C S]*[U(npx1) U(npy1) U(npx2) U(npy2)]';% Stress of element I
    force(I)=stress(I)*A(I); % Force of element I
end
FBC=K0*U; % Boundary forces  
       
%--------------------------------------------- 
% Plot deformed figure of truss
%---------------------------------------------
x_disp(1:NumNP,1)=U(1:2:2*NumNP);
y_disp(1:NumNP,1)=U(2:2:2*NumNP);
xdef(1:NumNP)=XORD(1:NumNP)+x_disp(1:NumNP)*100
ydef(1:NumNP)=YORD(1:NumNP)+y_disp(1:NumNP)*100

for i = 1:NumEL
        xx = [XORD(NP(i,1)) XORD(NP(i,2))]; 
        yy = [YORD(NP(i,1)) YORD(NP(i,2))]; 
        line(xx,yy);
        hold on;
end    

for i = 1:NumEL
        xx = [xdef(NP(i,1)) xdef(NP(i,2))]; 
        yy = [ydef(NP(i,1)) ydef(NP(i,2))]; 
        line(xx,yy,'LineWidth',2,'Color',[.8 .8 .8]);
        hold on;
end