%  Two bar truss finite element model.  
%  A_a = A_b = 0.001 m^2    Pin 3 *                 
%  E_a = 200e9 N/m^2                \
%  E_b = 100e9 N/m^2                 (a)
%  P2y = -20e3 N                       \
%  node  x  y(m)            Pin 1 *-----* 2
%     1  0  0    elem j  k          (b) |                
%     2 1.5 0       a 3  2              v P                  
%     3  0  2       b 1  2

% Recover numerical data
E_a = 200e9; E_b = 100e9; A_a = 0.001; A_b = 0.001;
% Compute geometry
L_a = 2.5; C_a = 1.5/L_a; S_a = -2.0/L_a;
L_b = 2.0; C_b = 1;       S_b = 0; 

% Displacements of system:       U = [U1 V1 U2 V2 U3 V3]
% Flag zero displacements: BC_flag = [ 1, 1, 0, 0, 1, 1];
% Displacements of elements: U_a = [U3 V3 U2 V2]
% Displacements of elements: U_b = [U1 V1 U2 V2]
% Boolean matrix connection definitions: U_e = Boo_e * U

% Set displacement boundary condition flags and value
BC_flag = [ 1, 1, 0, 0, 1, 1]; BC_value = 0 ;

% Allocate dynamic memory for large system equations
K = zeros (6, 6) ;  % start stiffness sum at zero
F = zeros (6, 1) ;  % start loadings  sum at zero

% Begin LOOP OVER ELEMENTS =====> =====> =====> =====> =====>

% Constant element stiffness & load arrays: K_e, F_e
ka = E_a * A_a / L_a     ; % axial stiffness of 'a'
Ka = ka * [ 1,   -1; ...
           -1,    1]      % stiffness matrix 2 x 2
% fa = fb = [0,  0]. No distributed member loads  
% skip null element load vector           

% Expand 2x2 bar stiffness to truss member 4x4
% Compute transformation to system x-y axes
T_a = [C_a,  S_a,   0,   0; ...
         0,    0, C_a, S_a] 
KA = T_a' * Ka * T_a         % 4 x 4

% K = sum_over_e:  Boo_e ^T * K_e * Boo_e
% F = sum_over_e:  Boo_e ^T * F_e 

% Look up element's nodes in mesh table, set non-zeros 
Boo_a = [0,  0,  0,  0,  1,  0 ; ...  % x-disp 3
         0,  0,  0,  0,  0,  1 ; ...  % y-disp 3
         0,  0,  1,  0,  0,  0 ; ...  % x-disp 2
         0,  0,  0,  1,  0,  0 ] ;    % y-disp 2

K_a = Boo_a' * KA * Boo_a  % expand to system size, 6 x 6
% skip F_a = Boo_a' * Fa          

K = K + K_a                % sum rows and columns
% skip F = F + F_a                % sum rows 

% Repeate LOOP OVER ELEMENTS
kb = E_b * A_b / L_b     ; % axial stiffness of 'b'
Kb = kb * [ 1,   -1; ...
           -1,    1]      % stiffness matrix 2 x 2
% skip null element load vector           

% Expand 2x2 bar stiffness to truss member 4x4
% Compute transformation to system x-y axes
T_b = [C_b,  S_b,   0,   0; ...
         0,    0, C_b, S_b] 
KB = T_b' * Kb * T_b         % 4 x 4

Boo_b = [1,  0,  0,  0,  0,  0 ; ...  % x-disp 1
         0,  1,  0,  0,  0,  0 ; ...  % y-disp 1
         0,  0,  1,  0,  0,  0 ; ...  % x-disp 2
         0,  0,  0,  1,  0,  0 ] ;    % y-disp 2

K_b = Boo_b' * KB * Boo_b  % expand to system size, 6 x 6
% skip F_b = Boo_b' * Fb         

K = K + K_b                % sum rows and columns
% skip F = F + F_b                % sum rows 
% End of matrix assembly
% End   LOOP OVER ELEMENTS <===== <===== <===== <===== <=====
F = F + [0, 0, 0, -20e3, 0, 0]' % Add external applied loads

% K * U = F minimizes energy, but does not yet satisy
%        the essential displacement boundary condition

for BC_dof = 1:6  % Loop over possible BC changes --> --> -->
  if ( BC_flag (BC_dof) == 1 ) % then dof has no displacement
%   Enforce displacement boundary condition(s)
  F = F - BC_value* K (1:6, BC_dof) ; % move knowns to RHS

%    SOLVE  MATRIX EQUILIBRIUM EQUATIONS, with unique BC
%   partition the matrices or trick them, here use trick
  K (1:6, BC_dof   ) = 0        ; % zero known column
  K (BC_dof, 1:6)    = 0        ; % BC_dof = BC_value identity
  K (BC_dof, BC_dof) = 1        ; % diagonal in blank row, col
  F (BC_dof)         = BC_value ; % identity done, end trick
  end % if BC_flag
end % for over BC_dof <-- <-- <-- <-- <-- <-- <-- <-- <-- <--
disp(K) ; disp(F)

% Solve system equations, which now are non-singular
U = K \ F  % All displacements known. Now post-process elements

% Begin LOOP OVER ELEMENTS =====> =====> =====> =====> =====>
% Extract truss element displacements
U_a = Boo_a * U             % four displacements
% transform back to bar displacements
Ua_bar = T_a * U_a          % two dispacements

% Get element end force reactions (optional)
Ra_xy = KA * U_a % - Fa    % four forces external to element
% transform back to bar loads
Ra_bar = T_a * Ra_xy       % two forces external to element 

% Get element (centroid) strains
e_a = [ -1  1] * Ua_bar / L_a  % mid-length strain

% Get element (centroid) stresses
s_a = E_a * e_a               % mid-length stress

% Repeat LOOP OVER ELEMENTS  ===== ===== ===== ===== =====

% Extract truss element displacements 
U_b = Boo_b * U             % four displacements
% transform back to bar displacements
Ub_bar = T_b * U_b          % two displacements 

% Extract truss element reactions 
Rb_xy  = KB * U_b % - Fb  % four forces external to element
% transform back to bar reactions
Rb_bar = T_b * Rb_xy      % two forces external to element

% Get element (centroid) strains
e_b = [ -1  1] * Ub_bar / L_b  % mid-length strain

% Get element (centroid) stresses
s_b = E_b * e_b               % mid-length stress

% End   LOOP OVER ELEMENTS <===== <===== <===== <===== <=====

% Done