%% CANTILEVER BEAM MODEL :
% 2 meters long 10cm x 10cm cross section area :
% Two horizontal EBFE with a localized force 

clear all ; close all ;
clc; 

%% INPUTS
E = 200e9 ;
S = 0.1*0.1 ;
I = 0.1^3*0.1/12 ;
F = 1000 ;

%% MESH
XY(1,:) = [0 0] ;
XY(2,:) = [1 0] ;
XY(3,:) = [2 0] ;

TC(1,:) = [1 2] ;
TC(2,:) = [2 3] ;

DOF(1,:) = [1 2 3] ;
DOF(2,:) = [4 5 6] ;
DOF(3,:) = [7 8 9] ;


%% FE STIFFNESS MATRICES

% === FE1 FE1 FE1 FE1 FE1 ===
P1 = XY(1,:) ;
P2 = XY(2,:) ;
Le = norm(P2-P1) ;
ke1 = [E*S/Le  0 0 -E*S/Le 0 0 ; ...
      0       12*E*I/Le^3 6*E*I/Le^2 0 -12*E*I/Le^3 6*E*I/Le^2 ; ...
      0       6*E*I/Le^2 4*E*I/Le 0 -6*E*I/Le^2 2*E*I/Le ; ...
      -E*S/Le 0 0 E*S/Le 0 0 ; ...
      0       -12*E*I/Le^3 -6*E*I/Le^2 0 12*E*I/Le^3 -6*E*I/Le^2 ; ...
      0 6*E*I/Le^2 2*E*I/Le 0 -6*E*I/Le^2 4*E*I/Le] ;

% === FE2 FE2 FE2 FE2 FE2 ===
P2 = XY(2,:) ;
P3 = XY(3,:) ;
Le = norm(P3-P2) ;
ke2 = [E*S/Le  0 0 -E*S/Le 0 0 ; ...
      0       12*E*I/Le^3 6*E*I/Le^2 0 -12*E*I/Le^3 6*E*I/Le^2 ; ...
      0       6*E*I/Le^2 4*E*I/Le 0 -6*E*I/Le^2 2*E*I/Le ; ...
      -E*S/Le 0 0 E*S/Le 0 0 ; ...
      0       -12*E*I/Le^3 -6*E*I/Le^2 0 12*E*I/Le^3 -6*E*I/Le^2 ; ...
      0 6*E*I/Le^2 2*E*I/Le 0 -6*E*I/Le^2 4*E*I/Le] ;


%% FE ASSEMBLING
K = zeros(numel(DOF),numel(DOF)) ;

% === FE1 ===
numDOF = [DOF(1,:) DOF(2,:)] ;
K(numDOF,numDOF) = ke1 ;

% === FE2 ===
numDOF = [DOF(2,:) DOF(3,:)] ;
K(numDOF,numDOF) = K(numDOF,numDOF) + ke2 ;



%% LOADING
Fext = zeros(numel(DOF),1) ;
Fext(8) = 100*F ;



%% BC
numDOF_BC = [1 2 3] ;

K(numDOF_BC,:) = [] ;
K(:,numDOF_BC) = [] ;
Fext(numDOF_BC) = [] ;



%% Reso
U = inv(K)*Fext ;
U = [zeros(3,1) ; U] ;

%% Plot 
figure(1) ; clf ; hold on ;
plot(XY(:,1),XY(:,2),'-ko','LineWidth',2) ; 
Ux = U(1:3:end) ;
Uy = U(2:3:end) ;
amp =1 ;
plot(XY(:,1)+amp*Ux,XY(:,2)+amp*Uy,'-ro','LineWidth',2) ; 
xlabel('x (m)')
ylabel('y (m)')
axis equal ; grid on ;