TAPsModelLaparoscopicAdjustableGastricBand.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsModelLaparoscopicAdjustableGastricBand.cpp

A specific model for a laparoscopic adjustable gastric band.

SUKITTI PUNAK   (07/02/2008)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsModelLaparoscopicAdjustableGastricBand.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__OpenGL
//=============================================================================
//-----------------------------------------------------------------------------
// Default constructor
template <typename T>
ModelLaparoscopicAdjustableGastricBand<T>::ModelLaparoscopicAdjustableGastricBand ()
    : ModelStrand<T>( 128, 0.1, 0.1, Vector3<T>(), 0.05 )
      //, m_iLevel( 3 )
{
    InitHead();
    //---------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "Default ModelLaparoscopicAdjustableGastricBand<" 
                << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//-----------------------------------------------------------------------------
// Constructor
template <typename T>
ModelLaparoscopicAdjustableGastricBand<T>::ModelLaparoscopicAdjustableGastricBand 
    (   int iNoLinks, T tLength, T tWeight, 
        Vector3<T> & posOfVertex0, T tRadius )
    : ModelStrand<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius )
      //, m_iLevel( 3 )
{
    InitHead();
    //---------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "ModelLaparoscopicAdjustableGastricBand<" 
                << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//-----------------------------------------------------------------------------
// Constructor
template <typename T>
ModelLaparoscopicAdjustableGastricBand<T>::ModelLaparoscopicAdjustableGastricBand 
    (   int iNoLinks, T tLength, T tWeight, 
        Vector3<T> & posOfVertex0, T tRadius, 
        const char * head 
    )
    : ModelStrand<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius )
      //, m_iLevel( 3 )
{
    InitHead( head );
    //---------------------------------------------------------------
    //#ifdef    TAPs_ENABLE_DEBUG
    std::cout << "GastricBand: "    << iNoLinks << ", " 
                                    << tLength << ", " 
                                    << tWeight << ", " 
                                    << posOfVertex0 << ", " 
                                    << tRadius << ", " 
                                    << head << "\n";

    std::cout   << "ModelLaparoscopicAdjustableGastricBand<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    //#endif//TAPs_DEBUG_MODE
}
//-----------------------------------------------------------------------------
// Constructor
template <typename T>
ModelLaparoscopicAdjustableGastricBand<T>::ModelLaparoscopicAdjustableGastricBand 
    (   int iNoLinks, T tLength, T tWeight, 
        Vector3<T> & posOfVertex0, T tRadius, 
        T tKRatioOfStretchingAllowed,
        T tKRatioOfCompressionAllowed,
        T tKStretching,
        T tKBending,
        T tKTwisting,
        T tKFriction,
        T tKContact,
        T tKGravity,
        const char * head 
    )

    : ModelStrand<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius, 
            tKRatioOfStretchingAllowed, tKRatioOfCompressionAllowed, 
            tKStretching, tKBending, tKTwisting, 
            tKFriction, tKContact, tKGravity )
      //, m_iLevel( 3 )
{
    InitHead ( head );
    //---------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "ModelLaparoscopicAdjustableGastricBand<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//-----------------------------------------------------------------------------
// Destructor
template <typename T>
ModelLaparoscopicAdjustableGastricBand<T>::~ModelLaparoscopicAdjustableGastricBand ()
{
    //if ( m_pHead )    delete m_pHead;     // ERROR Deleting Object!  FIX IT!
    //if ( m_pTail )    delete m_pTail;
    //---------------------------------------------------------------
    //if ( m_prPickPts )    delete [] m_prPickPts;
    //if ( m_prbIsSharp )   delete [] m_prbIsSharp;
    //---------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout << "ModelLaparoscopicAdjustableGastricBand<" << typeid(T).name() 
              << "> Destructor\n";
    #endif//TAPs_DEBUG_MODE
}
//-----------------------------------------------------------------------------
// Initialize Head
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::InitHead ()
{
    //char fileName1[] = "Data/3dsMax/HeadNeedle.ASE";
    //char fileName1[] = "Data/3dsMax/HeadNeedle2.ASE";
    char head[] = "Data/3dsMax/HeadNeedle3.ASE";
    //char fileName2[] = "Data/3dsMax/HeadNeedle.ASE";
    InitHead( head );
}
//-----------------------------------------------------------------------------
// Initialize Head
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::InitHead ( const char * head )
{
    //m_pHead   = NULL;
    //m_pTail   = NULL;
    //---------------------------------------------------------------
    m_pHead = new MultiMeshModelWithMBV<T>();
    //m_pHead   = new OpenGLHalfEdgeModel<T>();
    //m_pTail   = new OpenGLHalfEdgeModel<T>();
    //---------------------------------------------------------------

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Create Head of an Adjustable Gastric Band" << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( !ReadModels<Real>::readFile( head, m_pHead ) ) {
        std::cout << "ERROR: In File Reading for creating the head of an adjustable gastric band!" << std::endl;
        exit ( EXIT_FAILURE );
    }
    else {
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "Creating Head of an adjustable gastric band" << std::endl;
        #endif//TAPs_DEBUG_MODE
    }
    //---------------------------------------------------------------
    /*

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Create Tail of an adjustable gastric band" << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( !ReadModels<Real>::readFile( tail, m_pTail ) ) {
        std::cout << "ERROR: In File Reading for creating the tail of an adjustable gastric band!" << std::endl;
        exit ( EXIT_FAILURE );
    }
    else {
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "Creating tail of an adjustable gastric band" << std::endl;
        #endif//TAPs_DEBUG_MODE

        m_pTail->SetMaterial( OpenGL::Enum::HEAD_NEEDLE );
        m_pTail->BuildBVHTree( TAPs::Enum::BVH_TREE_BINARY_SPHERE );
    }
    //*/
    //---------------------------------------------------------------

    //InitHeadPickPts();

    //SetHeadPickPts();

    //InitHeadSharpPts();

    SetHeadPosition();

    //std::cout << "Head of an adjustable gastric band: " << *m_pHead << "\n";
}
//-----------------------------------------------------------------------------
/*
// Initialize Head Pick Points
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::InitHeadPickPts ()
{
    // SET Number of Pick Pts HERE! ==> 2^m_iLevel
    m_iLevel = 4;
    //---------------------------------------------------------------
    // Set number of pick able points for head
    m_iNoPickPts = pow( 2, m_iLevel ); // * 2;

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Number of pickable points for the head is set at" << m_iNoPickPts << ".\n";
    #endif//TAPs_DEBUG_MODE

    m_prPickPts = new Vector3<T>*[ m_iNoPickPts ];
    assert( m_prPickPts );
    //---------------------------------------------------------------
    // Use a recursive function
    BVHNode<T> ** L = new BVHNode<T>*[1];
    L[0] = m_pHead->GetBVHTree()->Root();
    InitHeadPickPts( L, 0 );        // Node List and Level Number
    delete [] L;
}
//*/
//-----------------------------------------------------------------------------
/*
// Initialize Head Pick Points
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::InitHeadPickPts ( BVHNode<T> ** node, int level )
{
    #ifdef  TAPs_DEBUG_MODE
    std::cout << "InitHeadPickPts with LEVEL of " << level << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( level > m_iLevel ) return;
    //---------------------------------------------------------------
    int size = pow( 2, level );
    BVHNode<T> ** L = new BVHNode<T>*[ size*2 ];
    for ( int i = 0, j = 0; i < size; ++i, j+=2 ) {
        L[j  ] = node[i]->Child(0);
        L[j+1] = node[i]->Child(1);
    }
    InitHeadPickPts( L, ++level );
    //---------------------------------------------------------------
    if ( level == m_iLevel ) {
        //-------------------------------------------------
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "--------------------   HEAD PICK POINTS   --------------------\n";
        #endif//TAPs_DEBUG_MODE

        for ( int i = 0; i < m_iNoPickPts; ++i ) {
            m_prPickPts[i] = &(L[i]->GetCenter());

            #ifdef  TAPs_DEBUG_MODE
            std::cout << *(m_prPickPts[i]) << "\n";
            #endif//TAPs_DEBUG_MODE
        }
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "-------------------- END HEAD PICK POINTS --------------------\n";
        #endif//TAPs_DEBUG_MODE
    }
    delete [] L;
}
//*/
//-----------------------------------------------------------------------------
/*
// SetHeadPickPts
//   Align head with the movement of the strand
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::SetHeadPickPts ()
{
    static int i;
    static T angle;
    static Vector3<T> dir, axis;

    //---------------------------------------------------------------
    // Head
    i = 0;
    dir = m_prVertex[i] - m_prVertex[i+1];
    axis = dir ^ (-CGMath<T>::VectorY);
    angle = acos( (dir * (-CGMath<T>::VectorY)) / dir.Length() ) 
          * Math<T>::RAD_TO_DEG;
    // Translation and Rotation
    //m_vTransForHead  = m_prVertex[i];
    m_pHead->GetTransform().SetTranslation( m_prVertex[i] );
    //m_mRotMatForHead = CGMath<T>::CreateRotationMatrix3x3( axis, -angle );
    if ( !GetFixStatusOfPtNo( 0 ) )
        m_pHead->GetTransform().SetMatrixRotation( CGMath<T>::CreateRotationMatrix3x3( axis, -angle ) );
    //std::cout << m_pHead->GetTransform().GetMatrixRotation() << "\n";
}
//*/

