function [A,N] = fem3D
% function [A,N] = fem3D
% Finite Element Method for Tetrahedron. The vargout are the Stifness and
% Mass matrices for spacial variables.
% Universidad de Zaragoza - 2015
global E nu coords tet

dof = 3; % Degrees of Freedom per node.
numNodos = size(coords,1); % # of nodes.
numTet = size(tet,1); % # of 3D elements.
A = zeros(dof*numNodos); N = zeros(dof*numNodos); % Allocate memory.
%
% MATERIAL MATRIX
%
D = zeros(6); cte = E*(1-nu)/(1+nu)/(1-2*nu);
D(1) = cte; D(8) = D(1); D(15) = D(1);
D(2) = cte*nu/(1-nu); D(3) = D(2); D(7) = D(2); D(9) = D(2); D(13) = D(2);
D(14) = D(2); D(22) = cte*(1-2*nu)/2/(1-nu); D(29) = D(22); D(36) = D(22);
%
% INTEGRATION POINTS
%
sg = zeros(3,4); wg = 1./24.*ones(4,1); nph = numel(wg);
a = (5.0 - sqrt(5))/20.0; b= (5.0 + 3.0*sqrt(5))/20.0;
sg(:,1) = [a; a; a]; sg(:,2) = [a; a; b]; 
sg(:,3) = [a; b; a];  sg(:,4) = [b; a; a]; 
%
% FINITE ELEMENT LOOP
%
for i1=1:numTet
    elnodes = tet(i1,:);
    xcoord = coords(elnodes,:);
    K = zeros(dof*4); KK = zeros(dof*4);
    %
    % JACOBIAN
    %
    v1 = xcoord(1,:)-xcoord(2,:); v2 = xcoord(2,:)-xcoord(3,:); 
    v3 = xcoord(3,:)-xcoord(4,:); 
    jcob = abs(det([v1;v2;v3]));
    %
    % SHAPE FUNCTION CONSTANTS
    %
    a1 = det([xcoord(2,:); xcoord(3,:); xcoord(4,:)]);
    a2 = -det([xcoord(1,:); xcoord(3,:); xcoord(4,:)]);
    a3 = det([xcoord(1,:); xcoord(2,:); xcoord(4,:)]);
    a4 = -det([xcoord(1,:); xcoord(2,:); xcoord(3,:)]);
    b1 = -det([1 xcoord(2,2:end); 1 xcoord(3,2:end); 1 xcoord(4,2:end)]);
    b2 = det([1 xcoord(1,2:end); 1 xcoord(3,2:end); 1 xcoord(4,2:end)]);
    b3 = -det([1 xcoord(1,2:end); 1 xcoord(2,2:end); 1 xcoord(4,2:end)]);
    b4 = det([1 xcoord(1,2:end); 1 xcoord(2,2:end); 1 xcoord(3,2:end)]);
    c1 = det([1 xcoord(2,1) xcoord(2,end); 1 xcoord(3,1) xcoord(3,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c2 = -det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(3,1) xcoord(3,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c3 = det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(2,1) xcoord(2,end);...
        1 xcoord(4,1) xcoord(4,end)]);
    c4 = -det([1 xcoord(1,1) xcoord(1,end); 1 xcoord(2,1) xcoord(2,end);...
        1 xcoord(3,1) xcoord(3,end)]);
    d1 = -det([1 xcoord(2,1:end-1); 1 xcoord(3,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d2 = det([1 xcoord(1,1:end-1); 1 xcoord(3,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d3 = -det([1 xcoord(1,1:end-1); 1 xcoord(2,1:end-1);...
        1 xcoord(4,1:end-1)]);
    d4 = det([1 xcoord(1,1:end-1); 1 xcoord(2,1:end-1);...
        1 xcoord(3,1:end-1)]);
    %
    % INTEGRATION POINTS LOOP
    %
    for j1=1:nph
        chi = sg(3*(j1-1)+1);
        eta = sg(3*(j1-1)+2);
        tau = sg(3*j1);
        %
        % GEOMETRY APPROACH
        %
        SHPa(4) = tau; 
        SHPa(3) = eta; SHPa(2) = chi; SHPa(1) = 1.-chi-eta-tau;
        chiG = 0.0; etaG = 0.0; tauG = 0.0;
        for k1=1:4
            chiG = chiG + SHPa(k1)*xcoord(k1,1);
            etaG = etaG + SHPa(k1)*xcoord(k1,2);
            tauG = tauG + SHPa(k1)*xcoord(k1,3);
        end
        %
        % SHAPE FUNCTION COMPUTATION
        %
        SHP(1) = (a1 + b1*chiG + c1*etaG + d1*tauG)/jcob; 
        dSHPx(1) = b1/jcob; dSHPy(1) = c1/jcob; dSHPz(1) = d1/jcob;
        SHP(2) = (a2 + b2*chiG + c2*etaG + d2*tauG)/jcob; 
        dSHPx(2) = b2/jcob; dSHPy(2) = c2/jcob; dSHPz(2) = d2/jcob;
        SHP(3) = (a3 + b3*chiG + c3*etaG + d3*tauG)/jcob; 
        dSHPx(3) = b3/jcob; dSHPy(3) = c3/jcob; dSHPz(3) = d3/jcob;
        SHP(4) = (a4 + b4*chiG + c4*etaG + d4*tauG)/jcob; 
        dSHPx(4) = b4/jcob; dSHPy(4) = c4/jcob; dSHPz(4) = d4/jcob;
        %
        % B MATRIX COMPUTATION
        %
        B = [dSHPx(1) 0 0 dSHPx(2) 0 0 dSHPx(3) 0 0 dSHPx(4) 0 0; ...
            0 dSHPy(1) 0 0 dSHPy(2) 0 0 dSHPy(3) 0 0 dSHPy(4) 0;...
            0 0 dSHPz(1) 0 0 dSHPz(2) 0 0 dSHPz(3) 0 0 dSHPz(4);...
            dSHPy(1) dSHPx(1) 0 dSHPy(2) dSHPx(2) 0 dSHPy(3) dSHPx(3)...
            0 dSHPy(4) dSHPx(4) 0;...
            dSHPz(1) 0 dSHPx(1) dSHPz(2) 0 dSHPx(2) dSHPz(3) 0 dSHPx(3)...
            dSHPz(4) 0 dSHPx(4);...
            0 dSHPz(1) dSHPy(1) 0 dSHPz(2) dSHPy(2) 0 dSHPz(3) dSHPy(3)...
            0 dSHPz(4) dSHPy(4)];
        %
        % MASS MATRIX
        %
        M = [SHP(1) 0 0; 0 SHP(1) 0; 0 0 SHP(1); SHP(2) 0 0; 0 SHP(2) 0;...
            0 0 SHP(2); SHP(3) 0 0; 0 SHP(3) 0; 0 0 SHP(3); SHP(4) 0 0;...
            0 SHP(4) 0; 0 0 SHP(4)]';
        K = K + B'*D*B*jcob*wg(j1);  % Element Stifness matrix.
        KK = KK + M'*M*jcob*wg(j1); % Element Mass matrix.
    end
    % System degrees of freedom associated with each element.
    index = [3*elnodes-2;3*elnodes-1;3*elnodes]; 
    index = reshape(index,1,4*dof);
    % Assembling of the system stiffness matrix.
    A(index,index) = A(index,index) + K;
    N(index,index) = N(index,index) + KK;
end
return
