TAPsModelSurgicalSutureWithHeadNeedle_CD.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsModelSurgicalSutureWithHeadNeedle_CD.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (08/21/2009)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsModelSurgicalSutureWithHeadNeedle_CD.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
//=============================================================================
//-----------------------------------------------------------------------------

// Constructor
template <typename T>
ModelSurgicalSutureWithHeadNeedle_CD<T>::ModelSurgicalSutureWithHeadNeedle_CD () : 
    m_BVHTree( NULL ),
    m_pNeedleSharpPt( NULL ),
    m_pNeedleSutureConnectingPt( NULL )
{}


// Destructor
template <typename T>
ModelSurgicalSutureWithHeadNeedle_CD<T>::~ModelSurgicalSutureWithHeadNeedle_CD ()
{
    DeleteCD();
}


// Create collision detection process
template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::CreateCD (
    RigidBodyDynamics<T> *  pHeadNeedle,                
    InteractionPoint<T> *   pNeedleSharpPt,             
    InteractionPoint<T> *   pNeedleSutureConnectingPt   
)
{
    m_pHeadNeedle = pHeadNeedle;
    m_pNeedleSharpPt = pNeedleSharpPt;
    m_pNeedleSutureConnectingPt = pNeedleSutureConnectingPt;

    m_pHeadNeedleTranslation = const_cast< Vector3<T>* >( &(pHeadNeedle->GetOriginPoint()) );
    m_pHeadNeedleRotation = const_cast< Quaternion<T>* >( &(pHeadNeedle->GetOrientation()) );

    BuildBVHTree();
}

// Delete collision detection process
template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::DeleteCD ()
{
    if ( m_BVHTree ) {
        delete m_BVHTree;
        m_BVHTree = NULL;
    }
}

// Build the BVH tree for the elastic rod
template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::BuildBVHTree ()
{
    //----------------------------------------------------------------
    Vector3<T> center;
    //                  /                      -------------------
    // sphere radius = ( link's radius    +    link's half length
    //                  \                      -------------------
    //T radius = m_pParameters->Radius + m_pParameters->LinkLength/2.0;
    //----------------------------------------------------------------
    // Create Leaf Nodes
    int numOfLinks = m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().size() - 1;
    BVHNode<T> **nodeList = new BVHNode<T> *[ numOfLinks ];
    std::vector< InteractionPoint<T> >::iterator  nextNode  = m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().begin();
    std::vector< InteractionPoint<T> >::iterator  currNode  = nextNode++;

    //*
    int numOfNodes = -1;
    for ( int i = 0; i < numOfLinks; ++i, ++currNode, ++nextNode ) {
        BVHNodeLeafALinkFormedByTwoPointMasses<T> * pBVNode = new BVHNodeLeafALinkFormedByTwoPointMasses<T>( 
                                //TAPs::Enum::BVH_NODE_LEAF_SPHERE_UNSPECIFIED_PRIMS, 
                                TAPs::Enum::BVH_NODE_LEAF_SPHERE_TWO_POINT_MASSES, 
                                ++numOfNodes,   // node id
                                NULL            // parent node
        );
        pBVNode->SetPtrToPrimitive_1( currNode->PtrToPointMass() );
        pBVNode->SetPtrToPrimitive_2( nextNode->PtrToPointMass() );
        pBVNode->Update();
        nodeList[i] = pBVNode;

        //std::cout << "NODE: " << i << ":\t" << nodeList[i]->GetRadius() << "\n";
        //std::cout << "\t" << nodeList[i]->GetCenter() << "\n";
    }

    //----------------------------------------------------------------
    // Create Hierarchy Nodes
    m_BVHTree = BuildBVHTreeRecursively( numOfNodes+1, nodeList, numOfLinks );
    UpdateBVHTree();
    //m_BVHTree->Update();
    
    //std::cout << "TREE1\n";
    //std::cout << *m_BVHTree << "\n";
    //m_BVHTree->Update();
    //std::cout << "TREE2\n";
    //std::cout << *m_BVHTree << "\n";
}


