%  Two bar truss with thermal loads by direct assembly.   
%  A_a = A_b = 0.001 m^2    Pin 3 *                  
%  E_a = 200e9 N/m^2                \           plus
%  E_b = 100e9 N/m^2                 (a)        thermal
%  P2y = -20e3 N                       \        loading
%  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.  alpha_a= 20e-6/C, _b=17e-6
%                            dT_a = -20 C, dT_b = 0
nodes = [ 3,  2 ; ...
          1,  2 ]    ; % element connectivity table 
 
%  Application and element dependent controls 
n_e = 2  ; % number of elements 
n_g = 2  ; % number of DOF per node  
n_m = 3  ; % number of nodes in mesh 
n_n = 2  ; % number of nodes per element 
n_r = 1  ; % number of rows in B_e matrix 
n_s = 2  ; % number of space dimensions  
 
n_d = n_g * n_m  ; % system degrees of freedom (DOF) 
n_i = n_g * n_n  ; % number of DOF per element 
 
% Recover numerical data 
E_a = 200e9; E_b = 100e9; A_a = 0.001; A_b = 0.001; 
alpha_a=20e-6; alpha_b = 17e-6; dT_a = -20; dT_b = 0;
% 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;  
 
% System equation numbers:              1  2  3  4  5  6 
% Displacements of system:        U = [U1 V1 U2 V2 U3 V3] 
% Flag zero displacements:  BC_flag = [ 1, 1, 0, 0, 1, 1] 
% External point load flag: Pt_flag = [ 0, 0, 0, 1, 0, 0]
 
% 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 (n_d, n_d) ;  % start stiffness sum at zero 
F = zeros (n_d, 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 
% Thermal load = E A alpha dT [-1    1]'
fa = E_a * A_a * alpha_a * dT_a * [-1    1]' 

% 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 
FA = T_a' * fa             ; % 4 x 1
 
% K = sum_over_e:  Boo_e ^T * K_e * Boo_e 
% F = sum_over_e:  Boo_e ^T * F_e  
 
%      SCATTER TO (ASSEMBLE INTO) SYSTEM ARRAYS 
% Insert completed element matrices into system matrices 
e = 1                                           % element #
e_nodes = nodes (e, 1:n_n)                      % connectivity 
[rows] = get_element_index (n_g, n_n, e_nodes); % eq numbers 
K (rows, rows) = K (rows, rows) + KA   ; % add to system sq 
F (rows)       = F (rows) + FA         ; % add to sys column 
 
% Repeat 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 
% Thermal load = E A alpha dT [-1    1]'
fb = E_b * A_b * alpha_b * dT_b * [-1    1]' 
 
% 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 
FB = T_b' * fb             ; % 4 x 1

% Insert completed element matrices into system matrices 
e = 1                                           % element #
e_nodes = nodes (e, 1:n_n)                      % connectivity 
[rows] = get_element_index (n_g, n_n, e_nodes); % eq numbers 
K (rows, rows) = K (rows, rows) + KB   ; % add to system sq 
F (rows)       = F (rows) + FB           % add to sys column 
 
% End of matrix assembly 
% End   LOOP OVER ELEMENTS <===== <===== <===== <===== <===== 
Load_Eq = n_g * (2 - 1) + 2 ;        % Where is non-zero load 
F (Load_Eq) = F (Load_Eq) + (-20e3)  % Add external P_2_y
 
% K * U = F minimizes energy, but does not yet satisy 
%        the essential displacement boundary condition 
 
K_copy = K; F_copy = F ; % for reaction recovery, small system

for BC_dof = 1:n_d  % 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:n_d, BC_dof) ; % move knowns to RHS 
 
%   partition the matrices or trick them, here use trick 
  K (1:n_d,  BC_dof) = 0        ; % zero known column 
  K (BC_dof, 1:n_d)  = 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 
 
React = K_copy * U - F_copy ; % For small systems only
for BC_dof = 1:n_d  % Loop over reaction recovery   --> --> --> 
  if ( BC_flag (BC_dof) == 0 ) % then dof has no displacement 
    React (BC_dof) = 0 ;       % no reaction here
  end % if BC_flag 
end % for over BC_dof <-- <-- <-- <-- <-- <-- <-- <-- <-- <-- 
disp(React')



% Begin LOOP OVER ELEMENTS =====> =====> =====> =====> =====> 
e = 1                                           % element #
e_nodes = nodes (e, 1:n_n)                      % connectivity 
[rows] = get_element_index (n_g, n_n, e_nodes); % eq numbers 
 
% Extract truss element displacements 
U_a = U(rows)            ;  % four displacements 
% transform back to bar displacements 
Ua_bar = T_a * U_a       ;  % two dispacements 
 
% Get element axial strains. e = e_mech - e_thermal
e_a = [ -1  1] * Ua_bar / L_a         % mechanical strain 
e_a = e_a - alpha_a * dT_a            % thermal strain
 
% Get element axial stresses 
s_a = E_a * e_a               ; % constant stress 

% Get bar axial force + = tension
axial_a = A_a * s_a
 
% Repeat LOOP OVER ELEMENTS  ===== ===== ===== ===== ===== 
e = 2                                         % element # 
e_nodes = nodes (e, 1:n_n)                    % connectivity 
[rows] = get_element_index (n_g, n_n, e_nodes); % eq numbers 
 
% Extract truss element displacements  
U_b = U(rows)             ; % four displacements 
% transform back to bar displacements 
Ub_bar = T_b * U_b        ; % two displacements  
 
% Get element axial strains 
e_b = [ -1  1] * Ub_bar / L_b         % mechanical strain 
e_b = e_b - alpha_b * dT_b            % thermal strain
 
% Get element axial stresses 
s_b = E_b * e_b                  % constant stress 
 
% Get bar axial force + = tension
axial_b = A_b * s_b

% End   LOOP OVER ELEMENTS <===== <===== <===== <===== <===== 
 
% function [rows] = get_element_index (n_g, n_n, e_nodes) 
% % calculate system DOF numbers of element, for gather, scatter 
% rows = zeros (1, n_g*n_n)           ; % allow for node = 0 
% for k = 1:n_n                       ; % loop over element nodes 
%   global_node = round (e_nodes (k)) ; % corresponding sys node 
%   for i = 1:n_g                     ; % loop over DOF at node 
%     eq_global  = i + n_g * (global_node - 1) ; % sys DOF, if any 
%     eq_element = i + n_g * (k           - 1) ; % el DOF number 
%     if ( eq_global > 0 )                ; % check node=0 trick 
%       rows (1, eq_element) = eq_global  ; % valid DOF > 0 
%     end % if allow for omitted nodes 
%   end % for DOF i                       % end local DOF loop 
% end % for each element node             % end local node loop 
% % end get_element_index