%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This m-file forms the finite element model of a beam system and solves it.
% G.K. Ananthasuresh for MEAM 550 class in Fall 1998.
% 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 containing the applied external forces
%	dispID	-->     A vector containing the displacement boundary condition information
%	Inertia --> 	A vector containing the Inertia of all the elements
%
% Outputs:
%	u	-->	The displacements of all the dof
%	Rdisp	-->	The reaction forces at the dof which are specified to be fixed
%	King	-->     The global stiffness matrix before the application of the
%			boundary conditions.
%	SE	-->     The strain energy in the entire truss
%					
function [u, Rdisp] = fembeam(Ae, Le, Ee, nx, ny, ncon, NELEM, NNODE, F, dispID,Ine)

K = zeros(3*NNODE,3*NNODE);				% Initialize global stiffness matrix
k = zeros(6,6);						% Initialize local stiffness matrix

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);
  In= Ine(ie);
  lox = (nx(jay)-nx(eye))/L; mox = (ny(jay)-ny(eye))/L;
  loy = -mox; moy = lox;
  Lambda = [ lox mox  0   0   0   0   ; ...
	        -mox lox  0   0   0   0   ; ...
              0   0   1   0   0   0   ; ...
              0   0   0  lox  mox 0   ; ...
              0   0   0 -mox  lox 0   ; ... 
              0   0   0   0   0   1  ];
% Form local element matrix
  k(1,1)=E*A/L;			k(1,4)=-k(1,1);	
  k(2,2)=12*E*In/(L^3);		k(2,3)=6*E*In/(L^2);
  k(2,5)=-k(2,2);		k(2,6)=k(2,3);
  k(3,2)=k(2,3);		k(3,3)=4*E*In/L;
  k(3,5)=-k(2,3);		k(3,6)=2*E*In/L;
  k(4,1)=-k(1,1);		k(4,4)=k(1,1);
  k(5,2)=-k(2,2);		k(5,3)=-k(2,3);
  k(5,5)=k(2,2);		k(5,6)=-k(2,3);
  k(6,2)=k(2,3);		k(6,3)=k(3,6);
  k(6,5)=-k(2,3);		k(6,6)=k(3,3);

  klocal = Lambda' * k * Lambda;


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

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

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

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

  K(id6,id1) = K(id6,id1) + klocal(6,1);
  K(id6,id2) = K(id6,id2) + klocal(6,2);
  K(id6,id3) = K(id6,id3) + klocal(6,3);
  K(id6,id4) = K(id6,id4) + klocal(6,4);
  K(id6,id5) = K(id6,id5) + klocal(6,5);
  K(id6,id6) = K(id6,id6) + klocal(6,6);
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 = sn;
for nd=1:Ndbc,
 K = matcut(K,dispID(nd)-nd+1);
 F = veccut(F,dispID(nd)-nd+1);
end

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

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

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

iuc = 0;
for iu=1:3*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


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

