TAPsFEMMeshTetrahedraGen.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsFEMMeshTetrahedraGen.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (12/15/2009)
UPDATE          (04/16/2010)
******************************************************************************/
#include "TAPsFEMMeshTetrahedraGen.hpp"
// Using Inclusion Model (i.e. definitions are included in declarations)
//                       (this name.cpp is included in name.hpp)
// Each friend is defined directly inside its declaration.

BEGIN_NAMESPACE_TAPs__FEM
//=============================================================================
//template <typename T> std::vector< Tetrahedron<T> > * MeshTetrahedraGen<T>::m_pListOfElements = NULL;
//template <typename T> std::vector< Vector3<T> > * MeshTetrahedraGen<T>::m_pListOfDeformedMeshNodes = NULL;
//template <typename T> std::vector< Vector3<T> > * MeshTetrahedraGen<T>::m_pListOfUndeformedMeshNodes = NULL;
//-----------------------------------------------------------------------------

//template <typename T>
//void MeshTetrahedraGen<T>::SetAccessors ( MeshTetrahedra<T> & rTetFemMesh )
//{
//  m_pListOfElements               = &( rTetFemMesh.RetListOfElements() );
//  m_pListOfDeformedMeshNodes      = &( rTetFemMesh.RetListOfDeformedMeshNodes() );
//  m_pListOfUndeformedMeshNodes    = &( rTetFemMesh.RetListOfUndeformedMeshNodes() );
//}
//template <typename T>
//void MeshTetrahedraGen<T>::ClearAccessors ()
//{
//  m_pListOfElements               = NULL;
//  m_pListOfDeformedMeshNodes      = NULL;
//  m_pListOfUndeformedMeshNodes    = NULL;
//}

//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::OneElementMesh ( MeshTetrahedra<T> & rTetFemMesh, T mass, T YoungsModulus, T PoissonsRatio )
{
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    /*
    // 4 deformed nodes
    Nd.push_back( Vector3<T>(0,0,1.0) );
    Nd.push_back( Vector3<T>(0,1.0,0) );
    Nd.push_back( Vector3<T>(1.0,0,0) );
    Nd.push_back( Vector3<T>(0,0,0.0) );
    // 4 undeformed nodes
    Nu.push_back( Vector3<T>(0,0,1) );
    Nu.push_back( Vector3<T>(0,1,0) );
    Nu.push_back( Vector3<T>(1,0,0) );
    Nu.push_back( Vector3<T>(0,0,0) );
    //*/

    //*
    // TEST DATA
    // 4 deformed nodes
    Nd.push_back( Vector3<T>(2,3,4) );
    Nd.push_back( Vector3<T>(6,3,2) );
    Nd.push_back( Vector3<T>(2,5,1) );
    Nd.push_back( Vector3<T>(4,3,6) );
    // 4 undeformed nodes
    Nu.push_back( Vector3<T>(2,3,4) );
    Nu.push_back( Vector3<T>(6,3,2) );
    Nu.push_back( Vector3<T>(2,5,1) );
    Nu.push_back( Vector3<T>(4,3,6) );
    //*/

    // 1st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 1, 2, 3 ) );

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
}

//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::TwoElementMeshJoinByNode ( MeshTetrahedra<T> & rTetFemMesh, T mass, T YoungsModulus, T PoissonsRatio )
{
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    // 7 deformed nodes
    Nd.push_back( Vector3<T>(0,0,1) );
    Nd.push_back( Vector3<T>(0,1,0) );
    Nd.push_back( Vector3<T>(1,0,0) );
    Nd.push_back( Vector3<T>(0,0,0) );
    Nd.push_back( Vector3<T>(-1,0,0) );
    Nd.push_back( Vector3<T>(0,-1,0) );
    Nd.push_back( Vector3<T>(0,0,-1) );
    // 7 undeformed nodes
    Nu.push_back( Vector3<T>(0,0,1) );
    Nu.push_back( Vector3<T>(0,1,0) );
    Nu.push_back( Vector3<T>(1,0,0) );
    Nu.push_back( Vector3<T>(0,0,0) );
    Nu.push_back( Vector3<T>(-1,0,0) );
    Nu.push_back( Vector3<T>(0,-1,0) );
    Nu.push_back( Vector3<T>(0,0,-1) );

    // 1st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 1, 2, 3 ) );
    // 2st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 4, 5, 6, 3 ) );

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
}

//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::TwoElementMeshJoinByEdge ( MeshTetrahedra<T> & rTetFemMesh, T mass, T YoungsModulus, T PoissonsRatio )
{
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    // 6 deformed nodes
    Nd.push_back( Vector3<T>(0,0,1) );
    Nd.push_back( Vector3<T>(0,1,0) );
    Nd.push_back( Vector3<T>(1,0,0) );
    Nd.push_back( Vector3<T>(0,0,0) );
    Nd.push_back( Vector3<T>(-1,0,0) );
    Nd.push_back( Vector3<T>(0,-1,0) );
    // 6 undeformed nodes
    Nu.push_back( Vector3<T>(0,0,1) );
    Nu.push_back( Vector3<T>(0,1,0) );
    Nu.push_back( Vector3<T>(1,0,0) );
    Nu.push_back( Vector3<T>(0,0,0) );
    Nu.push_back( Vector3<T>(-1,0,0) );
    Nu.push_back( Vector3<T>(0,-1,0) );

    // 1st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 1, 2, 3 ) );
    // 2st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 3, 0, 4, 5 ) );

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
}

//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::TwoElementMeshJoinByFace ( MeshTetrahedra<T> & rTetFemMesh, T mass, T YoungsModulus, T PoissonsRatio )
{
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    // 5 deformed nodes
    Nd.push_back( Vector3<T>(-0.1,0.2,1) );
    Nd.push_back( Vector3<T>(0.15,1,-0.25) );
    Nd.push_back( Vector3<T>(1,-0.1,0.2) );
    Nd.push_back( Vector3<T>(-1.1,-1.1,-1.1) );
    Nd.push_back( Vector3<T>(-1,-0.1,0.2) );
    // 5 undeformed nodes
    Nu.push_back( Vector3<T>(-0.1,0.2,1) );
    Nu.push_back( Vector3<T>(0.15,1,-0.25) );
    Nu.push_back( Vector3<T>(1,-0.1,0.2) );
    Nu.push_back( Vector3<T>(-1.1,-1.1,-1.1) );
    Nu.push_back( Vector3<T>(-1,-0.1,0.2) );

    // 1st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 1, 2, 3 ) );
    // 2st element
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 1, 3, 4 ) );

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
}