//-----------------------------------------------------------------------------
// SetHeadPosition
//   Align head with the movement of the strand
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::SetHeadPosition ()
{
    static int i;
    static T angle;
    static Vector3<T> dir, axis;

    //---------------------------------------------------------------
    // Head
    i = 0;
    dir = m_prVertex[i] - m_prVertex[i+1];
    axis = dir ^ (-CGMath<T>::VectorY);
    angle = acos( (dir * (-CGMath<T>::VectorY)) / dir.Length() ) 
          * Math<T>::RAD_TO_DEG;
    // Translation and Rotation
    //m_vTransForHead  = m_prVertex[i];
    m_pHead->GetTransform().SetTranslation( m_prVertex[i] );
    //m_mRotMatForHead = CGMath<T>::CreateRotationMatrix3x3( axis, -angle );
    if ( !GetFixStatusOfPtNo( 0 ) )
        m_pHead->GetTransform().SetMatrixRotation( CGMath<T>::CreateRotationMatrix3x3( axis, -angle ) );
    //std::cout << m_pHead->GetTransform().GetMatrixRotation() << "\n";
}

//-----------------------------------------------------------------------------
/*
// InitHeadSharpPts
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::InitHeadSharpPts ()
{
    //---------------------------------------------------------------
    // For Head
    m_prbIsSharp = new bool[ m_iNoPickPts ];
    for ( int i = 0; i < m_iNoPickPts; ++i ) {
        m_prbIsSharp[i] = false;
    }
    //---------------------------------------------------------------
    int headSharpPt = 0;
    T maxDistance = (*(m_prPickPts[0]) - m_prVertex[0]).Length();
    T distance;
    for ( int i = 1; i < m_iNoPickPts; ++i ) {
        distance = (*(m_prPickPts[i]) - m_prVertex[0]).Length();
        if ( distance > maxDistance ) {
            headSharpPt = i;
            maxDistance = distance;
        }
    }
    m_prbIsSharp[headSharpPt] = true;
    m_iHeadSharpPt = headSharpPt;
}
//*/
//-----------------------------------------------------------------------------
template <typename T>
inline Vector3<T> ModelLaparoscopicAdjustableGastricBand<T>::GetPointPosition ( int i ) const
{
    if ( i <= m_iNoLinks )  return m_prVertex[i];
    else                    return GetHeadPosition( i-m_iNoLinks-1 );
}
//-----------------------------------------------------------------------------
template <typename T>
inline Vector3<T> ModelLaparoscopicAdjustableGastricBand<T>::GetHeadPosition ( int i ) const
{
    assert( 0 <= i && i < m_iNoPickPts );
    // Head
    //return m_mRotMatForHead * *(m_prPickPts[i]) + m_vTransForHead;
    return /*m_pHead->GetTransform().GetMatrixRotation() * *(m_prPickPts[i]) +*/ m_pHead->GetTransform().GetTranslation();
}
//-----------------------------------------------------------------------------
template <typename T>
inline void ModelLaparoscopicAdjustableGastricBand<T>::SetPointPosition ( 
    int i, const Vector3<T> & position )
{
    if ( i <= m_iNoLinks )  {
        ModelStrand<T>::SetPointPosition( i, position );
        //SetHeadPickPts();
    }
    else {
        //SetHeadPosition( i-m_iNoLinks-1, position, m_mRotMatForHead );

        SetHeadPosition( i-m_iNoLinks-1, position, m_pHead->GetTransform().GetMatrixRotation() );

        //ModelStrand<T>::SetPointPosition( 0, m_vTransForHead );

        //ModelStrand<T>::SetPointPosition( 1, position );
        ModelStrand<T>::SetPointPosition( 0, m_pHead->GetTransform().GetTranslation() );
    }
}
//-----------------------------------------------------------------------------
template <typename T>
inline void ModelLaparoscopicAdjustableGastricBand<T>::SetPointPosition ( 
    int i, const Vector3<T> & position, Matrix3x3<T> & rotation, 
    //T tMoveDistanceLimit, 
    MultiBoundingVolume<T> const * const pMBV, 
    TransformationSupport<T> const * const pTransform 
)
{
    if ( i <= m_iNoLinks )  {
        ModelStrand<T>::SetPointPosition( i, position, pMBV, pTransform );
        //SetHeadPickPts();
    }
    else {
        SetHeadPosition( i-m_iNoLinks-1, position, rotation );
        //ModelStrand<T>::SetPointPosition( 0, m_vTransForHead );
        ModelStrand<T>::SetPointPosition( 0, m_pHead->GetTransform().GetTranslation() );
    }
}
//-----------------------------------------------------------------------------
template <typename T>
inline void ModelLaparoscopicAdjustableGastricBand<T>::SetHeadPosition ( 
    int i, const Vector3<T> & position, Matrix3x3<T> & rotation )
{
    assert( 0 <= i && i < m_iNoPickPts );
    //---------------------------------------------------------------
    // Head
    //m_mRotMatForHead =  rotation;

    //rotation.MakeIdentity();

    // Rotate with the first link of the strand
    m_pHead->GetTransform().SetMatrixRotation( rotation );

    Vector3<T> translation = position - GetHeadPosition( i );
    //m_vTransForHead += translation;
    m_pHead->GetTransform().SetTranslation( 
        m_pHead->GetTransform().GetTranslation() + translation
    );
}
//-----------------------------------------------------------------------------
//=============================================================================
// Get/Set Fn(s)
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//=============================================================================
// Helper Fn(s)
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//=============================================================================
// For Simulation
//-----------------------------------------------------------------------------
// DxDt
template <typename T>
bool ModelLaparoscopicAdjustableGastricBand<T>::DxDt (  
    T dt,                           // i/p: time step
    Simulation::VectorSet<T> &x,    // i/p: array  x = {pos, vel}
    Simulation::VectorSet<T> &xdot, // o/p: array xdot = {vel, accel}
    void *userData                  // o/p: array of user data
)
{
    return ModelStrand<T>::DxDt( dt, x, xdot, userData );
}
//-----------------------------------------------------------------------------
// StateToArray
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::StateToArray ( T *dst )
{
    ModelStrand<T>::StateToArray( dst );
}
//-----------------------------------------------------------------------------
// ArrayToState
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::ArrayToState ( T *src )
{
    ModelStrand<T>::ArrayToState( src );
}
//-----------------------------------------------------------------------------
// DdtStateToArray
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::DdtStateToArray ( T *xdot, T dt )
{
    ModelStrand<T>::DdtStateToArray( xdot, dt );
}
//-----------------------------------------------------------------------------
// AdvanceSimulation
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::AdvanceSimulation ( T tCurrent, T tNext )
{
    ModelStrand<T>::AdvanceSimulation( tCurrent, tNext );
    //SetHeadPickPts();
}
//-----------------------------------------------------------------------------
// AdvanceSimulation
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::AdvanceSimulation ( Simulation::SimClock<T> & simClock )
{
    ModelStrand<T>::AdvanceSimulation( simClock );
    //SetHeadPickPts();
}
//-----------------------------------------------------------------------------