// Build the BVH tree recursively (called by BuildBVHTree function)
template <typename T>
BVHTree<T> * ModelSurgicalSutureWithHeadNeedle_CD<T>::BuildBVHTreeRecursively (
    int startID,                    
    BVHNode<T> ** childNodeList,    
    int numOfChildNodes             
)
{
    //std::cout << "numOfChildNodes: " << numOfChildNodes << "\n";
    BVHNode<T> ** parentNodeList;
    if ( numOfChildNodes > 1 ) {
        int numOfParentNodes;
        if ( numOfChildNodes % 2 == 0 ) {
            numOfParentNodes = numOfChildNodes/2;
            //std::cout << "numOfParentNodes (even): " << numOfParentNodes << "\n";
            parentNodeList = new BVHNode<T> *[ numOfParentNodes ];
            for ( int i = 0, p = 0; i < numOfChildNodes; i+=2, ++p, ++startID ) {
                parentNodeList[p] = new BVHNode<T>( 
                        TAPs::Enum::BVH_NODE_BINARY_SPHERE, 
                        startID, NULL, NULL );
                parentNodeList[p]->Child( 0, childNodeList[i] );
                parentNodeList[p]->Child( 1, childNodeList[i+1] );
            }
        }
        else {
            numOfParentNodes = numOfChildNodes/2 + 1;
            //std::cout << "numOfParentNodes (odd): " << numOfParentNodes << "\n";
            parentNodeList = new BVHNode<T> *[ numOfParentNodes ];
            for ( int i = 0, p = 0; i < numOfChildNodes-1; i+=2, ++p, ++startID ) {
                parentNodeList[p] = new BVHNode<T>( 
                        TAPs::Enum::BVH_NODE_BINARY_SPHERE, 
                        startID, NULL, NULL );
                parentNodeList[p]->Child( 0, childNodeList[i] );
                parentNodeList[p]->Child( 1, childNodeList[i+1] );
            }
            parentNodeList[numOfParentNodes-1] = childNodeList[numOfChildNodes-1];
        }
        delete [] childNodeList;
        return BuildBVHTreeRecursively( startID, parentNodeList, numOfParentNodes );
    }
    else {
        BVHTree<T> * tree = new BVHTree_BinarySphere<T>( m_DummyTransform, 
                /*TAPs::Enum::BVH_TREE_BINARY_SPHERE,*/ startID, childNodeList[0] );
        delete [] childNodeList;
        return tree;
    }
}


template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::UpdateBVHTree ()
{
    if ( m_BVHTree->Root() ) {
        UpdateBVHNodeRecursively( m_BVHTree->Root() );
    }
}

    
template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::UpdateBVHNodeRecursively ( BVHNode<T> * node )
{
    if ( node->Child(0) ) {
        UpdateBVHNodeRecursively( node->Child(0) );
    }
    if ( node->Child(1) ) {
        UpdateBVHNodeRecursively( node->Child(1) );
    }
    //----------------------------------------------------------------
    if ( node->Child(0) == NULL && node->Child(1) == NULL ) {
        int id = node->GetID();
        //node->SetRadius( (m_prVertex[id] - m_prVertex[id+1]).Length()/2.0 + m_tRadius );
        //node->SetCenter( (m_prVertex[id] + m_prVertex[id+1]) / 2.0 );
    }
    else {
        node->Update();
    }
}