//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::SixTetrahedraSubFromHexahedron ( MeshTetrahedra<T> & rTetFemMesh, T mass, T YoungsModulus, T PoissonsRatio )
{
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    T s = T(0.5);
    // 8 undeformed nodes
    Nu.push_back( Vector3<T>(-s,-s,-s) );
    Nu.push_back( Vector3<T>(+s,-s,-s) );
    Nu.push_back( Vector3<T>(+s,+s,-s) );
    Nu.push_back( Vector3<T>(-s,+s,-s) );
    Nu.push_back( Vector3<T>(-s,-s,+s) );
    Nu.push_back( Vector3<T>(+s,-s,+s) );
    Nu.push_back( Vector3<T>(+s,+s,+s) );
    Nu.push_back( Vector3<T>(-s,+s,+s) );

    //s += T(0.05);
    // 8 deformed nodes
    Nd.push_back( Vector3<T>(-s,-s,-s) );
    Nd.push_back( Vector3<T>(+s,-s,-s) );
    Nd.push_back( Vector3<T>(+s,+s,-s) );
    Nd.push_back( Vector3<T>(-s,+s,-s) );
    Nd.push_back( Vector3<T>(-s,-s,+s) );
    Nd.push_back( Vector3<T>(+s,-s,+s) );
    Nd.push_back( Vector3<T>(+s,+s,+s) );
    Nd.push_back( Vector3<T>(-s,+s,+s) );

    // 6 tetrahedra
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 2, 3, 6 ) );
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 3, 7, 6 ) );
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 7, 4, 6 ) );
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 0, 5, 6, 4 ) );
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 1, 5, 6, 0 ) );
    E.push_back( Tetrahedron<T>( &Nd, &Nu, YoungsModulus, PoissonsRatio, 1, 6, 2, 0 ) );

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
}


//-----------------------------------------------------------------------------
template <typename T, typename DATA>
bool MeshTetrahedraGen<T,DATA>::MeshFromGridGenerator (
        MeshTetrahedra<T> & rTetFemMesh,    
        GridGenerator<T,DATA> & rGridGen,   
        T   mass,                           
        T   YoungsModulus,                  
        T   PoissonsRatio                   
)
{

    if ( !rGridGen.IsGridGenerated() ) {
        return false;
    }

    // Grid layout (right-handed coordinates)
    //      ^ y
    //      |
    //      |
    //      0-----> x       x = column
    //     /                y = row
    //    /                 z = depth
    //   v z
    //
    //  Where X := #columns-1, Y := #rows-1, and Z := #slices-1.
    //
    //                   O-------O-------O---------------O-------O
    //                  /|      /|      /|      ...     /|      /|
    //                 / |(0,Y,0)|     / |      ...    / |(X,Y,0)|
    //                /  |    /  |    /  |      ...   /  |    /  |
    //               /   O---/---O---/---O-----------/---O---/---O
    //              /    |  /    |  /    |      ... /    |  /   /|
    //             /     | /     | /     |      .../     | /   / |
    //            /      |/      |/      |      ../      |/   /  |
    //           /       /-------/-------O-------/-------/---/---O
    //          /       /|      /               HIGHEST / POINT /|
    //         O-------O-------O---------------O-------@   /   / |
    //         |       |       |      ...      | .     |  /   /  |
    //         |(0,Y,Z)|       |      ...      |(X,Y,Z)| /   /   |
    //         |       |       |      ...      | .     |/   /    |
    //         O-------O-------O---------------O-------O   /     |
    //         |       |       |      ...      | .     |  /      |
    //         |       |       |      ...      | .     | /       |
    //         |       |       |      ...      | .     |/        |
    //         O-------O-------O---------------O-------O         |
    //         |         |             .         .     |         |
    //         |         O-------O-----.-O-------------|-O-------0
    //         |        /|       |     . |      ...    | |      /|
    //         |       / |(0,0,0)|     . |      ...    | |(X,0,0)|
    //         |      /  |       |     . |      ...    | |    /  |
    //         |     /   @-------O-----.-O-------------|-O---/---O
    //         | LOWEST / POINT /      ./              |/   /   /
    //         |   /   /       /       .               |   /   /
    //         |  /   /       /       /.              /|  /   /
    //         | /   /       /       / .             / | /   /
    //         |/   /       /       /  .            /  |/   /
    //         O-------O-------O---------------O-------O   /
    //         |  /    |  /    |  /   ...      |  /    |  /
    //         |(0,0,Z)| /     | /    ...      |(X,0,Z)| /
    //         |/      |/      |/     ...      |/      |/
    //         O-------O-------O---------------O-------O
    //
    //  where D := Z;  R := Y;  C := X;
    //
    //  The n-th Z-slice
    //
    //  (nRC+(R-1)+C+0)--(nRC+(R-1)+C+1)--(nRC+(R-1)+C+2)-- ... --(nRC+(R+1)C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //         ...              ...              ...        ...          ...
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //     (nRC+2C+0)-------(nRC+2C+1)-------(nRC+2C+2)---- ... -----(nRC+2C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //      (nRC+C+0)--------(nRC+C+1)--------(nRC+C+2)---- ... ------(nRC+C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //       (nRC+0)----------(nRC+1)----------(nRC+2)----- ... -------(nRC+C-1)
    //
    //
    //
    //                 @---------@      ^ y
    //                /|        /|      |
    //               / | SLICE /A|      |
    //              @---------@  |      |
    //             /|  @- - -/| -@      +------>x
    //            / | SLICE /B| /      /
    //           @---------@  |/      /
    //           |  @------|--@      v z
    //           | /SLICE C| /
    //           |/        |/
    //           @---------@

    //*
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    // Z goes first follows by Y then X.
    // So the slice is in Z-plane with Y as rows and X as columns
    T **** const GridVertices = rGridGen.ReturnPtrToVertexPositionData();
    unsigned int z,y,x;
    unsigned int C = rGridGen.GetGridSizeX();
    unsigned int R = rGridGen.GetGridSizeY();
    unsigned int D = rGridGen.GetGridSizeZ();

    // Add FEM mesh vertices from the grid data
    for ( z = 0; z < D; ++z ) {
        for ( y = 0; y < R; ++y ) {
            for ( x = 0; x < C; ++x ) {
                Vector3<T> V( GridVertices[x][y][z][0], GridVertices[x][y][z][1], GridVertices[x][y][z][2] );
                Nd.push_back( V );
                Nu.push_back( V );
            }
        }
    }

    // Add tetrahedra from the FEM mesh vertices
    unsigned int v[8];
    unsigned int RC  = R*C;
    unsigned int SLICE = 0;
    unsigned int ROW;
    for ( z = 0; z < D-1; ++z ) {
        ROW = 0;
        for ( y = 0; y < R-1; ++y ) {
            for ( x = 0; x < C-1; ++x ) {
                // Pt 1 to 8
                //
                //         3---------2      ^ y
                //        /|        /|      |
                //       / |       / |      |
                //      7---------6  |      |
                //      |  0- - - | -1      +------>x
                //      | /       | /      /
                //      |/        |/      /
                //      4---------5      v z
                v[0] = SLICE + ROW + x;
                v[3] = v[0] + C;
                v[4] = v[0] + RC;
                v[7] = v[4] + C;
                v[1] = v[0]+1;
                v[2] = v[3]+1;
                v[5] = v[4]+1;
                v[6] = v[7]+1;
                // Subdivided into six tetrahedra
                // 0-2-3-6
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[0], v[2], v[3], v[6] )
                );
                // 0-3-7-6
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[0], v[3], v[7], v[6] )
                );
                // 0-7-4-6
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[0], v[7], v[4], v[6] )
                );
                // 0-5-6-4
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[0], v[5], v[6], v[4] )
                );
                // 1-5-6-0
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[1], v[5], v[6], v[0] )
                );
                // 1-6-2-0
                E.push_back( Tetrahedron<T>( 
                    &Nd,            // the 4 deformed nodes
                    &Nu,            // the 4 undeformed nodes
                    YoungsModulus,  // Young's modulus
                    PoissonsRatio,  // Poisson's ratio
                    v[1], v[6], v[2], v[0] )
                );
            }
            ROW += C;
        }
        SLICE += RC;
    }

    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );
    //*/

    // DEBUG
    std::cout << "Generated Tetrahedral FEM mesh has " << rTetFemMesh.GetNumOfNodes() 
        << " nodes and " << rTetFemMesh.GetNumOfElements() << " elements.\n";

    return true;
}


