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

clear
%----------------------
%Mesh Information
%----------------------
NumNP = 51; %number of nodal points
NumEL = 50; % number of elements
NP=zeros(NumEL,2);
for i=1:NumEL
   NP(i,:)=[i i+1]; %Nodes in elements from 1 to NumEL
end
XORD(1:NumNP,1)=[zeros(1,30) 0:.1:2]'; % X-coordinate of nodal points
YORD(1:NumNP,1)=[0:.1:3 zeros(1,20)+3]'; % Y-coordinate of nodal points

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

NPBC=zeros(3*NumNP,1);
UBC=zeros(NumNP,3);
NPBC(1)=1; %X-Displacement of Node 1 is constrained
NPBC(2)=1; 
NPBC(152)=1; 

UBC(1,1)=0; % Specified X-displacement for Node 1 
UBC(1,2)=0;  
UBC(51,2)=0;  
  
%----------------------
%Specify Material Properties
%----------------------
E(1:NumEL,1)=200e9; % Young's modulus (unit:Pa)
G=77.5e9;
mu=0.29;
ks=5/6;
A(1:NumEL,1)=0.01; % Cross section area(unit:m2)
II(1:NumEL,1)=0.1^4/12;
rho=7860;
%--------------------------------------
% IMPLEMENTATION OF BOUNDARY CONDITIONS
%--------------------------------------

 NumEQ=0;
 
 for I=1:NumNP
   for n=1:3
      if NPBC(3*I-3+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(51,1),1)=1000; % Specified X-force for Node 2 (unit:lb)

%----------------------
% Form Stiffness Matrix
%----------------------
K=zeros(NumEQ, NumEQ); 
for I=1:NumEL
    Ke=zeros(6,6);
    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));
    phi=12*E(I)*II(I)/A(I)/G/L(I)^2/ks;
    Ke(1,:)=E(I)/L(I)/(1+phi)*[A(I)*C*C+12*II(I)*S^2/L(I)^2  C*S*(A(I)-12*II(I)/L(I)^2)    -6*II(I)*S/L(I) -A(I)*C*C-12*II(I)*S*S/L(I)^2 (-A(I)+12*II(I)/L(I)^2)*S*C    -6*II(I)*S/L(I)];
    Ke(2,:)=E(I)/L(I)/(1+phi)*[C*S*(A(I)-12*II(I)/L(I)^2)    A(I)*S*S+12*II(I)*C^2/L(I)^2  6*II(I)*C/L(I)  -A(I)*S*C+12*II(I)*C*S/L(I)^2 -A(I)*S*S-12*II(I)*C*C/L(I)^2  6*II(I)*C/L(I)];
    Ke(3,:)=E(I)/L(I)/(1+phi)*[-6*II(I)*S/L(I)               6*II(I)*C/L(I)                (4+phi)*II(I)         6*II(I)*S/L(I)                -6*II(I)*C/L(I)             (2-phi)*II(I)];
    Ke(4,:)=E(I)/L(I)/(1+phi)*[-A(I)*C*C-12*II(I)*S*S/L(I)^2 -A(I)*S*C+12*II(I)*C*S/L(I)^2 6*II(I)*S/L(I)  A(I)*C*C+12*II(I)*S^2/L(I)^2  A(I)*C*S-12*II(I)*S*C/L(I)^2   6*II(I)*S/L(I)];
    Ke(5,:)=E(I)/L(I)/(1+phi)*[-A(I)*C*S+12*II(I)*S*C/L(I)^2 -A(I)*S*S-12*II(I)*C*C/L(I)^2 -6*II(I)*C/L(I) A(I)*C*S-12*II(I)*S*C/L(I)^2  A(I)*S*S+12*II(I)*C*C/L(I)^2   -6*II(I)*C/L(I)];
    Ke(6,:)=E(I)/L(I)/(1+phi)*[-6*II(I)*S/L(I)               6*II(I)*C/L(I)               (2-phi)*II(I)        6*II(I)*S/L(I)                -6*II(I)*C/L(I)             (4+phi)*II(I)]; %%Element stiffness matrix
    Me=zeros(6,6);
    Me(1,1)=rho*A(I)*L(I)/2;
    Me(2,2)=rho*A(I)*L(I)/2;
    Me(4,4)=rho*A(I)*L(I)/2;
    Me(5,5)=rho*A(I)*L(I)/2;
    Me=rho*A(I)*L(I)*[1/3 0 0 1/6 0 0;0 156/420 22*L(I)/420 0 54/420 -13*L(I)/420;0 22*L(I)/420 4*L(I)^2/420 0 13*L(I)/420 -3*L(I)^2/420;...
                      1/6 0 0 1/3 0 0;0 54/420 13*L(I)/420 0 156/420 -22*L(I)/420;0 -13*L(I)/420 -3*L(I)^2/420 0 -22*L(I)/420 4*L(I)^2/420]; 
    Tt=[C -S 0 0 0 0;S C 0 0 0 0;0 0 1 0 0 0;0 0 0 C -S 0;0 0 0 S C 0;0 0 0 0 0 1];
    Me=Tt*Me*Tt';
    for J1=1:2
        for J2=1:3
            for K1=1:2
                for K2=1:3
                    if NPBC(3*NP(I,J1)-3+J2)==0
                        if NPBC(3*NP(I,K1)-3+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(3*J1-3+J2,3*K1-3+K2);
                            M(bc(NP(I,J1),J2), bc(NP(I,K1),K2))=M(bc(NP(I,J1),J2), bc(NP(I,K1),K2))+Me(3*J1-3+J2,3*K1-3+K2);
                        else
                            F(bc(NP(I,J1),J2),1)=F(bc(NP(I,J1),J2),1)-Ke(3*J1-3+J2,3*K1-3+K2)*UBC(NP(I,K1),K2);
                        end
                    end
                end
            end
        end
    end
end
K0=K;

[vector f]=eigs(K,M,5,'SM');
U=K\F;