%% Systematic truss analysis
%
% One dimensional rod with two displacements and two forces.
% Recall a spring force: F=k\;x, now we apply this on our case
% N=k\;\underset{\delta }{\underbrace{\left(u_2 -u_1 \right)} }
% where k=\frac{E\;A}{L}. With N=f_2 and f_1 =-N we can write
% \left\lbrace \begin{array}{ll}
% f_2 =k\left(u_2 -u_1 \right) & \\
% f_1 =k\left(u_1 -u_2 \right) &
% \end{array}\right.
% or on matrix form
% k\left\lbrack \begin{array}{cc}
% 1 & -1\\
% -1 & 1
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% u_1 \\
% u_2
% \end{array}\right\rbrack =\left\lbrack \begin{array}{c}
% f_1 \\
% f_2
% \end{array}\right\rbrack
% or
% {\mathit{\mathbf{K}}}^e \mathit{\mathbf{u}}=\mathit{\mathbf{f}}
% where {\mathit{\mathbf{K}}}^e \;is known as the element stiffness matrix.
% Now, consider some arbitrary rod in a two dimensional setting.
%
% Each node can be translated in the x- and y-direction and each node can carry loads in x- and y-directions.
% We shall derive the element stiffness matrix for this rod, but it is easier to first derive it in local coordinates and then transform it into the global coordinates.
%
% Each rod has a local coordinate system x^{\prime } and y^{\prime } and local node numbers 1 and 2. The local coordinate system is rotated compared to the global coordinate system, and the angle is denoted by \theta. In the local coordinates we have
% \left\lbrace \begin{array}{ll}
% k\left(u_1^{\prime } -u_3^{\prime } \right) & =f_1^{\prime } \\
% 0 & =f_2^{\prime } \\
% k\left(u_3^{\prime } -u_1^{\prime } \right) & =f_3^{\prime } \\
% 0 & =f_4^{\prime }
% \end{array}\right.
% We have zero force normal to the rod since the rod only can carry axial forces. The above relation can be written on matrix form
% k\left\lbrack \begin{array}{cccc}
% 1 & 0 & -1 & 0\\
% 0 & 0 & 0 & 0\\
% -1 & 0 & 1 & 0\\
% 0 & 0 & 0 & 0
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% u_1^{\prime } \\
% u_2^{\prime } \\
% u_3^{\prime } \\
% u_4^{\prime }
% \end{array}\right\rbrack =\left\lbrack \begin{array}{c}
% f_1^{\prime } \\
% f_2^{\prime } \\
% f_3^{\prime } \\
% f_4^{\prime }
% \end{array}\right\rbrack
% or
% K^{e^{\prime } } u^{e^{\prime } } =f^{e^{\prime } } (1)
% known as the local element stiffness matrix.
%% The transformation matrix
% We need a way to transform u^{e^{\prime } } to u_i and u_j. In other words, we need a relation between u^{e^{\prime } } and u_i and u_j
%
% \left\lbrace \begin{array}{ll}
% u_i^x =u_1^{\prime } \cos \theta -u_2^{\prime } \sin \theta  & \\
% u_i^y =u_1^{\prime } \sin \theta +u_2^{\prime } \cos \theta  & \\
% u_j^x =u_3^{\prime } \cos \theta -u_4^{\prime } \sin \theta  & \\
% u_j^y =u_3^{\prime } \sin \theta +u_4^{\prime } \cos \theta  &
% \end{array}\right.
% which on matrix form is
% \left\lbrack \begin{array}{c}
% u_i^x \\
% u_i^y \\
% u_j^x \\
% u_j^y
% \end{array}\right\rbrack =\left\lbrack \begin{array}{cccc}
% \cos \theta  & -\sin \theta  & 0 & 0\\
% \sin \theta  & \cos \theta  & 0 & 0\\
% 0 & 0 & \cos \theta  & -\sin \theta \\
% 0 & 0 & \sin \theta  & \cos \theta
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% u_1^{\prime } \\
% u_2^{\prime } \\
% u_3^{\prime } \\
% u_4^{\prime }
% \end{array}\right\rbrack
% or
% {\mathit{\mathbf{u}}}^e ={\mathit{\mathbf{L}}}^T {\mathit{\mathbf{u}}}^{e^{\prime } }
% where {\mathit{\mathbf{L}}}^T is known as the transformation matrix, and we recognize the rotation matrix within. The transformation matrix has interesting properties: {\mathit{\mathbf{L}}}^T \mathit{\mathbf{L}}=\mathit{\mathbf{L}}\;{\mathit{\mathbf{L}}}^T =\mathit{\mathbf{I}} and {\mathit{\mathbf{L}}}^{-1} ={\mathit{\mathbf{L}}}^T.
% Using the transformation matrix we can transform local quantities to global
% {\mathit{\mathbf{u}}}^e ={\mathit{\mathbf{L}}}^T {\mathit{\mathbf{u}}}^{e^{\prime } } \Leftrightarrow {\mathit{\mathbf{u}}}^{e^{\prime } } =\mathit{\mathbf{L}}\;{\mathit{\mathbf{u}}}^e (2)
% {\mathit{\mathbf{f}}}^e ={\mathit{\mathbf{L}}}^T {\mathit{\mathbf{f}}}^{e^{\prime } } \Leftrightarrow f^{e^{\prime } } =\mathit{\mathbf{L}}\;{\mathit{\mathbf{f}}}^e (3)
% Insert 2 and 3 into 1
% {\mathit{\mathbf{L}}}^T {\mathit{\mathbf{K}}}^{e^{\prime } } \mathit{\mathbf{L}}\;{\mathit{\mathbf{u}}}^e ={\mathit{\mathbf{f}}}^e
% or
% {\mathit{\mathbf{K}}}^e {\mathit{\mathbf{u}}}^e ={\mathit{\mathbf{f}}}^e
% where {\mathit{\mathbf{K}}}^e is known as the element stiffness matrix.
%% How to compute the tranformation matrix {\mathit{\mathbf{L}}}^T
% One could determine the angle \theta. But there is a cheaper way. Let \mathit{\mathbf{r}} define the rod vector such that
% \mathit{\mathbf{r}}={\mathit{\mathbf{p}}}_j -{\mathit{\mathbf{p}}}_i
%
% then the normalized vector
% \hat{\mathit{\mathbf{r}}} =\frac{\mathit{\mathbf{r}}}{|\mathit{\mathbf{r}}|}=\left\lbrack \begin{array}{c}
% \cos \theta \\
% \sin \theta
% \end{array}\right\rbrack =:\left\lbrack \begin{array}{c}
% c\\
% s
% \end{array}\right\rbrack
% such that we get
% {\mathit{\mathbf{L}}}^T =\left\lbrack \begin{array}{cccc}
% c & -s & 0 & 0\\
% s & c & 0 & 0\\
% 0 & 0 & c & -s\\
% 0 & 0 & s & c
% \end{array}\right\rbrack
% and
% K^e =k\left\lbrack \begin{array}{cccc}
% c^2  & c\;s & -c^2  & -c\;s\\
% c\;s & s^2  & -c\;s & -s^2 \\
% -c^2  & -c\;s & c^2  & c\;s\\
% c\;s & -s^2  & c\;s & s^2
% \end{array}\right\rbrack
% We note that the element stiffness matrix is symmetric.
% Does is matter how the rod vector is defined? The vector can be defined either by \mathit{\mathbf{r}}={\mathit{\mathbf{p}}}_2 -{\mathit{\mathbf{p}}}_1 or \mathit{\mathbf{r}}={\mathit{\mathbf{p}}}_1 -{\mathit{\mathbf{p}}}_2 and we get two situations.
%
%
% It turns out that {\mathit{\mathbf{K}}}^e is independent of the how the rod vector (or angle \theta) is defined, both alternatives yield the same result.
%% Rod forces, deformations, strains and stresses
%
%
% The deformation of the rod is given by
% \begin{array}{l}
% \delta =\Delta u=\left(u_j^x \cos \theta +u_j^y \sin \theta \right)-\left(u_i^x \cos \theta +u_i^y \sin \theta \right)\\
% \;\\
% \delta =\underset{\mathit{\mathbf{T}}}{\underbrace{\left\lbrack \begin{array}{cccc}
% -c & -s & c & s
% \end{array}\right\rbrack } } \left\lbrack \begin{array}{c}
% u_i^x \\
% u_i^y \\
% u_j^x \\
% u_j^y
% \end{array}\right\rbrack =\mathit{\mathbf{T}}\cdot {\mathit{\mathbf{u}}}^e
% \end{array}
% where \mathit{\mathbf{T}} is called the transformation vector.
% We have the rod element rod force
% N=\frac{A\;E}{L}\delta =k\;\mathit{\mathbf{T}}\cdot {\mathit{\mathbf{u}}}^e
% and the element load vector (or reaction forces)
% \begin{array}{l}
% {\mathit{\mathbf{f}}}^e ={\mathit{\mathbf{T}}}^T N=k\;{\mathit{\mathbf{T}}}_{4\times 1}^T {\mathit{\mathbf{T}}}_{1\times 4} {\;\mathit{\mathbf{u}}}_{4\times 1}^e =k\left\lbrack \begin{array}{c}
% -c\\
% -s\\
% c\\
% s
% \end{array}\right\rbrack \left\lbrack \begin{array}{cccc}
% -c & -s & c & s
% \end{array}\right\rbrack \left\lbrack \begin{array}{c}
% u_i^x \\
% u_i^y \\
% u_j^x \\
% u_j^y
% \end{array}\right\rbrack \\
% \;\\
% \left\lbrack \begin{array}{c}
% f_i^x \\
% f_i^y \\
% f_j^x \\
% f_j^y
% \end{array}\right\rbrack =\underset{{\mathit{\mathbf{K}}}^e }{\underbrace{\frac{A\;E}{L}\left\lbrack \begin{array}{cccc}
% c^2  & c\;s & -c^2  & -c\;s\\
% c\;s & s^2  & -c\;s & -s^2 \\
% -c^2  & -c\;s & c^2  & c\;s\\
% c\;s & -s^2  & c\;s & s^2
% \end{array}\right\rbrack } } \left\lbrack \begin{array}{c}
% u_i^x \\
% u_i^y \\
% u_j^x \\
% u_j^y
% \end{array}\right\rbrack
% \end{array}
% The element strain is obtained by
% \varepsilon =\frac{\delta }{L}
% and the stress by
% \sigma =\frac{N}{A}
% note that the length of the element can be easily computed by
% L=|\mathit{\mathbf{r}}|=|{\mathit{\mathbf{p}}}_2 -{\mathit{\mathbf{p}}}_1 |
%% Complete Truss solver - Matlab implementation
% Let us analyse a structure as an example and look at the implementation details in Matlab.
%
% The structure is loaded by a force P with the topology given by