//-----------------------------------------------------------------------------
template <typename T, typename DATA>
bool MeshTetrahedraGen<T,DATA>::MeshFromGridGenerator (
        MeshTetrahedra<T> & rTetFemMesh,    
        std::vector< TetBarycentricCoordsForVertexRef<T> > & rBaryCoords, 
        //std::vector< unsigned int > & r_ElementID,    //!< identify the element that the barycentric coordinates are embedded in
        GridGenerator<T,DATA> & rGridGen,   
        T   mass,                           
        T   YoungsModulus,                  
        T   PoissonsRatio,                  
        bool bGenElementsInsideVolume       
)
{

    if ( !rGridGen.IsGridGenerated() ) {
        return false;
    }

    // Grid layout (right-handed coordinates)
    //      ^ y
    //      |
    //      |
    //      0-----> x       x = column
    //     /                y = row
    //    /                 z = depth
    //   v z
    //
    //  Where X := #columns-1, Y := #rows-1, and Z := #slices-1.
    //
    //                   O-------O-------O---------------O-------O
    //                  /|      /|      /|      ...     /|      /|
    //                 / |(0,Y,0)|     / |      ...    / |(X,Y,0)|
    //                /  |    /  |    /  |      ...   /  |    /  |
    //               /   O---/---O---/---O-----------/---O---/---O
    //              /    |  /    |  /    |      ... /    |  /   /|
    //             /     | /     | /     |      .../     | /   / |
    //            /      |/      |/      |      ../      |/   /  |
    //           /       /-------/-------O-------/-------/---/---O
    //          /       /|      /               HIGHEST / POINT /|
    //         O-------O-------O---------------O-------@   /   / |
    //         |       |       |      ...      | .     |  /   /  |
    //         |(0,Y,Z)|       |      ...      |(X,Y,Z)| /   /   |
    //         |       |       |      ...      | .     |/   /    |
    //         O-------O-------O---------------O-------O   /     |
    //         |       |       |      ...      | .     |  /      |
    //         |       |       |      ...      | .     | /       |
    //         |       |       |      ...      | .     |/        |
    //         O-------O-------O---------------O-------O         |
    //         |         |             .         .     |         |
    //         |         O-------O-----.-O-------------|-O-------0
    //         |        /|       |     . |      ...    | |      /|
    //         |       / |(0,0,0)|     . |      ...    | |(X,0,0)|
    //         |      /  |       |     . |      ...    | |    /  |
    //         |     /   @-------O-----.-O-------------|-O---/---O
    //         | LOWEST / POINT /      ./              |/   /   /
    //         |   /   /       /       .               |   /   /
    //         |  /   /       /       /.              /|  /   /
    //         | /   /       /       / .             / | /   /
    //         |/   /       /       /  .            /  |/   /
    //         O-------O-------O---------------O-------O   /
    //         |  /    |  /    |  /   ...      |  /    |  /
    //         |(0,0,Z)| /     | /    ...      |(X,0,Z)| /
    //         |/      |/      |/     ...      |/      |/
    //         O-------O-------O---------------O-------O
    //
    //  where D := Z;  R := Y;  C := X;
    //
    //  The n-th Z-slice
    //
    //  (nRC+(R-1)+C+0)--(nRC+(R-1)+C+1)--(nRC+(R-1)+C+2)-- ... --(nRC+(R+1)C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //         ...              ...              ...        ...          ...
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //     (nRC+2C+0)-------(nRC+2C+1)-------(nRC+2C+2)---- ... -----(nRC+2C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //      (nRC+C+0)--------(nRC+C+1)--------(nRC+C+2)---- ... ------(nRC+C+C-1)
    //          |                |                |                       |
    //          |                |                |                       |
    //          |                |                |                       |
    //       (nRC+0)----------(nRC+1)----------(nRC+2)----- ... -------(nRC+C-1)
    //
    //
    //
    //                 @---------@      ^ y
    //                /|        /|      |
    //               / | SLICE /A|      |
    //              @---------@  |      |
    //             /|  @- - -/| -@      +------>x
    //            / | SLICE /B| /      /
    //           @---------@  |/      /
    //           |  @------|--@      v z
    //           | /SLICE C| /
    //           |/        |/
    //           @---------@

    //*
    std::vector< Tetrahedron<T> > & E = rTetFemMesh.RetListOfElements();
    std::vector< Vector3<T> > & Nd = rTetFemMesh.RetListOfDeformedMeshNodes();
    std::vector< Vector3<T> > & Nu = rTetFemMesh.RetListOfUndeformedMeshNodes();

    // Clear the mesh
    E.clear();
    Nd.clear();
    Nu.clear();

    // Create temp data
    unsigned int gridsize[3] = { rGridGen.GetGridSizeX()-1, rGridGen.GetGridSizeY()-1, rGridGen.GetGridSizeZ()-1 };
    unsigned int gx = gridsize[0];
    unsigned int gxgy = gridsize[0]*gridsize[1];
    bool **** pValidElements = new bool***[gridsize[2]];
    for ( unsigned int k = 0; k < gridsize[2]; ++k ) {
        pValidElements[k] = new bool**[gridsize[1]];
        for ( unsigned int j = 0; j < gridsize[1]; ++j ) {
            pValidElements[k][j] = new bool*[gridsize[0]];
            for ( unsigned int i = 0; i < gridsize[0]; ++i ) {
                pValidElements[k][j][i] = new bool[6];
                for ( unsigned int e = 0; e < 6; ++e ) {
                    pValidElements[k][j][i][e] = false;
                }
            }
        }
    }

    // Find all barycentric coordinates and mark all valid elements
    //FindAllBaryCoordsAndTheirLocInTheGrids( rBaryCoords, r_ElementID, rGridGen, pValidElements );
    FindAllBaryCoordsAndTheirLocInTheGrids( rBaryCoords, rGridGen, pValidElements );
    // Now rBaryCoords contain the barycentric coordinates of all vertices 
    // but not yet have the correct pointer to tetrehedra since they are not yet created.
    // pValidElements now contain all valid elements
    // r_ElementID store the element IDs associated with rBaryCoords in the format of 6(z(R-1)(C-1)+y(C-1)+x).
    // These values (format) has to be changed to the element IDs stored in the FEM mesh.

    /*
    // DID NOT SEE THE BENEFIT OF THIS ONE -- SEPARATING OF RIGHT_OUTSIDE_BOUNDARY AND ON_BOUNDARY FROM RIGHT_INSIDE_BOUNDARY AND INSIDE_MODEL!
    // FEM MESH DATA FROM THE ADJACENT ELEMENTS ON THE BOUNDARY OF THE SURFACE MESH
    if ( true ) {
        // Mark the valid vertices (location) in temp data based on the grid vertex data stored in rGridGen
        GridGenerator<T,DATA>::VertexFlag *** const vertexFlag = rGridGen.ReturnPtrToVertexFlagData();
        // Mark the valid vertices (location) in temp data
        bool b[8];
        for ( unsigned int x = 0; x < gridsize[0]; ++x ) {
            for ( unsigned int y = 0; y < gridsize[1]; ++y ) {
                for ( unsigned int z = 0; z < gridsize[2]; ++z ) {
                    // Find valid FEM mesh vertices
                    if (    vertexFlag[x][y][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[0] = true;} else {b[0] = false;}
                    if (    vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[1] = true;} else {b[1] = false;}
                    if (    vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[2] = true;} else {b[2] = false;}
                    if (    vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[3] = true;} else {b[3] = false;}
                    if (    vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[4] = true;} else {b[4] = false;}
                    if (    vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[5] = true;} else {b[5] = false;}
                    if (    vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[6] = true;} else {b[6] = false;}
                    if (    vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY )
                    {b[7] = true;} else {b[7] = false;}

                    // Six tetrahedra subdivided from a brick
                    // 0-2-3-6 --> #0
                    // 0-3-7-6 --> #1
                    // 0-7-4-6 --> #2
                    // 0-5-6-4 --> #3
                    // 1-5-6-0 --> #4
                    // 1-6-2-0 --> #5
                    if ( b[0] && b[2] && b[3] && b[6] ) pValidElements[z][y][x][0] = true;
                    if ( b[0] && b[3] && b[7] && b[6] ) pValidElements[z][y][x][1] = true;
                    if ( b[0] && b[7] && b[4] && b[6] ) pValidElements[z][y][x][2] = true;
                    if ( b[0] && b[5] && b[6] && b[4] ) pValidElements[z][y][x][3] = true;
                    if ( b[1] && b[5] && b[6] && b[0] ) pValidElements[z][y][x][4] = true;
                    if ( b[1] && b[6] && b[2] && b[0] ) pValidElements[z][y][x][5] = true;
                }
            }
        }
    }
    //*/

    // FEM MESH DATA FROM VOLUME INSIDE THE SURFACE MESH
    if ( bGenElementsInsideVolume ) {
        // Mark the valid vertices (location) in temp data based on the grid vertex data stored in rGridGen
        GridGenerator<T,DATA>::VertexFlag *** const vertexFlag = rGridGen.ReturnPtrToVertexFlagData();
        // Mark the valid vertices (location) in temp data
        bool b[8];
        for ( unsigned int x = 0; x < gridsize[0]; ++x ) {
            for ( unsigned int y = 0; y < gridsize[1]; ++y ) {
                for ( unsigned int z = 0; z < gridsize[2]; ++z ) {
                    // Find valid FEM mesh vertices
                    if (    vertexFlag[x][y][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x][y][z] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[0] = true;} else {b[0] = false;}
                    if (    vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[1] = true;} else {b[1] = false;}
                    if (    vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[2] = true;} else {b[2] = false;}
                    if (    vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[3] = true;} else {b[3] = false;}
                    if (    vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x][y][z+1] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[4] = true;} else {b[4] = false;}
                    if (    vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y][z+1] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[5] = true;} else {b[5] = false;}
                    if (    vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x+1][y+1][z+1] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[6] = true;} else {b[6] = false;}
                    if (    vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_OUTSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::ON_BOUNDARY
                        ||  vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::RIGHT_INSIDE_BOUNDARY
                        ||  vertexFlag[x][y+1][z+1] == GridGenerator<T,DATA>::INSIDE_MODEL )
                    {b[7] = true;} else {b[7] = false;}

                    // Six tetrahedra subdivided from a brick
                    // 0-2-3-6 --> #0
                    // 0-3-7-6 --> #1
                    // 0-7-4-6 --> #2
                    // 0-5-6-4 --> #3
                    // 1-5-6-0 --> #4
                    // 1-6-2-0 --> #5
                    if ( b[0] && b[2] && b[3] && b[6] ) pValidElements[z][y][x][0] = true;
                    if ( b[0] && b[3] && b[7] && b[6] ) pValidElements[z][y][x][1] = true;
                    if ( b[0] && b[7] && b[4] && b[6] ) pValidElements[z][y][x][2] = true;
                    if ( b[0] && b[5] && b[6] && b[4] ) pValidElements[z][y][x][3] = true;
                    if ( b[1] && b[5] && b[6] && b[0] ) pValidElements[z][y][x][4] = true;
                    if ( b[1] && b[6] && b[2] && b[0] ) pValidElements[z][y][x][5] = true;
                }
            }
        }
    }

    // Z goes first follows by Y then X.
    // So the slice is in Z-plane with Y as rows and X as columns
    T **** const GridVertices = rGridGen.ReturnPtrToVertexPositionData();
    unsigned int C = rGridGen.GetGridSizeX();
    unsigned int R = rGridGen.GetGridSizeY();
    unsigned int D = rGridGen.GetGridSizeZ();
    unsigned int RC  = R*C;
    unsigned int DRC = D*RC;
    unsigned int SLICE = 0;
    unsigned int ROW;

    // Temp data for finding valid FEM mesh vertices
    unsigned int *** pVertexLocs = new unsigned int**[D];
    for ( unsigned int k = 0; k < D; ++k ) {
        pVertexLocs[k] = new unsigned int*[R];
        for ( unsigned int j = 0; j < R; ++j ) {
            pVertexLocs[k][j] = new unsigned int[C];
            for ( unsigned int i = 0; i < C; ++i ) {
                pVertexLocs[k][j][i] = DRC;
            }
        }
    }

    // Temp data for storing element ID
    std::vector< unsigned int > elementIDs( gridsize[2]*gridsize[1]*gridsize[0]*6 );

    // Find valid FEM mesh vertices
    // Six tetrahedra subdivided from a brick
    // 0-2-3-6
    // 0-3-7-6
    // 0-7-4-6
    // 0-5-6-4
    // 1-5-6-0
    // 1-6-2-0
    //                 @---------@      ^ y
    //                /|        /|      |
    //               / | SLICE /A|      |
    //              3---------2  |      |
    //             /|  @- - -/| -@      +------>x
    //            / | SLICE /B| /      /
    //           7---------6  |/      /
    //           |  0------|--1      v z
    //           | /SLICE C| /
    //           |/        |/
    //           4---------5
    unsigned int countV = 0;
    for ( unsigned int k = 0; k < gridsize[2]; ++k ) {
        for ( unsigned int j = 0; j < gridsize[1]; ++j ) {
            for ( unsigned int i = 0; i < gridsize[0]; ++i ) {

                //std::cout << "k,j,i: " << k << "," << j << "," << i << "\n";
                //std::cout << pValidElements[k][j][i][0]
                //      << " " <<   pValidElements[k][j][i][1]
                //      << " " <<   pValidElements[k][j][i][2]
                //      << " " <<   pValidElements[k][j][i][3]
                //      << " " <<   pValidElements[k][j][i][4]
                //      << " " <<   pValidElements[k][j][i][5] << "\n";

                // v0
                if ( pVertexLocs[k][j][i] == DRC ) {
                    if (    pValidElements[k][j][i][0]
                        ||  pValidElements[k][j][i][1]
                        ||  pValidElements[k][j][i][2]
                        ||  pValidElements[k][j][i][3]
                        ||  pValidElements[k][j][i][4]
                        ||  pValidElements[k][j][i][5] )
                    {
                        Vector3<T> V( GridVertices[i][j][k][0], GridVertices[i][j][k][1], GridVertices[i][j][k][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k][j][i] = countV;
                        ++countV;
                    }
                }
                // v1
                if ( pVertexLocs[k][j][i+1] == DRC ) {
                    if (    pValidElements[k][j][i][4]
                        ||  pValidElements[k][j][i][5] )
                    {
                        Vector3<T> V( GridVertices[i+1][j][k][0], GridVertices[i+1][j][k][1], GridVertices[i+1][j][k][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k][j][i+1] = countV;
                        ++countV;
                    }
                }
                // v2
                if ( pVertexLocs[k][j+1][i+1] == DRC ) {
                    if (    pValidElements[k][j][i][0]
                        ||  pValidElements[k][j][i][5] )
                    {
                        Vector3<T> V( GridVertices[i+1][j+1][k][0], GridVertices[i+1][j+1][k][1], GridVertices[i+1][j+1][k][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k][j+1][i+1] = countV;
                        ++countV;
                    }
                }
                // v3
                if ( pVertexLocs[k][j+1][i] == DRC ) {
                    if (    pValidElements[k][j][i][0]
                        ||  pValidElements[k][j][i][1] )
                    {
                        Vector3<T> V( GridVertices[i][j+1][k][0], GridVertices[i][j+1][k][1], GridVertices[i][j+1][k][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k][j+1][i] = countV;
                        ++countV;
                    }
                }
                // v4
                if ( pVertexLocs[k+1][j][i] == DRC ) {
                    if (    pValidElements[k][j][i][2]
                        ||  pValidElements[k][j][i][3] )
                    {
                        Vector3<T> V( GridVertices[i][j][k+1][0], GridVertices[i][j][k+1][1], GridVertices[i][j][k+1][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k+1][j][i] = countV;
                        ++countV;
                    }
                }
                // v5
                if ( pVertexLocs[k+1][j][i+1] == DRC ) {
                    if (    pValidElements[k][j][i][3]
                        ||  pValidElements[k][j][i][4] )
                    {
                        Vector3<T> V( GridVertices[i+1][j][k+1][0], GridVertices[i+1][j][k+1][1], GridVertices[i+1][j][k+1][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k+1][j][i+1] = countV;
                        ++countV;
                    }
                }
                // v6
                if ( pVertexLocs[k+1][j+1][i+1] == DRC ) {
                    if (    pValidElements[k][j][i][0]
                        ||  pValidElements[k][j][i][1]
                        ||  pValidElements[k][j][i][2]
                        ||  pValidElements[k][j][i][3]
                        ||  pValidElements[k][j][i][4]
                        ||  pValidElements[k][j][i][5] )
                    {
                        Vector3<T> V( GridVertices[i+1][j+1][k+1][0], GridVertices[i+1][j+1][k+1][1], GridVertices[i+1][j+1][k+1][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k+1][j+1][i+1] = countV;
                        ++countV;
                    }
                }
                // v7
                if ( pVertexLocs[k+1][j+1][i] == DRC ) {
                    if (    pValidElements[k][j][i][1]
                        ||  pValidElements[k][j][i][2] )
                    {
                        Vector3<T> V( GridVertices[i][j+1][k+1][0], GridVertices[i][j+1][k+1][1], GridVertices[i][j+1][k+1][2] );
                        Nd.push_back( V );
                        Nu.push_back( V );
                        pVertexLocs[k+1][j+1][i] = countV;
                        ++countV;
                    }
                }
            }
        }
    }

    // Add tetrahedron elements from the valid FEM mesh vertices
    unsigned int countE = 0;
    for ( unsigned int k = 0; k < D-1; ++k ) {
        ROW = 0;
        for ( unsigned int j = 0; j < R-1; ++j ) {
            for ( unsigned int i = 0; i < C-1; ++i ) {
                unsigned int elementID = (k*gxgy + j*gx + i) * 6;
                // Pt 1 to 8
                //
                //         3---------2      ^ y
                //        /|        /|      |
                //       / |       / |      |
                //      7---------6  |      |
                //      |  0- - - | -1      +------>x
                //      | /       | /      /
                //      |/        |/      /
                //      4---------5      v z
                // Subdivided into six tetrahedra
                // 0-2-3-6
                if ( pValidElements[k][j][i][0] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i], pVertexLocs[k][j+1][i+1], pVertexLocs[k][j+1][i], pVertexLocs[k+1][j+1][i+1] )
                    );
                    elementIDs[elementID] = countE++;
                }
                // 0-3-7-6
                if ( pValidElements[k][j][i][1] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i], pVertexLocs[k][j+1][i], pVertexLocs[k+1][j+1][i], pVertexLocs[k+1][j+1][i+1] )
                    );
                    elementIDs[elementID+1] = countE++;
                }
                // 0-7-4-6
                if ( pValidElements[k][j][i][2] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i], pVertexLocs[k+1][j+1][i], pVertexLocs[k+1][j][i], pVertexLocs[k+1][j+1][i+1] )
                    );
                    elementIDs[elementID+2] = countE++;
                }
                // 0-5-6-4
                if ( pValidElements[k][j][i][3] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i], pVertexLocs[k+1][j][i+1], pVertexLocs[k+1][j+1][i+1], pVertexLocs[k+1][j][i] )
                    );
                    elementIDs[elementID+3] = countE++;
                }
                // 1-5-6-0
                if ( pValidElements[k][j][i][4] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i+1], pVertexLocs[k+1][j][i+1], pVertexLocs[k+1][j+1][i+1], pVertexLocs[k][j][i] )
                    );
                    elementIDs[elementID+4] = countE++;
                }
                // 1-6-2-0
                if ( pValidElements[k][j][i][5] ) {
                    E.push_back( Tetrahedron<T>( 
                        &Nd,            // the 4 deformed nodes
                        &Nu,            // the 4 undeformed nodes
                        YoungsModulus,  // Young's modulus
                        PoissonsRatio,  // Poisson's ratio
                        pVertexLocs[k][j][i+1], pVertexLocs[k+1][j+1][i+1], pVertexLocs[k][j+1][i+1], pVertexLocs[k][j][i] )
                    );
                    elementIDs[elementID+5] = countE++;
                }
            }
            ROW += C;
        }
        SLICE += RC;
    }

    // Associated barycentric coordinates with tetrahedra
    // MUST BE DONE AFTER ALL TETRAHEDRAL ELEMENTS ARE GENERATED!
    // OTHERWISE THE POINTERS WILL NOT BE VALID DUE TO
    // THE AUTOMATICAL RESIZING OF STD::VECTOR!
    for ( unsigned int i = 0; i < rBaryCoords.size(); ++i ) {
        rBaryCoords[i].SetElementID( elementIDs[ rBaryCoords[i].GetElementID() ] );
        //r_ElementID[i] = elementIDs[ r_ElementID[i] ];
        //rBaryCoords[i].SetPtrToTetrahedron( &(E[ r_ElementID[i] ]) );
        rBaryCoords[i].SetPtrToTetrahedron( &(E[ rBaryCoords[i].GetElementID() ]) );
    }

    // Delete temp data
    for ( unsigned int k = 0; k < gridsize[2]; ++k ) {
        for ( unsigned int j = 0; j < gridsize[1]; ++j ) {
            delete [] pVertexLocs[k][j];
        }
        delete [] pVertexLocs[k];
    }
    delete [] pVertexLocs;

    // Delete temp data
    for ( unsigned int k = 0; k < gridsize[2]; ++k ) {
        for ( unsigned int j = 0; j < gridsize[1]; ++j ) {
            for ( unsigned int i = 0; i < gridsize[0]; ++i ) {
                delete [] pValidElements[k][j][i];
            }
            delete [] pValidElements[k][j];
        }
        delete [] pValidElements[k];
    }
    delete [] pValidElements;

    // Assemble the stiffness matrix
    rTetFemMesh.SetSizeOfStiffnessMatrix();
    rTetFemMesh.AssembleStiffnessMatrix();

    // Allocate data for simulation
    rTetFemMesh.AllocateDataForSimulation( mass );

    // DEBUG
    std::cout << "Generated Tetrahedral FEM mesh has " << rTetFemMesh.GetNumOfNodes() 
        << " nodes and " << rTetFemMesh.GetNumOfElements() << " elements.\n";
    std::cout << std::flush;

    return true;
}


