%% CANTILEVER BEAM MODEL :
% 2 meters long 10cm x 10cm cross section area :
% Two inclined 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 = 1 ;

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

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) ;
% LOCAL frame
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] ;
% GLOBAL frame
ex = (P2-P1)/norm(P2-P1) ; ex = ex' ;
ey = [-ex(2) ; ex(1)]  ;

Pass = zeros(3) ;
Pass(1:2,1:2) = [ex ey] ;
Pass(3,3) = 1 ;
Pass6 = [Pass zeros(3,3) ; zeros(3,3) Pass] ; % Passage matrix

Ke1 = Pass6*ke1*Pass6' ;

% === FE2 FE2 FE2 FE2 FE2 ===
P2 = XY(2,:) ;
P3 = XY(3,:) ;
Le = norm(P3-P2) ;
% LOCAL frame
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] ;

% GLOBAL frame
ex = (P3-P2)/norm(P3-P2) ; ex = ex' ;
ey = [-ex(2) ; ex(1)]  ;

Pass = zeros(3) ;
Pass(1:2,1:2) = [ex ey] ;
Pass(3,3) = 1 ;
Pass6 = [Pass zeros(3,3) ; zeros(3,3) Pass] ; 

Ke2 = Pass6*ke2*Pass6' ;



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

% === FE1 ===
numDOF_FE1 = [DOF(1,:) DOF(2,:)] ;
K(numDOF_FE1,numDOF_FE1) = Ke1 ;

% === FE2 ===
numDOF_FE2 = [DOF(2,:) DOF(3,:)] ;
K(numDOF_FE2,numDOF_FE2) = K(numDOF_FE2,numDOF_FE2) + Ke2 ;



%% LOADING
Fext = zeros(numel(DOF),1) ;
Fext(5) = 100000*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] ;



%% POST PROCESSING

% Deformed shape
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 ;

return


FAIREE DESSIN sortir EFFORTS INTERNES DANS REPERE LOCAL

% Internal efforts
Ke1*U(numDOF_FE2)