TAPsOpenWound_TycoEndostitch_V2.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsOpenWound_TycoEndostitch_V2.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (06/15/2010)
UPDATE          (03/02/2011)
******************************************************************************/
#include "TAPsOpenWound_TycoEndostitch_V2.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
//=============================================================================
// Constructors
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
OpenWound_TycoEndostitch_V2<T,DATA>::OpenWound_TycoEndostitch_V2 ( std::string const & modelFile )
    : ModelDefBasedOnFEMHex<T,DATA>( modelFile )
{}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
OpenWound_TycoEndostitch_V2<T,DATA>::~OpenWound_TycoEndostitch_V2 ()
{
    ClearAllPuncturedLocations();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
std::string OpenWound_TycoEndostitch_V2<T,DATA>::StrInfo () const
{
    std::ostringstream ss;
    ss << "OpenWound_TycoEndostitch_V2<" << typeid(T).name() << "," << typeid(DATA).name() << ">";
    return ss.str();
}
//-----------------------------------------------------------------------------
//=============================================================================




//=============================================================================
// Simulation
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::ClearAllPunctureInfo ()
{
    ClearAllPuncturedPoints();      // on the surface mesh
    ClearAllPuncturedLocations();   // inside the FEM mesh
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::AddPuncturedPoint ( HEVertex<T> * pV )
{
    m_SetOfPuncturedPoints.push_back( pV );
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DeleteTheLastPuncturedPoint ()
{
    m_SetOfPuncturedPoints.erase( m_SetOfPuncturedPoints.end() - 1 );
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
HEVertex<T> * OpenWound_TycoEndostitch_V2<T,DATA>::GetPuncturedPointOnSurface ( unsigned int i )
{
    assert( i < m_SetOfPuncturedPoints.size() );
    return m_SetOfPuncturedPoints[i];
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::AddInterpolatedPuncturedLocations (
    int sutureID,           
    int suturePa,           
    int suturePb,           
    int modelID,            
    HEVertex<T> * pModelVa, 
    HEVertex<T> * pModelVb, 
    int ListOfSuturePts[],  
    HEVertex<T> * ListOfInterpolatedPuncturedLocations[],   
    int & numOfPts,         
    T * pMinIntervalDistance    
)
{
    if ( numOfPts <= 0 )    return;

    // BOTH pModelVa and pModelVb MUST BE FROM THIS MODEL'S SURFACE MESH VERTICES!
    // Since the both vertices pointed by pModelVa and pModelVb are the model's surface mesh vertices, 
    // the vertices' positions are in the local coordinates.
    // Therefore, no transforamtion is needed.

    // THE LOCATION IS ALREADY IN THE LOCAL FEM MESH COORDINATES
    Vector3<T> locA = pModelVa->RefToPosition();
    Vector3<T> locB = pModelVb->RefToPosition();
    //TransformationSupport<T> & trx = RefToTransformationSupport();
    //Matrix4x4<T> invTrxMat = trx.GetMatrixTransform().GetInverse();
    //locA = invTrxMat * locA;
    //locB = invTrxMat * locB;

    // Swap if suturePa is less than suturePb
    int * sutureIntPts = NULL;
    if ( suturePa > suturePb ) {
        int tmp = suturePa;
        suturePa = suturePb;
        suturePb = tmp;
        Vector3<T> V = locA;
        locA = locB;
        locB = V;
    }

    // Determine the number of interpolated locations available and initialize variables
    int posNumOfPts = suturePb - suturePa - 1;
    if ( posNumOfPts <= 0 ) return; // no suture points left for interpolations
    if ( posNumOfPts < numOfPts )   numOfPts = posNumOfPts;

    // Adjust the numOfPts if the pMinIntervalDistance is greater than the current interval distance
    T totalDistance = (locB - locA).Length();
    T intervalDist = totalDistance / ( numOfPts+1 );
    if ( pMinIntervalDistance != NULL && intervalDist < *pMinIntervalDistance ) {
        int finalNumOfPts = static_cast<int>( totalDistance / *pMinIntervalDistance ) - 1;
        if ( finalNumOfPts <= 0 ) {
            numOfPts = 0;
            return;
        }
        else if ( finalNumOfPts < numOfPts ) {
            numOfPts = finalNumOfPts;
        }
    }

    //int incPt = posNumOfPts / numOfPts;
    int incPt = 1;
    int sutureP = suturePa+incPt;
    Vector3<T> incLoc = ( locB - locA ) / ( numOfPts+1 );
    Vector3<T> Loc = locA + incLoc;


    //std::cout << "suturePb & Pa: " << suturePb << " & " << suturePa << "\n";
    //std::cout << "incPt: " << incPt << "\n";


    // Find the element id (i.e. grid locations) that locA is in.
    // Do the same for locB.
    unsigned int gridLocs[2][3];
    HexBarycentricCoordsForVertexRef<T,DATA> * pBCa = dynamic_cast< HexBarycentricCoordsForVertexRef<T,DATA> * >( pModelVa->GetBarycentricPtr() );
    HexBarycentricCoordsForVertexRef<T,DATA> * pBCb = dynamic_cast< HexBarycentricCoordsForVertexRef<T,DATA> * >( pModelVb->GetBarycentricPtr() );
    pBCa->GetGridLocations( gridLocs[0][0], gridLocs[0][1], gridLocs[0][2] );
    pBCb->GetGridLocations( gridLocs[1][0], gridLocs[1][1], gridLocs[1][2] );
    T paraCoords[2][3];
    pModelVa->GetBarycentricPtr()->GetGenBaryCoords( paraCoords[0][0], paraCoords[0][1], paraCoords[0][2] );
    pModelVb->GetBarycentricPtr()->GetGenBaryCoords( paraCoords[1][0], paraCoords[1][1], paraCoords[1][2] );

    //std::cout << "GridA: " << Vector3<unsigned int>( gridLocs[0] ) << "\n";
    //std::cout << "GridB: " << Vector3<unsigned int>( gridLocs[1] ) << "\n";
    //std::cout << "ParaCoordA: " << Vector3<T>( paraCoords[0] ) << "\n";
    //std::cout << "ParaCoordB: " << Vector3<T>( paraCoords[1] ) << "\n";
    //std::cout << "ElementAid: " << GetElementIDFromGridLocation( gridLocs[0][0], gridLocs[0][1], gridLocs[0][2] ) << "\n";
    //std::cout << "ElementBid: " << GetElementIDFromGridLocation( gridLocs[1][0], gridLocs[1][1], gridLocs[1][2] ) << "\n";

    unsigned int gridSets[3][2]; // grid [x,y,z][low-to-high]
    for ( unsigned int i = 0; i < 3; ++i ) {
        if ( gridLocs[0][i] < gridLocs[1][i] ) {
            gridSets[i][0] = gridLocs[0][i];
            gridSets[i][1] = gridLocs[1][i];
        }
        else {
            gridSets[i][0] = gridLocs[1][i];
            gridSets[i][1] = gridLocs[0][i];
        }
    }
    //std::cout << "gridSets x: " << gridSets[0][0] << " to " << gridSets[0][1] << "\n";
    //std::cout << "gridSets y: " << gridSets[1][0] << " to " << gridSets[1][1] << "\n";
    //std::cout << "gridSets z: " << gridSets[2][0] << " to " << gridSets[2][1] << "\n";

    // Interpolate and record the puncture locations
    int elementID;
    T intepolatedParaCoords[3];
    int countFoundLocations = 0;
    for ( int i = 0; i < numOfPts; ++i ) {
        bool bFound = false;
        for ( unsigned int x = gridSets[0][0]; !bFound && x <= gridSets[0][1]; ++x ) {
            for ( unsigned int y = gridSets[1][0]; !bFound && y <= gridSets[1][1]; ++y ) {
                for ( unsigned int z = gridSets[2][0]; !bFound && z <= gridSets[2][1]; ++z ) {
                    //std::cout << x << " " << y << " " << z << "\n";
                    elementID = GetElementIDFromGridLocation( x, y, z );
                    //std::cout << "elementID: " << elementID << "\n";
                    if ( elementID >= 0 ) {
                        bFound = RefToFEMMesh().RefToElement( elementID ).EstimateParametricCoords( 
                            Loc[0], Loc[1], Loc[2], 
                            intepolatedParaCoords[0], intepolatedParaCoords[1], intepolatedParaCoords[2] ) 
                            > 0 ? true : false;
                        //std::cout << "intetpolatedParaCoords: " << Vector3<T>( intepolatedParaCoords ) << "\n";

                        if ( bFound ) {
                            //std::cout << "FOUND intetpolatedParaCoords: " << Vector3<T>( intepolatedParaCoords ) << "\n";

                            ++countFoundLocations;

                            AddExtraVertexInLocalSpace( Loc, elementID, &RefToFEMMesh().RefToElement( elementID ), Vector3<unsigned int>(x,y,z), intepolatedParaCoords );
                            ListOfInterpolatedPuncturedLocations[i] = m_SetOfExtraVertices.back();
                            //*/

                            //std::cout << "sutureP: " << sutureP << "\n";

                            ListOfSuturePts[i] = sutureP;
                        }
                    }
                }
            }
        }
        sutureP += incPt;
        Loc += incLoc; 
    }
    numOfPts = countFoundLocations; // record the number of interpolated points
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
HEVertex<T> * OpenWound_TycoEndostitch_V2<T,DATA>::GetPuncturedLocationInsideModel ( unsigned int i )
{
    assert( i < m_SetOfExtraVertices.size() );
    return m_SetOfExtraVertices[i];
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::AddPointClue ( unsigned int vertexNumber )
{
    if ( vertexNumber < static_cast<unsigned int>( m_pHEModel->GetNumVertices() ) ) {
        HEVertex<T> * pV = m_pHEModel->GetVertexList()->Head();
        for ( unsigned int i = 0; i < vertexNumber; ++i ) {
            pV = pV->Next();
        }
        m_SetOfPointClues.push_back( pV );
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::ClearAllPuncturedPoints ()
{
    m_SetOfPuncturedPoints.clear();
    m_SetOfPairPuncturedPoints.clear();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::ClearAllPuncturedLocations ()
{
    ClearAllExtraVertices();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::ClearAllPointClues ()
{
    m_SetOfPointClues.clear();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::PairTwoPuncturedPoints ( unsigned int pt1, unsigned int pt2 )
{
    assert( pt1 != pt2 && pt1 < m_SetOfPuncturedPoints.size() && pt2 < m_SetOfPuncturedPoints.size() );
    m_SetOfPairPuncturedPoints.push_back( PairPuncturedPoints( pt1, pt2 ) );
}
//-----------------------------------------------------------------------------
// Simulation
//=============================================================================




//=============================================================================
// OpenGL
#if defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::Draw ( OpenGL::Enum::DrawMode DM = OpenGL::Enum::POLYGON )
{
    ModelDefBasedOnFEMHex<T,DATA>::Draw( DM );

    // DEBUG
    // Draw all puncture points
    //DrawAllPuncturedPoints();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawAllPuncturedPoints ()
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
        //glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT | GL_POINT_BIT );
        glEnable( GL_BLEND );
        glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        TransformationSupport<T> & trx = RefToTransformationSupport();
        for ( unsigned int i = 0; i < m_SetOfPuncturedPoints.size(); ++i ) {
            Vector3<T> pos = m_SetOfPuncturedPoints[i]->GetPosition();
            pos = trx.GetMatrixTransform() * pos;
            glPushMatrix();
                glTranslatef( pos[0], pos[1], pos[2] );
                //m_HelpDrawingUsefulObj.SetColor( 0.5, 0.1, 0.1, 0.15 );
                //m_HelpDrawingUsefulObj.DrawSphere(0.025f);
                if      ( i % 4 == 0 )  m_HelpDrawingUsefulObj.SetColor( 1, 0, 0, 0.5 );
                else if ( i % 4 == 1 )  m_HelpDrawingUsefulObj.SetColor( 0, 1, 0, 0.5 );
                else if ( i % 4 == 2 )  m_HelpDrawingUsefulObj.SetColor( 0, 0, 1, 0.5 );
                else if ( i % 4 == 3 )  m_HelpDrawingUsefulObj.SetColor( 1, 1, 0, 0.5 );
                m_HelpDrawingUsefulObj.DrawSphere(0.025f);
            glPopMatrix();
        }
    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawAllPuncturedLocations ()
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
        //glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT | GL_POINT_BIT );
        glEnable( GL_BLEND );
        glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        TransformationSupport<T> & trx = RefToTransformationSupport();
        for ( unsigned int i = 0; i < m_SetOfExtraVertices.size(); ++i ) {
            Vector3<T> pos = m_SetOfExtraVertices[i]->GetPosition();
            pos = trx.GetMatrixTransform() * pos;
            glPushMatrix();
                glTranslatef( pos[0], pos[1], pos[2] );
                m_HelpDrawingUsefulObj.SetColor( 0.5, 0.5, 0.5, 0.25 );
                m_HelpDrawingUsefulObj.DrawSphere(0.05f);
            glPopMatrix();
        }
    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawAllPuncturedPointPairs ()
{
    TransformationSupport<T> & trx = RefToTransformationSupport();
    std::vector< PairPuncturedPoints >::iterator it = m_SetOfPairPuncturedPoints.begin();
    glPushMatrix();
    glPushAttrib( GL_ALL_ATTRIB_BITS );
        glLineWidth( 5 );
        glBegin( GL_LINES );
        while ( it != m_SetOfPairPuncturedPoints.end() ) {
            Vector3<T> P1 = m_SetOfPuncturedPoints[it->P1]->GetPosition();
            Vector3<T> P2 = m_SetOfPuncturedPoints[it->P2]->GetPosition();
            P1 = trx.GetMatrixTransform() * P1;
            P2 = trx.GetMatrixTransform() * P2;
            glColor3f( 0, 0, 1 );
            glVertex3fv( P1.GetDataFloat() );
            glColor3f( 1, 0, 0 );
            glVertex3fv( P2.GetDataFloat() );
            ++it;
        }
        glEnd();
    glPopAttrib();
    glPopMatrix();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawAllPointClues ()
{
    for ( unsigned int i = 0; i < m_SetOfPointClues.size(); ++i ) {
        DrawPointClue( i );
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawPointClue ( unsigned int i )
{
    assert( i < m_SetOfPointClues.size() );
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    //glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT | GL_POINT_BIT );
    glPushMatrix();
        glEnable( GL_BLEND );
        glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        TransformationSupport<T> & trx = RefToTransformationSupport();
        Vector3<T> pos = m_SetOfPointClues[i]->GetPosition();
        pos = trx.GetMatrixTransform() * pos;
        glTranslatef( pos[0], pos[1], pos[2] );
        m_HelpDrawingUsefulObj.SetColor( 0.5, 1.0, 0.5, 0.25 );
        m_HelpDrawingUsefulObj.DrawSphere(0.02f);
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void OpenWound_TycoEndostitch_V2<T,DATA>::DrawASphereOnVertex ( HEVertex<T> * pV )
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    //glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT | GL_POINT_BIT );
    glPushMatrix();
        glEnable( GL_BLEND );
        glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
        TransformationSupport<T> & trx = RefToTransformationSupport();
        Vector3<T> pos = pV->GetPosition();
        pos = trx.GetMatrixTransform() * pos;
        glTranslatef( pos[0], pos[1], pos[2] );
        m_HelpDrawingUsefulObj.SetColor( 0.0, 1.0, 0.0, 0.3 );
        m_HelpDrawingUsefulObj.DrawSphere(0.04f, 0.04f, 0.04f);
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
#endif
// OpenGL
//=============================================================================




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