//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void MeshTetrahedraGen<T,DATA>::FindAllBaryCoordsAndTheirLocInTheGrids (
        std::vector< TetBarycentricCoordsForVertexRef<T> > & rBaryCoords, 
        //std::vector< unsigned int > & r_ElementID,    //!< identify the element that the barycentric coordinates are embedded in
        GridGenerator<T,DATA> & rGridGen,   
        bool **** pValidElements    
)
{
    // FOR HALFEDGE MESH MODEL ONLY
    TAPs::OpenGL::HalfEdgeModel<T> * pHEModel = dynamic_cast< TAPs::OpenGL::HalfEdgeModel<T> * >( rGridGen.GetModelUseToGenGrid() );
    if ( pHEModel == NULL ) {
        std::cout << "MeshTetrahedraGen<...> --> MeshModel is not HalfEdge!" << std::endl;
        exit(-1);
    }
    rBaryCoords.clear();
    //std::cout << "#vertices: " << pHEModel->GetNoVertices() << "\n";
    //std::cout << "#vertices: " << pHEModel->GetNumVertices() << "\n";
    //std::cout << "#vertices: " << pHEModel->GetVertexList()->Size() << "\n";
    HEVertex<T> * pVertex = pHEModel->GetVertexList()->Head();
    unsigned int i,j,k;
    //T u,v,w;
    //T sx = 1.0 / (m_atGridDimension[0]*m_atGridDimension[0]);
    //T sy = 1.0 / (m_atGridDimension[1]*m_atGridDimension[1]);
    //T sz = 1.0 / (m_atGridDimension[2]*m_atGridDimension[2]);
    unsigned int gx = rGridGen.GetGridSizeX()-1;    // width
    unsigned int gy = rGridGen.GetGridSizeY()-1;    // height
    unsigned int gz = rGridGen.GetGridSizeZ()-1;    // depth
    unsigned int gxgy = gx*gy;
    unsigned int numOfElements = gx*gy*gz*6;

    // checkedElements for marking that the computed inverted matrices of 6 tetrahedra forming a brick
    // has been computed and stored in storedMatrices
    // so that the computations do not have to be repeated.
    std::vector< unsigned int > checkedElements( numOfElements );
    unsigned int locateMatrices = 0;
    std::vector< Matrix3x3<T> > storedMatrices;
    for ( unsigned int i = 0; i < numOfElements; ++i ) {
        checkedElements[i] = numOfElements;
    }

    unsigned int count = 0;
    T **** VP = rGridGen.ReturnPtrToVertexPositionData();
    while ( pVertex != NULL ) {

        Vector3<T> P = pVertex->GetPosition();
        rGridGen.LocateHexGrid( P[0], P[1], P[2], i, j, k );

        Vector3<T> v0( VP[i  ][j  ][k  ][0], VP[i  ][j  ][k  ][1], VP[i  ][j  ][k  ][2] );
        Vector3<T> v1( VP[i+1][j  ][k  ][0], VP[i+1][j  ][k  ][1], VP[i+1][j  ][k  ][2] );
        Vector3<T> v2( VP[i+1][j+1][k  ][0], VP[i+1][j+1][k  ][1], VP[i+1][j+1][k  ][2] );
        Vector3<T> v3( VP[i  ][j+1][k  ][0], VP[i  ][j+1][k  ][1], VP[i  ][j+1][k  ][2] );
        Vector3<T> v4( VP[i  ][j  ][k+1][0], VP[i  ][j  ][k+1][1], VP[i  ][j  ][k+1][2] );
        Vector3<T> v5( VP[i+1][j  ][k+1][0], VP[i+1][j  ][k+1][1], VP[i+1][j  ][k+1][2] );
        Vector3<T> v6( VP[i+1][j+1][k+1][0], VP[i+1][j+1][k+1][1], VP[i+1][j+1][k+1][2] );
        Vector3<T> v7( VP[i  ][j+1][k+1][0], VP[i  ][j+1][k+1][1], VP[i  ][j+1][k+1][2] );

        // Test against six tetrahedra
        // 0-2-3-6
        // 0-3-7-6
        // 0-7-4-6
        // 0-5-6-4
        // 1-5-6-0
        // 1-6-2-0

        // Computed the inverted matrices of 6 tetrahedra
        unsigned int enumber = i + j*gx + k*gxgy;
        if ( checkedElements[enumber] == numOfElements ) {
            checkedElements[enumber] = locateMatrices++;
            // 0-2-3-6
            storedMatrices.push_back( Matrix3x3<T>( 
                v2[0]-v0[0], v3[0]-v0[0], v6[0]-v0[0], 
                v2[1]-v0[1], v3[1]-v0[1], v6[1]-v0[1], 
                v2[2]-v0[2], v3[2]-v0[2], v6[2]-v0[2]
            ).Inversed() );
            // 0-3-7-6
            storedMatrices.push_back( Matrix3x3<T>( 
                v3[0]-v0[0], v7[0]-v0[0], v6[0]-v0[0], 
                v3[1]-v0[1], v7[1]-v0[1], v6[1]-v0[1], 
                v3[2]-v0[2], v7[2]-v0[2], v6[2]-v0[2]
            ).Inversed() );
            // 0-7-4-6
            storedMatrices.push_back( Matrix3x3<T>( 
                v7[0]-v0[0], v4[0]-v0[0], v6[0]-v0[0], 
                v7[1]-v0[1], v4[1]-v0[1], v6[1]-v0[1], 
                v7[2]-v0[2], v4[2]-v0[2], v6[2]-v0[2]
            ).Inversed() );
            // 0-5-6-4
            storedMatrices.push_back( Matrix3x3<T>( 
                v5[0]-v0[0], v6[0]-v0[0], v4[0]-v0[0], 
                v5[1]-v0[1], v6[1]-v0[1], v4[1]-v0[1], 
                v5[2]-v0[2], v6[2]-v0[2], v4[2]-v0[2]
            ).Inversed() );
            // 1-5-6-0
            storedMatrices.push_back( Matrix3x3<T>( 
                v5[0]-v1[0], v6[0]-v1[0], v0[0]-v1[0], 
                v5[1]-v1[1], v6[1]-v1[1], v0[1]-v1[1], 
                v5[2]-v1[2], v6[2]-v1[2], v0[2]-v1[2]
            ).Inversed() );
            // 1-6-2-0
            storedMatrices.push_back( Matrix3x3<T>( 
                v6[0]-v1[0], v2[0]-v1[0], v0[0]-v1[0], 
                v6[1]-v1[1], v2[1]-v1[1], v0[1]-v1[1], 
                v6[2]-v1[2], v2[2]-v1[2], v0[2]-v1[2]
            ).Inversed() );
        }

        bool notFound = true;
        unsigned int elementID = enumber * 6;
        unsigned int matID = checkedElements[enumber] * 6;

        // 0-2-3-6
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[matID] * (P-v0);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][0] = true;
                //r_ElementID.push_back( elementID );
                rBaryCoords[count].SetElementID( elementID );
                ++count;
                //std::cout << "0 baryCoords: " << baryCoords << "\n";
            }
        }
        // 0-3-7-6
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[++matID] * (P-v0);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][1] = true;
                //r_ElementID.push_back( elementID+1 );
                rBaryCoords[count].SetElementID( elementID+1 );
                ++count;
                //std::cout << "1 baryCoords: " << baryCoords << "\n";
            }
        }
        // 0-7-4-6
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[++matID] * (P-v0);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][2] = true;
                //r_ElementID.push_back( elementID+2 );
                rBaryCoords[count].SetElementID( elementID+2 );
                ++count;
                //std::cout << "2 baryCoords: " << baryCoords << "\n";
            }
        }
        // 0-5-6-4
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[++matID] * (P-v0);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][3] = true;
                //r_ElementID.push_back( elementID+3 );
                rBaryCoords[count].SetElementID( elementID+3 );
                ++count;
                //std::cout << "3 baryCoords: " << baryCoords << "\n";
            }
        }
        // 1-5-6-0
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[++matID] * (P-v1);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][4] = true;
                //r_ElementID.push_back( elementID+4 );
                rBaryCoords[count].SetElementID( elementID+4 );
                ++count;
                //std::cout << "4 baryCoords: " << baryCoords << "\n";
            }
        }
        // 1-6-2-0
        if ( notFound ) {
            Vector3<T> baryCoords = storedMatrices[++matID] * (P-v1);
            if (
                0 <= baryCoords[0] && baryCoords[0] <= 1
            &&  0 <= baryCoords[1] && baryCoords[1] <= 1
            &&  0 <= baryCoords[2] && baryCoords[2] <= 1 )
            {
                notFound = false;
                rBaryCoords.push_back( 
                    TetBarycentricCoordsForVertexRef<T>( 
                        pVertex->RefToPosition(),
                        NULL,
                        baryCoords[0], baryCoords[1], baryCoords[2]
                    ) 
                );
                pValidElements[k][j][i][5] = true;
                //r_ElementID.push_back( elementID+5 );
                rBaryCoords[count].SetElementID( elementID+5 );
                ++count;
                //std::cout << "5 baryCoords: " << baryCoords << "\n";
            }
        }

        pVertex = pVertex->Next();
    }

    // Set the barycentric pointer for each vertex
    // MUST BE DONE AFTER ALL BARY COORDS ARE GENERATED!
    // OTHERWISE THE POINTERS WILL NOT BE VALID DUE TO
    // THE AUTOMATICAL RESIZING OF STD::VECTOR!
    count = 0;
    pVertex = pHEModel->GetVertexList()->Head();
    while ( pVertex != NULL ) {
        pVertex->SetBarycentricPtr( &rBaryCoords[count] );
        //std::cout << count << "\tTet\t" << pVertex << "\t" << pVertex->GetBarycentricPtr() << "\t" << *(pVertex->GetBarycentricPtr()) << "\n";
        pVertex = pVertex->Next();
        ++count;
    }

    // DEBUG
    //std::cout << "Surface mesh with Tetrahedral FEM mesh - #surface mesh vertices: " << pHEModel->GetNumVertices() << "\n";
    //std::cout << "Surface mesh with Tetrahedral FEM mesh - #barycoords (must equal to # surface mesh vertices): " << count << "\n";
    //std::cout << "#ElementIDs: " << r_ElementID.size() << "\n";
    //for ( unsigned int i = 0; i < r_ElementID.size(); ++i ) {
    //  std::cout << "ElementID["<<i<<"]: " << r_ElementID[i] << "\n";
    //}
    //std::cout << "#BaryCoords: " << rBaryCoords.size() << "\n";
    //for ( unsigned int i = 0; i < rBaryCoords.size(); ++i ) {
    //  std::cout << "rBaryCoords["<<i<<"]: " << rBaryCoords[i] << "\n";
    //}
}

//=============================================================================
END_NAMESPACE_TAPs__FEM
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----