//=============================================================================
// Collision Detection
//-----------------------------------------------------------------------------
template <typename T>
bool ModelLaparoscopicAdjustableGastricBand<T>::TestOverlapWith ( BVHTree<T> const * const that )
{
    m_svCollidedNode.clear();
    m_svCollidedNodeThat.clear();
    //---------------------------------------------------------------
    bool result = GetBVHTree()->TestOverlapWithTillLeafNodes( that );
    if ( !result )  return result;
    //---------------------------------------------------------------
    BVHNode<T> * cylinder;
    BVHNode<T> * triangle;
    //---------------------------------------------------------------
    int size = static_cast<int>( GetBVHTree()->GetListOfCollidedNodes().size() );
    //*
    int linkNo;
    //std::cout << "\n";
    for ( int i = 0; i < size; ++i ) {
        cylinder = GetBVHTree()->GetListOfCollidedNodes()[i];
        triangle = GetBVHTree()->GetListOfCollidedNodesThat()[i];
        linkNo = cylinder->GetID();
        //std::cout << "Link No: " << linkNo;
        HEFace<T> * heFace = triangle->GetAPrimitiveHalfEdgeFace();
        std::vector< HEVertex<T> * const > vertices = heFace->GetPtrsToVertices();
        //*
        result = CGMath<T>::FindIntersectionCylinderTriangle(
                    GetPointPosition( linkNo ), GetPointPosition( linkNo+1 ), GetRadius(), 
                    vertices[0]->GetPosition(), 
                    vertices[1]->GetPosition(), 
                    vertices[2]->GetPosition() );
        //*/
        //result = true;
        
        if ( result ) {
            //-------------------------------------------------------
            // Collision Response (roll the link back to its previous position)
            //m_prVertex[ linkNo   ] += (m_prVertexPrev[ linkNo   ] - m_prVertex[ linkNo   ]);
            //m_prVertex[ linkNo+1 ] += (m_prVertexPrev[ linkNo+1 ] - m_prVertex[ linkNo+1 ]);
//          m_prVertex[ linkNo   ] = m_prVertexPrev[ linkNo-1 ];
//          m_prVertex[ linkNo+1 ] = m_prVertexPrev[ linkNo   ];
            //-------------------------------------------------------
            //SavePreviousPositions();
//          UpdateBVHTree();
            //-------------------------------------------------------
            //-------------------------------------------------------

            //std::cout << " ---> in collsions";
            m_svCollidedNode.push_back( GetBVHTree()->GetListOfCollidedNodes()[i] );
            m_svCollidedNodeThat.push_back( GetBVHTree()->GetListOfCollidedNodesThat()[i] );
        }
        //std::cout << "\n";
    }
    //std::cout << "Old Size: " << GetBVHTree()->GetListOfCollidedNodes().size() << " " << GetBVHTree()->GetListOfCollidedNodesThat().size() << "\n";
    //std::cout << "New Size: " << m_svCollidedNode.size() << " " << m_svCollidedNodeThat.size() << "\n";
    //*/
    //---------------------------------------------------------------
    if ( static_cast<int>( m_svCollidedNode.size() ) > 0 )  return true;
    else                                                    return false;
}
//-----------------------------------------------------------------------------
//=============================================================================

//-----------------------------------------------------------------------------
// SetupShape Fn
template <typename T>
void ModelLaparoscopicAdjustableGastricBand<T>::SetupShape ( Vector3<T> startPosition )
{
    ModelStrand<T>::SetupShape( startPosition );
}
//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs__OpenGL
//-----------------------------------------------------------------------------
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----