%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This m-file forms the finite element model of a truss system and solves it.
% G.K. Ananthasuresh for MEAM 310 class in Spring 1997.
% Inputs:
% Ae    --> A vector containing the cross-section areas of all the elements
% Le    --> A vector containing the lengths of all the elements
% Ee    --> A vector containing the Young's moduli of all the elements
% nx    --> A vector containing the X-coordinates of all the nodes
% ny    --> A vector containing the Y-coordinates of all the nodes
% ncon  --> A matrix containing the nodal-connectivity of all the elements
% F     --> A vector contating the applied external forces
% dispID--> A vector containing the displacement boundary conditions
% Outputs:
% u     --> The displacements of all the dof
% Rdisp --> The reaction forces at the dof which are specified to be fixed
% P     --> A matrix containing the X and Y internal reaction forces at 
%           the two ends of each element.
% King  --> The global stiffness matrix before the application of the
%           boundary conditions.
%               SE              --> The strain energy in the entire truss
%
function [u,Rdisp,P,Ksing,SE,SEgrad] = ...
         femtruss(Ae, Le, Ee, nx, ny, ncon, NELEM, NNODE, F, dispID)

K = zeros(2*NNODE,2*NNODE);     % Initialize global stiffness matrix
SEgrad = zeros(NELEM,1);

for ie=1:NELEM,
  eye = ncon(ie,1);
  jay = ncon(ie,2);

% Form the transformation matrix, Lambda.
  L = Le(ie);
  A = Ae(ie);
  E = Ee(ie);
  lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
  loy = -mox; moy = lox;
  Lambda = [ lox mox  0  0    ; ...
              0   0  lox mox ];


  k = [ 1 -1; -1 1 ];                     % Local element stiffness matrix

  k = k*(A*E/L);

  klocal = Lambda' * k * Lambda;


% Form ID matrix to assemble klocal into the global stiffness matrix, K.
  id1 = 2*(eye-1) + 1;
  id2 = id1 + 1;
  id3 = 2*(jay-1) + 1;
  id4 = id3 + 1;

  K(id1,id1) = K(id1,id1) + klocal(1,1);
  K(id1,id2) = K(id1,id2) + klocal(1,2);
  K(id2,id1) = K(id2,id1) + klocal(2,1);
  K(id2,id2) = K(id2,id2) + klocal(2,2);

  K(id1,id3) = K(id1,id3) + klocal(1,3);
  K(id1,id4) = K(id1,id4) + klocal(1,4);
  K(id2,id3) = K(id2,id3) + klocal(2,3);
  K(id2,id4) = K(id2,id4) + klocal(2,4);

  K(id3,id1) = K(id3,id1) + klocal(3,1);
  K(id3,id2) = K(id3,id2) + klocal(3,2);
  K(id4,id1) = K(id4,id1) + klocal(4,1);
  K(id4,id2) = K(id4,id2) + klocal(4,2);

  K(id3,id3) = K(id3,id3) + klocal(3,3);
  K(id3,id4) = K(id3,id4) + klocal(3,4);
  K(id4,id3) = K(id4,id3) + klocal(4,3);
  K(id4,id4) = K(id4,id4) + klocal(4,4);

end

% Store unlaltered K as Ksing before applying the boundary conditions.
  Ksing = K;

%det(K)
%inv(K);
%pause

% Imposing displacement boundary conditions
% ------------------------------------------
% dispID array contains the dof which are assigned zero values.
[sm,sn] = size(dispID);
Ndbc = sm;
for nd=1:Ndbc,
 K = matcut(K,dispID(nd)-nd+1);
 F = veccut(F,dispID(nd)-nd+1);
end

% To solve for unknown displacements.
F;
U = inv(K)*F;
SE = 0.5*U'*K*U;

% Results
% ---------------
% "u" for all nodes (including those where values were specified)
u = zeros(2*NNODE,1);

for iu=1:Ndbc,
  u(dispID(iu)) = 12345.12345;
end

iuc = 0;
for iu=1:2*NNODE,
    if u(iu) == 12345.12345
     iuc = iuc+1;
  else
     u(iu) = U(iu-iuc);
  end
end

for iu=1:Ndbc,
  u(dispID(iu)) = 0;
end

dx = zeros(1,NNODE);
dy = zeros(1,NNODE);

for iu=1:NNODE,
  dx(iu) = u(2*(iu-1)+1);
  dy(iu) = u(2*(iu-1)+2);
end

%----------------------------------------------
% Computation of reactions at constrained nodes
%----------------------------------------------
R = Ksing*u;
Rdisp = zeros(1,Ndbc);
for iu=1:Ndbc,
  Rdisp(iu) = R(dispID(iu));
end

%-------------------------------------------
% Computation of internal reaction forces
% and storing in P(NNODE,4)
%-------------------------------------------
M = zeros(NNODE,1);

for ie=1:NELEM,

  eye = ncon(ie,1);
  jay = ncon(ie,2);


% Form the transformation matrix, Lambda.
  L = Le(ie); A = Ae(ie);
  lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
  loy = -mox; moy = lox;
  Lambda = [ lox mox  0  0    ; ...
              0   0  lox mox ];


  k = [ 1 -1; -1 1 ];

  k = k*(A*E/L);

  klocal = Lambda' * k * Lambda;


% Form ID matrix to identify respective displacements.
  id1 = 2*(eye-1) + 1;
  id2 = id1 + 1;
  id3 = 2*(jay-1) + 1;
  id4 = id3 + 1;

  ulocal = [u(id1) u(id2) u(id3) u(id4)];

  Rint = klocal*ulocal';
  SEgrad(ie) = -0.5*ulocal*klocal*ulocal'/A;


  P(ie,1) = Rint(1);
  P(ie,2) = Rint(2);
  P(ie,3) = Rint(3);
  P(ie,4) = Rint(4);

  nxd1 = nx(eye) + u(id1);
  nyd1 = ny(eye) + u(id2);
  nxd2 = nx(jay) + u(id3);
  nyd2 = ny(jay) + u(id4);

  Ld = sqrt( (nxd2-nxd1)^2 + (nyd2-nyd1)^2 );

  if Ld > L P(ie,5) = 1; else P(ie,5) = -1; end
  if Ld==L P(ie,5) = 0; end

end

