clear all;
clc
% Finite element analysis program for 3D beam elements;
% G. K. Ananthasuresh, IISc-Bangalore
% June-August, 2005
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% READ INPUT from files 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

load node.dat
load elem.dat
load dispbc.dat
load forces.dat

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PRE-PROCESSING 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Identify the number of nodes, X, Y and Z coordinates of the nodes

NNODE = size(node,1);
nx = node(:,2);
ny = node(:,3);
nz = node(:,4);

% Identify the number of elements and form an element connectivity array,
% Young's modulus array, width, depth, and n1 unit vector arrays.

NELEM = size(elem,1);
ncon = elem(:,[2 3]);	% connectivity
Be = elem(:,4); % width
He = elem(:,5); % depth
Y = elem(:,6);		% Young's Modulus
n1 = elem(:,[7 8 9]);	% n1 directional vector
% Make n1 a unit vector
for i = 1:NELEM,
n1(i,:) = n1(i,:) / sqrt( n1(i,1)^2 + n1(i,2)^2 + n1(i,3)^2 ); % n1 directional unit vector
end
rho = .16;	% density = .16lb/in^3;

% Force vector
F = zeros(6*NNODE,1);
for i = 1:size(forces,1),
  F((forces(i,2)-1)*6+forces(i,3)) = forces(i,4);
end

% Displacement boundary conditions
Nfix = size(dispbc,1);
j = 0;
for i = 1:Nfix,
    j = j + 1; 
    dispID(j) = (dispbc(i,2)-1)*6+dispbc(i,3);
     dispVal(j) = dispbc(i,4);
end
[dispID sortIndex] = sort(dispID);
dispVal = dispVal(sortIndex);

% Compute the lengths of the elements

for ie=1:NELEM,
  ey = ncon(ie,1);
  jay = ncon(ie,2);
  Le(ie) = sqrt ( (nx(jay) - nx(ey))^2 + (ny(jay) - ny(ey))^2 + ...
  (nz(jay) - nz(ey))^2 );
end

% Compute t and n2 directions (t, n1 and n2 form an orthogonal basis)
% since t and n1 are planar. 

t = zeros(NELEM,3);
for i = 1:size(ncon,1),
    t(i,1) = ( nx(ncon(i,2))-nx(ncon(i,1)) );
    t(i,2) = ( ny(ncon(i,2))-ny(ncon(i,1)) );
    t(i,3) = ( nz(ncon(i,2))-nz(ncon(i,1)) );
    t(i,:) = t(i,:)./sqrt(sum(t(i,:).^2));
end
n2 = cross(t,n1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SOLUTION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Call fem3D.m to solve for the following.
%       Global Stiffness and Mass matrices M, K.
[u,Rdisp,Ksing,SE] = fea3D(Be,He,Le,Y,ncon,NELEM,NNODE,F,dispID,dispVal,t,n1,n2);

clf
plot3(nx,ny,nz,'r.');
hold on
% Plot undeformed
for i = 1:NELEM,
   id1 = ncon(i,1);
   id2 = ncon(i,2);
   plot3([nx(id1) nx(id2)], [ny(id1) ny(id2)], [nz(id1) nz(id2)], '--r');
end
% Plot deformed with a cubic between every pair of nodes.
for i = 1:NELEM,
   id1 = ncon(i,1);
   id2 = ncon(i,2);
   uid1 = 6*(id1-1) + 1;
   uid2 = 6*(id2-1) + 1;
   plot3([nx(id1)+u(uid1) nx(id2)+u(uid2)], [ny(id1)+u(uid1+1) ny(id2)+u(uid2+1)], ...
      [nz(id1)+u(uid1+2) nz(id2)+u(uid2+2)], '-b');
   plot3([nx(id1)+u(uid1) nx(id2)+u(uid2)], [ny(id1)+u(uid1+1) ny(id2)+u(uid2+1)], ...
      [nz(id1)+u(uid1+2) nz(id2)+u(uid2+2)], 'r.');

end
xlabel('x');
ylabel('y');
zlabel('z');
tmp = 1.2*[min(nx) max(nx) min(ny) max(ny) min(nz) max(nz)] + 0.1*[-1 1 -1 1 -1 1];
axis(tmp);
grid
