%{
  Truss systems: FEM solution (direct matrix assembly)

  clear all; close all; clc; 

  % Assign the characteristics of the composite truss structure:
  % A     : cross-sectional area
  % E     : Young modulus
  % L     : length of the element
  % theta : angle between the element and the global x axis (degrees)
  A     = pi*0.05^2*0.25*ones(3,1);
  E     = 250e9*ones(3,1);
  L     = 3*ones(3,1);
  theta = [60;-60;0];

  % Element-to-nodes connectivity table
  map   = [1 2;2 3;3 1];

  % On output: 
  % "U" : nodal displacements in the global coordinate system. 
  %       The x and y components are stored in "U" following the
  %       sequence of nodes: U1, V1, U2, V2, ....
  %
  % "b" : Nodal reactions on constrained nodes
  %       The x and y components are stored in "b" following the
  %       sequence of nodes: Rx1, Ry1, Rx2, Ry2, ....
  %
  % "Actions" : loads on the beams, stored following the
  %             sequence of elements. Each row of
  %             "Actions" contains the x and y global components of
  %             the loads acting on the first and on the second node
  %             of the element (they should be equilibrated):
  %             Ax1, Ay1, Ax2, Ay2
  %
  % "Actions_l" : loads on the beams, stored following the
  %             sequence of elements. Each row of
  %             "Actions" contains the x and y local components of
  %             the loads acting on the first and on the second node
  %             of the element (they should be equilibrated):
  %             Ax1, Ay1, Ax2, Ay2
  %
  % "sigma", "epsilon" : stress and deformation for each element


  [U,b,Actions,Actions_l,sigma,epsilon]=truss_system(A,E,L,theta,map);
%}
function [U,b,Actions,Actions_l,sigma,epsilon]=truss_system(A,E,L,theta,map)

% Connectivity between elements and degrees of freedom.
% In this case there are 2 degrees of freedom per node.
mapd  = [repmat(map(:,1),[1,2]).*repmat([2,2],[size(map,1),1])-repmat([1,0],[size(map,1),1]) , ...
    repmat(map(:,2),[1,2]).*repmat([2,2],[size(map,1),1])-repmat([1,0],[size(map,1),1])];

% External force. Running through columns means running through nodes.
% Running along columns means looking at Fx then Fy.
F = [0   0    0;
     0 -1e4   0];

% Constraints: node, "u" or "v", value
constr = {1 'v' 0.;3 'u' 0;3 'v' 0};

% Number of elements, number of nodes, number of degrees of freedom
Nel    = size(map,1);
Nnodes = max(map(:));
Ndofs  = 2*Nnodes;

% Allocate global stiffness matrix and load vector
Kappa = sparse(Ndofs,Ndofs);
b     = zeros(Ndofs,1);

for e=1:Nel
    % Assembly of local stiffness matrix in the global coordinate system
    [KE,~] = local_stiffness_rotated(A(e),E(e),L(e),theta(e));
    
    Kappa(mapd(e,:),mapd(e,:)) = Kappa(mapd(e,:),mapd(e,:)) + KE;
end

b = F(:);

% Store for post-processing
Kappa_stored = Kappa;
b_stored     = b;

% Assign boundary conditions on displacement
[Kappa,b] = enforce_constraints(Ndofs,Kappa,b,constr);

% Solve for nodal displacements
U = Kappa\b;

% Nodal reactions
b = Kappa_stored*U - b_stored;

% Actions on the trusses: Fx1, Fy1, Fx2, Fy2
Actions   = zeros(Nel,4);
Actions_l = zeros(Nel,4);
for e=1:Nel
  [KE,Te] = local_stiffness_rotated(A(e),E(e),L(e),theta(e));
  Actions(e,:) = (KE*U(mapd(e,:))).';
  Actions_l(e,:) = (Te.')*Actions(e,:).';

  % Stress
  sigma(e) = -Actions_l(e,1)/A(e);

  % Deformation
  epsilon(e) = sigma(e)/E(e);
end



end

function [KE,Te] = local_stiffness_rotated(A,E,L,theta)

    % Local stiffness matrix (in the coordinate system solidal with the
    % element)
    ke = A*E/L;
    Ke = [ke 0 -ke 0;zeros(1,4);-ke 0 ke 0;zeros(1,4)];
    % Rotation matrices
    Re = [cosd(theta),-sind(theta);sind(theta),cosd(theta)];
    Te = [Re,zeros(2,2);zeros(2,2),Re];
    % Local stiffness matrix (in the global coordinate system)
    KE = Te*Ke*(Te.');

end

function [Kappa,b] = enforce_constraints(Ndofs,Kappa,b,constr)

for k=1:size(constr,1)
    nn = constr{k,1};
    if strcmp(constr(k,2),'u')
        j=2*nn-1;
    else
        j=2*nn;
    end
    b(j) = constr{k,3};
    b((1:Ndofs)~=j) = b((1:Ndofs)~=j)-Kappa((1:Ndofs)~=j,j)*constr{k,3};
    Kappa(:,j) = 0;
    Kappa(j,:) = 0;
    Kappa(j,j) = 1;
end

end