% 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 = 5; % number of elements
NP=[1 2;2 3;1 3;3 4;1 4]; %Nodes in elements from 1 to NumEL
XORD(1:NumNP,1)=[0,0,50,50]'; % X-coordinate of nodal points
YORD(1:NumNP,1)=[0,50,50,0]'; % Y-coordinate of nodal points

%----------------------
%Specify Boundary Conditions
%----------------------

NPBC=zeros(NumNP*2,1);
UBC=zeros(NumNP,2);
NPBC(1)=1; %X-Displacement of Node 1 is constrained
NPBC(2)=1; %Y-Displacement of Node 1 is constrained
NPBC(2*4)=1; %Y-Displacement of Node 4 is constrained
UBC(1,1)=0; % Specified X-displacement for Node 1 
UBC(1,2)=0; % Specified Y-displacement for Node 1 
UBC(4,2)=0; % Specified Y-displacement for Node 4 


%----------------------
%Specify Material Properties
%----------------------
E(1:NumEL,1)=30e6; % Young's modulus (unit:Pa)
A(1:NumEL,1)=30; % Cross section area(unit:in2) 
rho=7.4e-4;
%--------------------------------------
% IMPLEMENTATION OF BOUNDARY CONDITIONS
%--------------------------------------

 NumEQ=0;
 
 for I=1:NumNP
   for n=1:2
      if NPBC(2*I-2+n)==0
         NumEQ=NumEQ+1;
         bc(I,n)=NumEQ;
      else
         bc(I,n)=UBC(I,n);
      end
   end
 end
 
K=zeros(NumEQ, NumEQ); 
M=zeros(NumEQ, NumEQ);
U=zeros(NumEQ,1);
F=zeros(NumEQ,1);
F(bc(3,1),1)=10000; % Specified X-force for Node 2 (unit:lb)

%----------------------
% Form Stiffness Matrix
%----------------------

for I=1:NumEL
    Ke=zeros(4,4);
    Me=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
    Me=rho*A(I)*L(I)/2*diag([1,1,1,1]); 
    
    for J1=1:2
        for J2=1:2
            for K1=1:2
                for K2=1:2
                    if NPBC(2*NP(I,J1)-2+J2)==0
                        if NPBC(2*NP(I,K1)-2+K2)==0
                            K(bc(NP(I,J1),J2), bc(NP(I,K1),K2))=K(bc(NP(I,J1),J2), bc(NP(I,K1),K2))+Ke(2*J1-2+J2,2*K1-2+K2);
                            M(bc(NP(I,J1),J2), bc(NP(I,K1),K2))=M(bc(NP(I,J1),J2), bc(NP(I,K1),K2))+Me(2*J1-2+J2,2*K1-2+K2);
                        else
                            F(bc(NP(I,J1),J2),1)=F(bc(NP(I,J1),J2),1)-Ke(2*J1-2+J2,2*K1-2+K2)*UBC(NP(I,K1),K2);
                        end
                    end
                end
            end
        end
    end    
end
K0=K;

%  ----------------------
%  Solve equations in multiple steps of time 
%  ----------------------
   nstep=1000;
   t=0.01;
   dt=t/nstep;
 
   F(1:NumEQ,2:nstep)=zeros(NumEQ,nstep-1);
   V=zeros(NumEQ,nstep);
   U=zeros(NumEQ,nstep);
   AC=zeros(NumEQ,nstep);
   def=zeros(2*NumNP,nstep);
   
   AC(:,1)=M\(F(:,1)-K*U(:,1));
   UP=U(:,1)-dt*V(:,1)+dt^2/2*AC(:,1);
   U(:,2)=M\(dt^2*F(:,1)+(2*M-dt^2*K)*U(:,1)-M*UP);
   
  for n=3:nstep
      U(:,n)=M\(dt^2*F(:,n-1)+(2*M-dt^2*K)*U(:,n-1)-M*U(:,n-2));
      AC(:,n-1)=M\(F(:,n-1)-K*U(:,n-1));
      V(:,n-1)=(U(:,n)-U(:,n-2))/2/dt;
      for I=1:NumNP
       if NPBC(2*I-1)==0
         def(2*I-1,n)=U(bc(I,1),n);
       else def(2*I-1,n)=bc(I,1);
       end
        if NPBC(2*I)==0
         def(2*I,n)=U(bc(I,2),n);
         else def(2*I,n)=bc(I,2);
        end
     end
  end
  % Frequency
  [vector f]=eigs(K,M);
  figure,plot([1:nstep],U(1,:),[1:nstep],U(2,:),[1:nstep],U(3,:),[1:nstep],U(4,:),[1:nstep],U(5,:))