/*
// Collision Detection and Response with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle_CD<T>::CDRwith (
    MultiBoundingVolume<T> * const pMBVObj,     //!< a multi bounding volume object
    TransformationSupport<T> * const pTransform //!< an extra transformation for the MBV object
)
{
    bool bResult = false;

    SetOfElasticRodNodes::iterator ERNodes = m_pNodeList->begin();

    //---------------------------------------------------------------
    TransformationSupport<T> transform;
    //---------------------------------------------------------------
    if ( pTransform ) {
        transform.RefToMatrixTransform() *= pTransform->GetMatrixTransform();
    }
    transform.RefToMatrixTransform() *= pMBVObj->GetTransform().GetMatrixTransform();

    // Check collision
    TransformationSupport<T> oldTrx( pMBVObj->GetTransform() );     // store the old transformation
    pMBVObj->GetTransform() = transform;                            // set to the new transformation
    bResult = this->GetBVHTree()->TestOverlapWithTillLeafNodes( pMBVObj );  // collision detection automatically uses the new transformation
    pMBVObj->GetTransform() = oldTrx;                               // restore the old transformation
    if ( !bResult ) return bResult;

    // Collision response constants are scaled by the number of core simutation iterations.
    T timestep_sqrt = m_pParameters->TimeStep * m_pParameters->TimeStep;
    T Ksphere   = m_pParameters->K_CDRwBV_Sphere / timestep_sqrt;
    T Kcylinder = m_pParameters->K_CDRwBV_Cylinder / timestep_sqrt;

    TransformationSupport<T> savedTransform( transform );

    // Get the collided node lists and the collided BVs
    std::vector< BVsAndNodesList<T> > & cdDat = this->GetBVHTree()->GetListOfCollidedBoundingVolumesAndNodes();

    // Iterate all of the collided BVs
    std::vector< BVsAndNodesList<T> >::iterator it = cdDat.begin();
    while ( it != cdDat.end() ) {
        BoundingVolume<T> * pBV = it->BV;
        assert( pBV != NULL );
        transform = savedTransform;

        //-------------------------------------------------
        transform.RefToMatrixTransform() *= pBV->GetTransform().GetMatrixTransform();
        //-------------------------------------------------
        // Transform each vertex of the strand to the bounding volume coordinate
        //int   iDeepestPoint       = -1;
        //T tDeepestDistance    = Math<T>::MAX;
        Vector3<T>      vDirection;
        Vector3<T>      translation( transform.GetTranslation() );
        Matrix3x3<T>    inversedRotMatrix( transform.GetMatrixRotation().GetInverse() );
        //T distance;
        Vector3<T> U;   // displacement vector
        std::vector< BVHNode<T> * >::iterator   node = it->NodeList.begin();

        //T epsilon = m_pParameters->Radius * 0.05; // for collision response

        switch ( pBV->GetType() ) {
            //---------------------------------------------
            case TAPs::Enum::BOUNDING_CYLINDER:
            //---------------------------------------------
                {
                    // Test only points against the cylinder BV, NOT cylinders againt the BV.

                    T combinedRadius = m_pParameters->Radius + pBV->GetRadius() + m_pParameters->Offset_CD_BV_Cylinder;
                    while ( node != it->NodeList.end() ) {
                        BVHNodeLeafElasticRodNode<T> * Node = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( *node );

                        // In the local coordinate of the bounding cylinder, 
                        // the cylinder axis is parallel to z-axis 
                        // where it is shifted by the cylinder center.
                        // The cylinder has radius, height and center.
                        // Its height is measured from -z and +z axis
                        // where its center is situated between height/2 in -z and +z axis.
                        Vector3<T> P1( Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition() );
                        Vector3<T> P2( Node->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition() );
                        P1 -= translation;
                        P1 = inversedRotMatrix * P1;
                        P2 -= translation;
                        P2 = inversedRotMatrix * P2;
                        // Shift the location of point by the cylinder center
                        // i.e. shift the cylinder to the origin (0,0,0)
                        P1 -= pBV->GetCenter();
                        P2 -= pBV->GetCenter();

                        // Now the cylinder is centered at the origin with its axis on the z-axis
                        // where its height is from -height/2 to +height/2.
                        // And the input point has been transformed into the cylinder coordinate.
                        // The test can be performed.

                        // The 1st point
                        if ( CGMath<T>::CD_CylinderAtOrigin_vs_Point( combinedRadius, pBV->GetHeight(), P1, U ) ) {
                            bResult = true;
                            // Now rotate it back by the rotation matrix (from the transform matrix).
                            // REMARK: Translation (from the transform matrix) is NOT applied back, 
                            //         because U represents a displacement vector from 
                            //         the input point to the cylinder surface.
                            //         I.e., the strand point is always in the world coordinates.
                            U = transform.GetMatrixRotation() * U;

                            U *= Kcylinder * Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass();
                            Node->GetPtrToPrimitive_1()->ExternalForce += U;

                            //U *= 1.0;
                            //Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetPosition( Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition() + U );

                        }

                        // The 2nd point
                        // SKIPPED to avoid duplicating the replusive force
                        //if ( CGMath<T>::CD_CylinderAtOrigin_vs_Sphere( pBV->GetRadius(), pBV->GetHeight(), P2, m_pParameters->Radius, U ) ) {
                        //  bResult = true;
                        //  // Now rotate it back by the rotation matrix (from the transform matrix).
                        //  // REMARK: Translation (from the transform matrix) is NOT applied back, 
                        //  //         because U represents a displacement vector from 
                        //  //         the input point to the cylinder surface.
                        //  //         I.e., the strand point is always in the world coordinates.
                        //  U = transform.GetMatrixRotation() * U;
                        //  U *= Kcylinder;
                        //  Node->GetPtrToPrimitive_2()->ExternalForce += U;
                        //}

                        ++node;
                    }
                }
                break;
                // END: BOUNDING_CYLINDER

            //---------------------------------------------
            case TAPs::Enum::BOUNDING_SPHERE:
            //---------------------------------------------
                {
                    // Each sphere bounding volume has its own center location and transformation.
                    // To get the world coordinate location of the sphere, the sphere center point 
                    // is transformed by the transformation.
                    Vector3<T> Cs = ( (transform.RefToMatrixTransform() * Vector4<T>( pBV->GetCenter() )).GetVector3() );
                    T Rs = ( ( transform.RefToMatrixTransform() * Vector4<T>(Cs + Vector3<T>(0,0,pBV->GetRadius())) ).GetVector3() - Cs ).Length();

                    T combinedRadius = m_pParameters->Radius + pBV->GetRadius() + m_pParameters->Offset_CD_BV_Sphere;
                    while ( node != it->NodeList.end() ) {

                        BVHNodeLeafElasticRodNode<T> * Node = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( *node );
    
                        T ratio;
                        Vector3<T> projPt;
                        CGMath<T>::FindProjectedPointOnALine(
                            Cs,     // sphere BV's center
                            Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition(),
                            Node->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition(),
                            ratio,
                            projPt
                        );
                        if ( 0 <= ratio && ratio <= 1 ) {
                            U = projPt - Cs;
                            if ( U.Length() < combinedRadius ) {
                                bResult = true;
                                // By force
                                U *= Ksphere * Node->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass();
                                Node->GetPtrToPrimitive_1()->ExternalForce += U;
                                // The 2nd point is skipped to avoid duplicating the repulsive force
                                //Node->GetPtrToPrimitive_2()->ExternalForce += U;
                            }
                        }
                        ++node;
                    }
                }
                break;
                // END: BOUNDING_SPHERE

        }//END: switch ( pBV->GetType() ) {...}
        ++it;   // Next Bounding Volume
    }
    return bResult;
}
//*/


