% 2D FEM model

% Problem: Truss
printf("2D FEM model: Truss\n")
printf("\n")
printf("              ^ \n")
printf(" D          C '->  Nodes   : A, B, C, D\n")  
printf("  .___________.    Elements: a, b, c, d, e, f\n")
printf("  |\\   c    /|     \n")
printf("  |  \\    /e |    A and B are articulated\n")
printf("  |    \\/    |         Ua = Va = Ub = Vb = 0\n")
printf("  |d   /\\    |b   Fuc, Fvc, Fud and Fvd are well known\n")
printf("  |  /   f\\  |         (1000, 500, 0, 0)kgf \n")
printf("  |/________\\|    Bar: elasticity E = 21000 kgf/mm2\n")
printf("  ^    a     ^          section A = 300*(.6, .8, .6, .8, 1., 1.)mm2\n")
printf("  A          B          length  L =1000*(.6, .8, .6, .8, 1., 1.)mm\n")
printf("\n")
printf("                   Uc, Ud, Fa and Fb ?\n")

% Model definition (how the elements are conected)
%   Nodes A, B, C, D and E  are enumerated from (1) to (5)
%   Bars a,... and f from 1 to 6.
Connection=[1,2; 2,3; 3,4; 4,1; 1,3; 2,4];

% Model parameters Kelem = E*A/L
A= 300*[.6, .8, .6, .8, 1., 1.];
L=1000*[.6, .8, .6, .8, 1., 1.];
E=21000*ones(1,size(A)(2));
Kelem=E.*A ./ L;
noElems = size(Kelem)(2);

% Compute mi=sin(alpha) and lambda=cos(alpha) for each bar
alpha=pi()/180*[0, 90, 180, 270, 0, 0];   %alpha taken from then 
alpha(5:6)=atan([L(2)/L(1), -L(4)/L(1)]); %compute alpha for e and f
mi=sin(alpha);
lambda=cos(alpha);

%Boundary conditions
%   Each node has two degrees of freedom, thus the size of F and D  
%   are 2 times the number of nodes.
Fknown=[1000, 500, 0, 0];
posFknown=[5, 6, 7, 8];

noNodes = 2*(4);
Dknown=[0, 0, 0, 0];
posDknown=[1, 2, 3, 4]; % A and B

% Building the equation F = K * u
F=zeros(1,noNodes);
F(posFknown)=Fknown;

D=zeros(1,noNodes);
D(posDknown)=Dknown;

K=zeros(noNodes);
for i=1:noElems
  ke=zeros(4); %matriz de rigidez local
  ke([1,3],[1,3])=Kelem(i)*[1,-1;-1,1];
  T=zeros(4);  %matriz de transformacao
  T(1:2,1:2)=[lambda(i),mi(i);-mi(i),lambda(i)];
  T(3:4,3:4)=T(1:2,1:2);
  Ke=T'*ke*T;  %matriz de rigidez global
  pos=zeros(1,4); %position in the global matrix
  pos=[Connection(i,1)*2-1, Connection(i,1)*2, Connection(i,2)*2-1, Connection(i,2)*2];
  K(pos,pos)+=Ke;
endfor

% solve icognitas
D(posFknown)=K(posFknown,posFknown)\F(posFknown)';
F(posDknown)=K(posDknown,:)*D';

% solve internal forces on each element
% it will be done in the local coordinate system
% equation to solve: Fi=(A*E/L)*(u2-u1)
du=zeros(1,noElems);
for i=1:noElems
  T=zeros(4);  %matriz de transformacao
  T(1:2,1:2)=[lambda(i),mi(i);-mi(i),lambda(i)];
  T(3:4,3:4)=T(1:2,1:2);
  pos=zeros(1,4); %position in the global matrix
  pos=[Connection(i,1)*2-1, Connection(i,1)*2, Connection(i,2)*2-1, Connection(i,2)*2];
  d=zeros(4); %compute D in local coordinate
  d=T*D(pos)';
  du(i)=d(3)-d(1); %compute deformation by compression in mm
endfor
Fi=Kelem.*du;

% print results
printf("\nDisplacements (x,y)mm:\n")
printf("\tA(%f,%f)\n\tB(%f,%f)\n\tC(%f,%f)\n\tD(%f,%f)\n",D)
printf("Forces (x,y)kgf:\n")
printf("\tA(%f,%f)\n\tB(%f,%f)\n\tC(%f,%f)\n\tD(%f,%f)\n",F)
printf("Internal Forces (compression)kgf:\n")
printf("\ta(%f)\n\tb(%f)\n\tc(%f)\n\td(%f)\n\te(%f)\n\tf(%f)\n",Fi)