P = [0 0
     5 0
     3 3
     6 3]*100; % mm
edges = [1 2
         1 3
         2 3
         2 4
         3 4];

% Problem statement:
% Given the structure above, determine all displacements, member forces, deformations, strains and stresses as well as reaction forces in the supports.
% Solution
% The coordinates are given by the variables P and the edges define the node connectivity.
% \textrm{nodes}=\left\lbrack \begin{array}{cc}
% 1 & 2\\
% 1 & 3\\
% 2 & 3\\
% 2 & 4\\
% 3 & 4
% \end{array}\right\rbrack \overset{\textrm{Element}}{\begin{array}{c}
% 1\\
% 2\\
% 3\\
% 4\\
% 5
% \end{array}}
% The order  of the columns in nodes is not important.

figure
hold on
patch('Faces',edges,'Vertices',P,'LineWidth',2)
plot(P(:,1),P(:,2),'ok','MarkerFaceColor','c','MarkerSize',14)
nnod = size(P,1);
text(P(:,1)-0.05,P(:,2),cellstr(num2str([1:nnod]')))
axis equal
axis off
for i=1:5
    inod = edges(i,:);
    xm = mean(P(inod,1));
    ym = mean(P(inod,2));
    text(xm,ym,num2str(i),'backgroundColor','y')
end

% \textrm{num}\ldotp \textrm{degrees}\;\textrm{of}\;\textrm{freedom}=\textrm{number}\;\textrm{of}\;\textrm{nodes}\times \textrm{number}\;\textrm{of}\;\textrm{local}\;\textrm{DOFs}
% \textrm{DOFs}=4\cdot 2=8
% \mathit{\mathbf{u}}=\left\lbrack \begin{array}{c}
% u_1^x =0\\
% u_1^y =0\\
% u_2^x \\
% u_2^y =0\\
% u_3^x \\
% u_3^y \\
% u_4^x \\
% u_4^y
% \end{array}\right\rbrack ,\mathit{\mathbf{f}}=\left\lbrack \begin{array}{c}
% f_1^x =0\\
% f_1^y =0\\
% f_2^x =0\\
% f_2^y =0\\
% f_3^x =0\\
% f_3^y =0\\
% f_4^x =0\\
% f_4^y =-P
% \end{array}\right\rbrack
% {\mathit{\mathbf{S}}}_{8\times 8} {\;\mathit{\mathbf{u}}}_{8\times 1} ={\mathit{\mathbf{f}}}_{8\times 1}
% The unknown displacements are computed by establishing the global stiffness matrix (system matrix). Creating the linear system and solving it. This is done by the following steps:
% Inititalize the stiffness matrix, i.e., set it to zero.
% Loop over all elements, and for each element:
% Compute the element stiffness matrix
% Add the element stiffness matrix into the global stiffness matrix
% Add the element load vector to the global load vector
% Apply boundary conditions
% Solve the linear system
% We begin with some preliminaries and initialize the global system matrix and vectors using the number of degrees of freedom.

nele = size(edges,1)
nnod = size(P,1)
dofs = nnod*2 %number of degrees of freedom
S = zeros(dofs, dofs);
f = zeros(dofs,1);
u = f;

kprim = [1 0 -1 0
         0 0  0 0
         -1 0 1 0
         0  0 0 0];
Ltot = 0; % Total length of the structure.
E = 210000*ones(nele,1);    % Young's modulus
A = 4*6*ones(nele,1);    % Cross sectional area

% Element 1
% We begin by computing which global degrees of freedom correspond to our first element.

iel = 1;
inod = edges(iel,:)

% The node numbers correspond to degrees of freedom, for our problem we have two, i.e., x- and y-translation. The data structure of ieqs is the same as for the displacements and loads above.

ieqs = zeros(1,4); %[0 0 0 0]
ieqs(1:2:end) = inod*2-1
ieqs(2:2:end) = inod*2

% Computing the element stiffness matrix

p1 = P(inod(1), :) % first point of the rod 
p2 = P(inod(2), :) % second point of the rod 
r = p2-p1 % rod vector
L = norm(r) % length of the rod
Ltot = Ltot + L
er = r/L % direction, unit vector
c = er(1);
s = er(2);
R = [c -s
     s  c];
% Transformation matrix
LT = [R, zeros(2);
     zeros(2), R]
% Element stiffness matrix
iA = A(iel);  % element area
iE = E(iel);  % element Young's modulus
k = (iA*iE/L) % element stiffness
Ke = k * LT*kprim*LT'

% Assembly process
% The element stiffness matrix {\mathit{\mathbf{K}}}^e needs to be added to the global stiffness matrix \mathit{\mathbf{S}}. We need to ensure that the correct components from the element matrix are mapped to the corresponding components of the global stiffness matrix. This is done using the previously computed list of degrees of freedom, stored in the variable ieqs. We append (add) the stiffness contributions using

S(ieqs,ieqs) = S(ieqs,ieqs) + Ke % Add to the global stiffness matrix

% This is the simplest form of assembly in code. It is easy to read and understand. It is however not very efficient for very large problems with hundreds of thousands of elements. So there exists other methods of dealing with this. But that is a topic for a course in high performance computing.
% The rest of the elements are dealt with in an automated manner by executing the above lines of code and changing out the element number, iel. This is done using the for loop:

for iel = 2:nele
    iel
    inod = edges(iel,:);
    ieqs = zeros(1,4); %[0 0 0 0]
    ieqs(1:2:end) = inod*2-1;
    ieqs(2:2:end) = inod*2;
    p1 = P(inod(1), :);
    p2 = P(inod(2), :);
    r = p2-p1;
    L = norm(r) % length of the bar
    Ltot = Ltot + L
    er = r/L % direction, unit vector
    c = er(1);
    s = er(2);
    R = [c -s
         s  c];
    % Transformation matrix
    LT = [R, zeros(2);
         zeros(2), R];
    % Element stiffness matrix
    iA = A(iel);  % element area
    iE = E(iel);  % element Young's modulus
    k = (iA*iE/L) % element stiffness
    Ke = k * LT*kprim*LT'
    S(ieqs,ieqs) = S(ieqs,ieqs) + Ke
end

% The resulting system \mathit{\mathbf{S}}\;\mathit{\mathbf{u}}=\mathit{\mathbf{f}} is however singular, it contains rigid body motions. To alieviate these rigid body motions, we need to clamp down the mechanical system by introducing boundary conditions which pose enough constraints on the displacements such that rigid body motions are prohibited. This is done by eliminating equations, i.e., rows and columns corresponding to the degrees of freedom where the displacements are known. Solving the reduced system gives a unique solution.
% Note that special treatment of the right-hand side is required in cases where the displacements are non-zero. See this section for more info.
% We assign the known boundary conditions and loads.

presc = [1 2 4];     % prescribed degrees of freedom
u(presc) = 0;
f(2*4) = -10000;     

free = setdiff(1:dofs, presc); % Free DOFs
u(free) = S(free,free)\f(free)
U = [u(1:2:end), u(2:2:end)]
UR = sum(U.^2,2).^0.5 % Resultant displacement

% Visualize displacements

figure
hold on
patch('Faces',edges,'Vertices',P,'LineWidth',2)
patch('Faces',edges,'Vertices',P+U*80,'LineWidth',1,'LineStyle',':')
axis equal
axis off

% Computing rod quantities
% Once the displacements have been computed we can compute the normal forces, resultant forces, deformations, strains and stresses.


React = zeros(dofs,1); % Reaction forces
delta = zeros(nele,1); % Deformation vector
N = delta;             % Force vector
epsilon = delta;       % Strain vector
sigma = delta;         % Stress vector

for iel = 1:nele
    inod = edges(iel,:);
    ieqs = zeros(1,4); %[0 0 0 0]
    ieqs(1:2:end) = inod*2-1;
    ieqs(2:2:end) = inod*2;
    iu = u(ieqs); % element displacements

    p1 = P(inod(1), :);
    p2 = P(inod(2), :);
    r = p2-p1;
    L = norm(r); % length of the bar
    er = r/L; % direction, unit vector
    c = er(1);
    s = er(2);
    T = [-c -s c s];
    
    iA = A(iel);   % element area
    iE = E(iel);   % element Young's modulus
    k = (iA*iE/L); % element stiffness

    idelta = T*iu;   % element deformation
    iN = k * idelta; % element force
    fe = T'*iN;      % nodal force
    iepsilon = idelta/L; % element strain
    isigma = iN/iA;      % element stress

    React(ieqs) = React(ieqs) + fe; % Add up all reaction forces
    N(iel) = iN;
    delta(iel) = idelta;
    epsilon(iel) = iepsilon;
    sigma(iel) = isigma;
end
React = [React(1:2:end), React(2:2:end)] % Reaction forces split into x and y components

% Once the quantities of interest are computed, we will visualize them on the deformed structure. Since the displacements are typically very small, we will scale up the deformation in order to see a trend.
% The quantities will be shown using a color field for a fast overview, but we will also add text boxes to give numerical values for more accurate reading.

scale = 80;
X = P(:,1); Y = P(:,2); Ux = U(:,1); Uy = U(:,2);
Xm = mean(X(edges')+Ux(edges')*scale,1);
Ym = mean(Y(edges')+Uy(edges')*scale,1);

figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[sigma,sigma]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
text(Xm,Ym,cellstr(num2str(sigma)),'BackgroundColor','w')
title('Member stresses [MPa]')
figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[N,N]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
text(Xm,Ym,cellstr(num2str(N)),'BackgroundColor','w')
title('Member forces [N]')
figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[delta,delta]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
text(Xm,Ym,cellstr(num2str(delta)),'BackgroundColor','w')
title('Member deformations [mm]')
figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[epsilon,epsilon]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
text(Xm,Ym,cellstr(num2str(epsilon)),'BackgroundColor','w')
title('Member strains [mm/mm]')

%%
%
%% Example - Prescribed displacements
% Here, in addition for nodal forces, we also have nodes with prescribed displacements which are non-zero. This is the same as deforming a spring to a certain length, it will correspond to a force, but we controll the displacement instead.
%
% \delta =1mm. P=10kN.
%

clear
P = [0 0
     6 0
     4 2
     0 2]*100; % mm
nnod = size(P,1);
edges = [1 2
         3 4
         1 3
         3 2];

% Initiate variables

nele = size(edges,1);
nnod = size(P,1);
dofs = nnod*2; %number of degrees of freedom
S = zeros(dofs, dofs);
f = zeros(dofs,1);
u = f;
kprim = [1 0 -1 0
         0 0  0 0
         -1 0 1 0
         0  0 0 0];
Ltot = 0; % Total length of the structure.

% Visualize the structure

figure; hold on
patch('Faces',edges,'Vertices',P,'LineWidth',2, 'HandleVisibility','off')
dx = max(P(:,1))-min(P(:,1));
dy = max(P(:,2))-min(P(:,2));
plot(P(:,1),P(:,2),'ok','MarkerFaceColor','k','MarkerSize',14, 'HandleVisibility','off')
text(P(:,1)-0.01*dx,P(:,2),cellstr(num2str([1:nnod]')),'Color','w', 'HandleVisibility','off');
axis equal
axis off
for i=1:nele
    inod = edges(i,:);
    xm = mean(P(inod,1));
    ym = mean(P(inod,2));
    text(xm,ym,num2str(i),'backgroundColor','y')
end

% User defined parameters

E = 210000*ones(nele,1);    % Young's modulus
A = 4*6*ones(nele,1);    % Cross sectional area

presc = [1 2 3 7 8];     % prescribed degrees of freedom
u(presc) = [0, 0, 2, 0, 0]; %mm
f(3*2) = -10000; 

%

free = setdiff(1:dofs, presc); % Free DOFs

% Visualize the Load case
% To visually verify load case and boundary conditions

px = presc(mod(presc,2)==1);
xinds = (px+1)/2;
py = presc(mod(presc,2)==0);
yinds = py/2;
Fx = f(1:2:end); Fy = f(2:2:end);
dd = sqrt(dx^2+dy^2);
quiver(P(:,1)+sign(Fx)*dd*0.03,P(:,2),Fx,zeros(nnod,1),0.08,'Color','r','LineWidth',2,'DisplayName','f_x')
quiver(P(:,1),P(:,2)+sign(Fy)*dd*0.03,zeros(nnod,1),Fy,0.08,'Color','b','LineWidth',2,'DisplayName','f_y')
plot(P(xinds,1),P(xinds,2),'sk','MarkerFaceColor','r','MarkerSize',25,'DisplayName','u_x');
plot(P(yinds,1),P(yinds,2),'dk','MarkerFaceColor','b','MarkerSize',14,'DisplayName','u_y');
text(P(:,1)-0.01*dx,P(:,2),cellstr(num2str([1:nnod]')),'Color','w','HandleVisibility','off');
legend show

% Computing the stiffness matrix

for iel = 1:nele
    inod = edges(iel,:);
    ieqs = zeros(1,4); %[0 0 0 0]
    ieqs(1:2:end) = inod*2-1;
    ieqs(2:2:end) = inod*2;
    p1 = P(inod(1), :);
    p2 = P(inod(2), :);
    r = p2-p1;
    L = norm(r); % length of the bar
    Ltot = Ltot + L;
    er = r/L; % direction, unit vector
    c = er(1);
    s = er(2);
    R = [c -s
         s  c];
    % Transformation matrix
    LT = [R, zeros(2);
         zeros(2), R];
    % Element stiffness matrix
    iA = A(iel);  % element area
    iE = E(iel);  % element Young's modulus
    k = (iA*iE/L); % element stiffness
    Ke = k * LT*kprim*LT';
    S(ieqs,ieqs) = S(ieqs,ieqs) + Ke;
end

% Now we deal with the prescribed displacements.

fr = S(:,presc)*u(presc) % Reaction forces due to prescribed displacements
f = f - fr
u(free) = S(free,free)\f(free);
U = [u(1:2:end), u(2:2:end)]
UR = sum(U.^2,2).^0.5 % Resultant displacement

% Visualize the displacements

scale = 5;
figure
hold on
patch('Faces',edges,'Vertices',P,'LineWidth',2)
patch('Faces',edges,'Vertices',P+U*scale,'LineWidth',1,'LineStyle',':')
axis equal
axis off

% Computing rod quantities


React = zeros(dofs,1);
delta = zeros(nele,1);
N = delta; epsilon = delta; sigma = delta;

for iel = 1:nele
    inod = edges(iel,:);
    ieqs = zeros(1,4); %[0 0 0 0]
    ieqs(1:2:end) = inod*2-1;
    ieqs(2:2:end) = inod*2;
    iu = u(ieqs);

    p1 = P(inod(1), :);
    p2 = P(inod(2), :);
    r = p2-p1;
    L = norm(r); % length of the bar
    er = r/L; % direction, unit vector
    c = er(1);
    s = er(2);
    R = [c -s
         s  c];
    % Transformation matrix
    LT = [R, zeros(2);
         zeros(2), R];
    T = [-c -s c s];
    
    iA = A(iel);   % element area
    iE = E(iel);   % element Young's modulus
    k = (iA*iE/L); % element stiffness

    idelta = T*iu;
    iN = k * idelta;
    fe = T'*iN;
    iepsilon = idelta/L;
    isigma = iN/iA;

    React(ieqs) = React(ieqs) + fe;
    N(iel) = iN;
    delta(iel) = idelta;
    epsilon(iel) = iepsilon;
    sigma(iel) = isigma;
end

% The reaction forces in each node.

React = [React(1:2:end), React(2:2:end)]

%
% Visualize color fields

X = P(:,1); Y = P(:,2); Ux = U(:,1); Uy = U(:,2);
Xm = mean(X(edges')+Ux(edges')*scale,1);
Ym = mean(Y(edges')+Uy(edges')*scale,1);

figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[sigma,sigma]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
for iel=1:nele
    text(Xm(iel),Ym(iel),sprintf('$\\sigma_{%d}=%0.0f$',iel,sigma(iel)),'BackgroundColor','w','Interpreter','latex')
end
title('Member stresses [MPa]')

%

figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[N,N]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
for iel=1:nele
    text(Xm(iel),Ym(iel),sprintf('$N_{%d}=%0.0f$',iel,N(iel)),'BackgroundColor','w','Interpreter','latex')
end
title('Member forces [N]')

%

figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[delta,delta]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
for iel=1:nele
    text(Xm(iel),Ym(iel),sprintf('$\\delta_{%d}=%0.3f$',iel,delta(iel)),'BackgroundColor','w','Interpreter','latex')
end
title('Member deformations [mm]')

%

figure; hold on
patch('Faces',edges,'Vertices',P,'EdgeAlpha',0.1)
patch(X(edges')+Ux(edges')*scale,Y(edges')+Uy(edges')*scale,[epsilon,epsilon]','EdgeColor','flat', 'LineWidth',2)
axis equal
colormap jet; colorbar
for iel=1:nele
    text(Xm(iel),Ym(iel),sprintf('$\\varepsilon_{%d}=%0.5f$',iel,epsilon(iel)),'BackgroundColor','w','Interpreter','latex')
end
title('Member strains [mm/mm]')