/*
// Collision Detection and Response with a HETriMeshOneModelMultiParts object
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle_CD<T>::CDRwith (
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pObj,    //!< a HETriMeshOneModelMultiParts object
    TransformationSupport<T> * const pTransform             //!< an extra transformation for the HETriMeshOneModelMultiParts object
)
{
    bool bResult = false;

    // IsCollide is for DEBUG
    SetOfElasticRodNodes::iterator it = m_pNodeList->begin();
    while ( it != m_pNodeList->end() ) {
        it->IsCollide = false;
        ++it;
    }

    TransformationSupport<T> transform;
    if ( pTransform ) {
        transform.RefToMatrixTransform() *= pTransform->RefToMatrixTransform();
    }
    transform.RefToMatrixTransform() *= pObj->GetTransform().RefToMatrixTransform();

    // Collision Detection
    TransformationSupport<T> oldTrx( pObj->GetTransform() );    // store the old transformation
    pObj->GetTransform() = transform;                           // set to the new transformation
    bResult = GetBVHTree()->TestOverlapWithTillLeafNodes( pObj->GetBVHTree() ); // collision detection automatically uses the new transformation
    pObj->GetTransform() = oldTrx;                              // restore the old transformation
    if ( !bResult ) return bResult;

    // Collision Response
    bResult = false;
    T Kc = m_pParameters->K_CDRwTriangle;

    // The list of collided BV leaf nodes
    std::vector< BVHNode<T> * > & ER_BVNodes  = GetBVHTree()->GetListOfCollidedNodes();
    std::vector< BVHNode<T> * > & Obj_BVNodes = GetBVHTree()->GetListOfCollidedNodesThat();
    std::vector< HEVertex<T> * const > vertexList;
    int noOfVertices;
    Matrix4x4<T> & TrxMat = transform.RefToMatrixTransform();

    for ( int i = 0; i < static_cast<int>( ER_BVNodes.size() ); ++i ) {
        BVHNodeLeafElasticRodNode<T> * NodeER = dynamic_cast< BVHNodeLeafElasticRodNode<T> * >( ER_BVNodes[i] );
        HEFace<T> * Triangle = Obj_BVNodes[i]->GetAPrimitiveHalfEdgeFace();
        vertexList = Triangle->GetPtrsToVerticesAndNumberOfVertices( noOfVertices );
        Vector3<T> cylA( NodeER->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition() );
        Vector3<T> cylB( NodeER->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition() );
        Vector3<T> posA( (TrxMat * Vector4<T>(vertexList[0]->GetPosition())).GetVector3() );
        Vector3<T> posB( (TrxMat * Vector4<T>(vertexList[1]->GetPosition())).GetVector3() );
        Vector3<T> posC( (TrxMat * Vector4<T>(vertexList[2]->GetPosition())).GetVector3() );
        if ( TAPs::CGMath<T>::FindIntersectionCylinderTriangle(
                cylA, cylB,
                m_pParameters->Radius + m_pParameters->Offset_CD_Triangle,
                posA, posB, posC
        ) ) {
            Vector3<T> triCentroid( ( posA + posB + posC ) / 3.0 );
            Vector3<T> N( Triangle->GetNormal() );
            N.Normalized();
            Vector3<T> projPt1, projPt2;
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylA, posB, N, projPt1 );
            TAPs::CGMath<T>::FindProjectedPointOnAPlane( cylB, posB, N, projPt2 );
            Vector3<T> V1( cylA - projPt1 );
            Vector3<T> V2( cylB - projPt2 );
            //Vector3<T> V1( cylA - triCentroid );
            //Vector3<T> V2( cylB - triCentroid );
            T value = V1 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_1()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_1()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetPosition( cylA - value * N );   // adjusted by position
                }
            }
            value = V2 * N;
            if ( value < 0.0 ) {
                if ( NodeER->GetPtrToPrimitive_2()->SimFlags.CheckSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE ) == false ) {
                    NodeER->GetPtrToPrimitive_2()->ExternalForce -= value*Kc * N;   // adjusted by force
                    //NodeER->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].SetPosition( cylB - value * N );   // adjusted by position
                }
            }
            bResult = true;

            // DEBUG
            NodeER->GetPtrToPrimitive_1()->IsCollide = true;
            NodeER->GetPtrToPrimitive_2()->IsCollide = true;

        }
    }
    return bResult;
}
//*/
//-----------------------------------------------------------------------------




#if defined(__gl_h_) || defined(__GL_H__)
//=============================================================================
template <typename T>
void ModelSurgicalSutureWithHeadNeedle_CD<T>::Draw () const
{
    if ( m_BVHTree ) {
        glPushMatrix();
        glPushAttrib( GL_ALL_ATTRIB_BITS );

        glTranslatef( m_pHeadNeedleTranslation->GetX(), m_pHeadNeedleTranslation->GetY(), m_pHeadNeedleTranslation->GetZ() );
        Matrix4x4<T> orientation = m_pHeadNeedleRotation->GetRotationMatrix4x4();
        GLfloat m[16] = {
            static_cast<GLfloat>(orientation[ 0]), static_cast<GLfloat>(orientation[ 4]), static_cast<GLfloat>(orientation[ 8]), static_cast<GLfloat>(orientation[ 12]), 
            static_cast<GLfloat>(orientation[ 1]), static_cast<GLfloat>(orientation[ 5]), static_cast<GLfloat>(orientation[ 9]), static_cast<GLfloat>(orientation[ 13]), 
            static_cast<GLfloat>(orientation[ 2]), static_cast<GLfloat>(orientation[ 6]), static_cast<GLfloat>(orientation[10]), static_cast<GLfloat>(orientation[ 14]), 
            static_cast<GLfloat>(orientation[ 3]), static_cast<GLfloat>(orientation[ 7]), static_cast<GLfloat>(orientation[11]), static_cast<GLfloat>(orientation[ 15])
        };
        glMultMatrixf( m );
        glPopAttrib();
        glPopMatrix();

        m_BVHTree->Draw();
    }
}
//=============================================================================
#endif // #if defined(__gl_h_) || defined(__GL_H__)

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