TAPsElasticRodCD.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsElasticRodCD.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (08/21/2009)
UPDATE          (01/04/2011)
******************************************************************************/
#include "TAPsElasticRodCD.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>
ElasticRodCD<T>::ElasticRodCD () : 
    m_BVHTree( NULL ),
    m_pIdxCurr( NULL ),
    m_pIdxNext( NULL ),
    m_pParameters( NULL ),
    m_pNodeList( NULL )
{}

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

//-----------------------------------------------------------------------------
// StrInfo
template <typename T>
std::string ElasticRodCD<T>::StrInfo () const
{
    std::stringstream output;
    output  <<  "Pointer: " << this << "\t"
            <<  "TAPs::ElasticRodCD<"
            <<  typeid(T).name() << "> Class:";
    //-------------------------------------------------
    output  << ""
            << "\n";
    
    return output.str();
}

//-----------------------------------------------------------------------------
// Create collision detection process
template <typename T>
void ElasticRodCD<T>::CreateCD (
    int *                       pIdxCurr,       
    int *                       pIdxNext,       
    ElasticRodParameters<T> *   pParameters,    
    SetOfElasticRodNodes *      pNodeList       
)
{
    m_pIdxCurr    = pIdxCurr;
    m_pIdxNext    = pIdxNext;
    m_pParameters = pParameters;
    m_pNodeList   = pNodeList;

    #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
        InitSelfCDExtraData();
    #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

    BuildBVHTree();
}

//-----------------------------------------------------------------------------
// Delete collision detection process
template <typename T>
void ElasticRodCD<T>::DeleteCD ()
{
    #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
        DeleteSelfCDExtraData();
    #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

    delete m_BVHTree;
    if ( m_BVHTree ) {
        m_BVHTree = NULL;
    }
}

//-----------------------------------------------------------------------------
// Reset
template <typename T>
void ElasticRodCD<T>::Reset ()
{
    #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
        ResetSelfCDExtraData();
    #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION
}

//-----------------------------------------------------------------------------
// Build the BVH tree for the elastic rod
template <typename T>
void ElasticRodCD<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_pParameters->NumOfNodes - 1;
    BVHNode<T> **nodeList = new BVHNode<T> *[ numOfLinks ];
    SetOfElasticRodNodes::iterator  nextNode  = m_pNodeList->begin();
    SetOfElasticRodNodes::iterator  currNode  = nextNode++;

    //*
    int numOfNodes = -1;
    for ( int i = 0; i < numOfLinks; ++i, ++currNode, ++nextNode ) {
        BVHNodeLeafElasticRodNode<T> * pBVNode = new BVHNodeLeafElasticRodNode<T>( 
                                TAPs::Enum::BVH_NODE_LEAF_SPHERE_TWO_ELASTIC_ROD_NODES, 
                                ++numOfNodes,   // node id
                                NULL            // parent node
        );
        pBVNode->SetPtrToPrimitive_1( &(*currNode) );
        pBVNode->SetPtrToPrimitive_2( &(*nextNode) );
        nodeList[i] = pBVNode;
        nodeList[i]->SetRadius( radius );
        nodeList[i]->SetCenter( ( currNode->Centerline[*m_pIdxCurr].GetPosition() + nextNode->Centerline[*m_pIdxCurr].GetPosition() ) / 2.0 );
        //nodeList[i]->SetCenter( Vector3<T>(0,0,0) );

        //std::cout << i << ": " << pBVNode->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr] << "\n";
    }
    //*/

    //----------------------------------------------------------------
    // Create Hierarchy Nodes
    m_BVHTree = BuildBVHTreeRecursively( numOfNodes+1, nodeList, numOfLinks );

    //std::cout << "*m_BVHTree: " << *m_BVHTree << "\n";
    //std::cout << m_BVHTree->DisplayAllNodesAsString() << "\n";
}

//-----------------------------------------------------------------------------
// Collision Detection and Response with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ElasticRodCD<T>::CDRwith (
    MultiBoundingVolume<T> * const pMBVObj,     
    TransformationSupport<T> * const pTransform 
)
{
    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 by selected segments with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ElasticRodCD<T>::CDRbySegmentsWith (
    std::vector<unsigned int> const * pSegments,    
    MultiBoundingVolume<T> * const pMBVObj,         
    TransformationSupport<T> * const pTransform     
)
{
    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();

    //
    // Create skip/unskip status of all nodes
    //
    std::vector<bool> unskips( m_pNodeList->size() - 1, false );
    if ( cdDat.size() > 0 ) {
        for ( unsigned int i = 0; i < pSegments->size() / 2; i+=2 ) {
            unsigned int start = (*pSegments)[i];
            unsigned int end = (*pSegments)[i+1];
            for ( unsigned int p = start; p <= end; ++p ) {
                unskips[p] = true;
            }
        }
    }

    // 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() ) {
                        if ( unskips[ (*node)->GetID() ] ) {
                            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() ) {
                        if ( unskips[ (*node)->GetID() ] ) {
                            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 an MBV (Multi-Bounding Volume) object
template <typename T>
bool ElasticRodCD<T>::CDRwithMBVforShaftLikeObject_wCheckWrappingLoops (
    MultiBoundingVolume<T> * const pMBVObj,         
    Vector3<T> * const pShaftStartPt,               
    Vector3<T> * const pShaftEndPt,                 
    T const *   pShaftRadiusOfInfluence,            
    TransformationSupport<T> * const pTransform,    
    T * pNumOfWrappingLoops,                        
    bool *  isCCW                                   
    //, std::vector<Loop> * pWrappingLoops  //!< list of formed wrapping loops
)
{
    bool result = CDRwith( pMBVObj, pTransform );

    // DEBUG
    //d_NodePositions.clear();

    if ( result )
    {
        Vector3<T> lineStartPt, lineEndPt;
        Vector3<T> lineStartPt_world, lineEndPt_world;
        // Transform the shaft line by the extra transformation
        if ( pTransform ) {
            lineStartPt_world   = (pTransform->GetMatrixTransform() * Vector4<T>(*pShaftStartPt)).GetVector3();
            lineEndPt_world     = (pTransform->GetMatrixTransform() * Vector4<T>(*pShaftEndPt)).GetVector3();
        }
        else {
            lineStartPt_world   = *pShaftStartPt;
            lineEndPt_world     = *pShaftEndPt;
        }

        // Transform the line start point to be at the origin
        // and the line end point to be at a positive z-value
        //lineEndPt -= lineStartPt;

        Matrix4x4<T> rotMat = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( (lineEndPt_world-lineStartPt_world).GetUnit(), CGMath<T>::VectorZ ); 
        Matrix4x4<T> transMat = Matrix4x4<T> (
                                    1, 0, 0, -lineStartPt_world[0],
                                    0, 1, 0, -lineStartPt_world[1],
                                    0, 0, 1, -lineStartPt_world[2],
                                    0, 0, 0, 1
                                );

        Matrix4x4<T> trx = rotMat * transMat;

        // 360 degrees divided into 8 zones; each contain 45 degrees
        const int NUM_ZONES = 8;
        //static unsigned char zoneCounts[NUM_ZONES];
        //memset( zoneCounts, 0, sizeof(unsigned char)*NUM_ZONES );
        int prevZone = 0, currZone = 0;

        T zStartPt = 0; // always equals to zero due to the transformation
        T zEndPt = (trx * lineEndPt_world).GetZ(); // always align with the z-axis due to the transformation

        int count = 0;

        // Check with all of elastic rod's nodes
        SetOfElasticRodNodes::iterator it = m_pNodeList->begin();
        while ( it != m_pNodeList->end() ) {
            Vector3<T> nodePos = trx * it->Centerline[*m_pIdxCurr].GetPosition();

            // DEBUG
            //d_NodePositions.push_back( nodePos );

            T xAbsVal = fabs( nodePos[0] );
            T yAbsVal = fabs( nodePos[1] );
            int greaterThan45Degs = ( xAbsVal < yAbsVal ) ? 1 : 0;
            if ( nodePos[0] > 0 ) {
                if ( nodePos[1] > 0 )   currZone = 0 + greaterThan45Degs;   // 0-90 degrees
                else                    currZone = 7 - greaterThan45Degs;   // 270-360 degrees
            }
            else {
                if ( nodePos[1] > 0 )   currZone = 3 - greaterThan45Degs;   // 90-180 degrees
                else                    currZone = 4 + greaterThan45Degs;   // 180-270 degrees
            }
            if ( zStartPt <= nodePos[2] && nodePos[2] <= zEndPt ) {
                // Check that the xy distance does not over the shaft's radius of influence
                if ( sqrt( xAbsVal*xAbsVal + yAbsVal*yAbsVal ) <= *pShaftRadiusOfInfluence ) {
                    if ( prevZone != currZone )
                    {
                        //++zoneCounts[currZone];

                        if ( currZone == 0 ) {
                            if ( prevZone == NUM_ZONES-1 ) {
                                ++count; 
                            }
                            else {
                                --count;
                            }
                        }
                        else if ( currZone == NUM_ZONES-1 ) {
                            if ( prevZone == 0 ) {
                                --count;
                            }
                            else {
                                ++count;
                            }
                        }
                        else if ( currZone > prevZone ) {
                            ++count;
                        }
                        else {//if ( currZone < prevZone ) {
                            --count;
                        }
                    }
                }
            }
            prevZone = currZone;
            ++it;
        }

        //int lowestVal = 1E3;
        //for ( int i = 0; i < NUM_ZONES; ++i ) {
        //  if ( lowestVal > zoneCounts[i] )    lowestVal = zoneCounts[i];
        //  //std::cout << "zone " << i << ": " << (int)zoneCounts[i] << "\n";
        //}
        //*pNumOfWrappingLoops = lowestVal;

        // Check the number of loops and direction based on the count value
        if ( count <= 0 )   *isCCW = true;
        else                *isCCW = false;
        T loops = static_cast<T>( abs(count) ) / NUM_ZONES;
        *pNumOfWrappingLoops = loops;

        // DEBUG -- by drawing
        d_NumOfWrappingLoops = static_cast<int>( *pNumOfWrappingLoops );
        d_ShaftSPt = trx * lineStartPt_world;
        d_ShaftEPt = trx * lineEndPt_world;
    }

    return result;
}

//-----------------------------------------------------------------------------
// Collision Detection and Response by selected segments with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ElasticRodCD<T>::CDRbySegmentsWithMBVforShaftLikeObject_wCheckWrappingLoops (
    std::vector<unsigned int> const * pSegments,    
    MultiBoundingVolume<T> * const pMBVObj,         
    Vector3<T> * const pShaftStartPt,               
    Vector3<T> * const pShaftEndPt,                 
    T const *   pShaftRadiusOfInfluence,            
    TransformationSupport<T> * const pTransform,    
    T * pNumOfWrappingLoops,                        
    bool *  isCCW                                   
    //, std::vector<Loop> * pWrappingLoops  //!< list of formed wrapping loops
)
{
    bool result = CDRbySegmentsWith( pSegments, pMBVObj, pTransform );

    // DEBUG
    //d_NodePositions.clear();

    if ( result )
    {
        Vector3<T> lineStartPt, lineEndPt;
        Vector3<T> lineStartPt_world, lineEndPt_world;
        // Transform the shaft line by the extra transformation
        if ( pTransform ) {
            lineStartPt_world   = (pTransform->GetMatrixTransform() * Vector4<T>(*pShaftStartPt)).GetVector3();
            lineEndPt_world     = (pTransform->GetMatrixTransform() * Vector4<T>(*pShaftEndPt)).GetVector3();
        }
        else {
            lineStartPt_world   = *pShaftStartPt;
            lineEndPt_world     = *pShaftEndPt;
        }

        // Transform the line start point to be at the origin
        // and the line end point to be at a positive z-value
        //lineEndPt -= lineStartPt;

        Matrix4x4<T> rotMat = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( (lineEndPt_world-lineStartPt_world).GetUnit(), CGMath<T>::VectorZ ); 
        Matrix4x4<T> transMat = Matrix4x4<T> (
                                    1, 0, 0, -lineStartPt_world[0],
                                    0, 1, 0, -lineStartPt_world[1],
                                    0, 0, 1, -lineStartPt_world[2],
                                    0, 0, 0, 1
                                );

        Matrix4x4<T> trx = rotMat * transMat;

        // 360 degrees divided into 8 zones; each contain 45 degrees
        const int NUM_ZONES = 8;
        //static unsigned char zoneCounts[NUM_ZONES];
        //memset( zoneCounts, 0, sizeof(unsigned char)*NUM_ZONES );
        int prevZone = 0, currZone = 0;

        T zStartPt = 0; // always equals to zero due to the transformation
        T zEndPt = (trx * lineEndPt_world).GetZ(); // always align with the z-axis due to the transformation

        int count = 0;

        //
        // Create skip/unskip status of all nodes
        //
        std::vector<bool> unskips( m_pNodeList->size(), false );
        for ( unsigned int i = 0; i < pSegments->size() / 2; i+=2 ) {
            unsigned int start = (*pSegments)[i];
            unsigned int end = (*pSegments)[i+1];
            for ( unsigned int p = start; p <= end; ++p ) {
                unskips[p] = true;
            }
        }

        // Check with all of elastic rod's nodes
        SetOfElasticRodNodes::iterator it = m_pNodeList->begin();
        int nodeID = 0;
        while ( it != m_pNodeList->end() ) {
            if ( unskips[ nodeID++ ] ) {
                Vector3<T> nodePos = trx * it->Centerline[*m_pIdxCurr].GetPosition();

                // DEBUG
                //d_NodePositions.push_back( nodePos );

                T xAbsVal = fabs( nodePos[0] );
                T yAbsVal = fabs( nodePos[1] );
                int greaterThan45Degs = ( xAbsVal < yAbsVal ) ? 1 : 0;
                if ( nodePos[0] > 0 ) {
                    if ( nodePos[1] > 0 )   currZone = 0 + greaterThan45Degs;   // 0-90 degrees
                    else                    currZone = 7 - greaterThan45Degs;   // 270-360 degrees
                }
                else {
                    if ( nodePos[1] > 0 )   currZone = 3 - greaterThan45Degs;   // 90-180 degrees
                    else                    currZone = 4 + greaterThan45Degs;   // 180-270 degrees
                }
                if ( zStartPt <= nodePos[2] && nodePos[2] <= zEndPt ) {
                    // Check that the xy distance does not over the shaft's radius of influence
                    if ( sqrt( xAbsVal*xAbsVal + yAbsVal*yAbsVal ) <= *pShaftRadiusOfInfluence ) {
                        if ( prevZone != currZone )
                        {
                            //++zoneCounts[currZone];

                            if ( currZone == 0 ) {
                                if ( prevZone == NUM_ZONES-1 ) {
                                    ++count; 
                                }
                                else {
                                    --count;
                                }
                            }
                            else if ( currZone == NUM_ZONES-1 ) {
                                if ( prevZone == 0 ) {
                                    --count;
                                }
                                else {
                                    ++count;
                                }
                            }
                            else if ( currZone > prevZone ) {
                                ++count;
                            }
                            else {//if ( currZone < prevZone ) {
                                --count;
                            }
                        }
                    }
                }
            }
            prevZone = currZone;
            ++it;
        }

        //int lowestVal = 1E3;
        //for ( int i = 0; i < NUM_ZONES; ++i ) {
        //  if ( lowestVal > zoneCounts[i] )    lowestVal = zoneCounts[i];
        //  //std::cout << "zone " << i << ": " << (int)zoneCounts[i] << "\n";
        //}
        //*pNumOfWrappingLoops = lowestVal;

        // Check the number of loops and direction based on the count value
        if ( count <= 0 )   *isCCW = true;
        else                *isCCW = false;
        T loops = static_cast<T>( abs(count) ) / NUM_ZONES;
        *pNumOfWrappingLoops = loops;

        // DEBUG -- by drawing
        d_NumOfWrappingLoops = static_cast<int>( *pNumOfWrappingLoops );
        d_ShaftSPt = trx * lineStartPt_world;
        d_ShaftEPt = trx * lineEndPt_world;
    }

    return result;
}

//-----------------------------------------------------------------------------
// Collision Detection and Response with a HETriMeshOneModelMultiParts object
template <typename T>
bool ElasticRodCD<T>::CDRwith (
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pObj,    
    TransformationSupport<T> * const pTransform             
)
{
    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;
}


//-----------------------------------------------------------------------------
#ifdef  TAPs_ADD_IMPLICIT_OBJECTS
//-----------------------------------------------------------------------------
// Collision Detection and Response with an implicit object
template <typename T>
bool ElasticRodCD<T>::CDRwithImplicitObject (
    ImplicitObject<T> * const pImpObj   
)
{
    bool bResult = false;

    #ifdef  TAPs_SHOW_COLLISION_STATE
        pImpObj->IsCollided = false;
    #endif//TAPs_SHOW_COLLISION_STATE

    switch ( pImpObj->GetShape() ) {
        case Enum::SPHERE:
            {
                bResult = GetBVHTree()->TestOverlapWithTillLeafNodes( pImpObj->GetBVHTree() );
                ImplicitObject_Sphere<T> * pObj = dynamic_cast<ImplicitObject_Sphere<T> *>( pImpObj );
                if ( bResult ) {
                    CollisionResponseWithImplicitObject_Sphere( pObj );
                }
            }
            break;
        case Enum::TORUS:
            {
                bResult = GetBVHTree()->TestOverlapWithTillLeafNodes( pImpObj->GetBVHTree() );
                ImplicitObject_Torus<T> * pObj = dynamic_cast<ImplicitObject_Torus<T> *>( pImpObj );
                if ( bResult ) {
                    CollisionResponseWithImplicitObject_Torus( pObj );
                }
            }
            break;
    }
    return bResult;
}
//-----------------------------------------------------------------------------
#endif//TAPs_ADD_IMPLICIT_OBJECTS
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// This function assumes no intersections between immediate adjacent links.
template <typename T>
void ElasticRodCD<T>::CDRforSelfIntersections ()
{
    CheckSelfIntersectionsNextLevelRecursively( m_BVHTree->Root()->Child(0), m_BVHTree->Root()->Child(1) );
}

//-----------------------------------------------------------------------------
// Build the BVH tree recursively (called by BuildBVHTree function)
template <typename T>
BVHTree<T> * ElasticRodCD<T>::BuildBVHTreeRecursively (
    int startID,                    
    BVHNode<T> ** childNodeList,    
    int numOfChildNodes             
)
{
    BVHNode<T> ** parentNodeList;
    if ( numOfChildNodes > 1 ) {
        int numOfParentNodes;
        if ( numOfChildNodes % 2 == 0 ) {
            numOfParentNodes = numOfChildNodes/2;
            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;
            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;
    }
}

//-----------------------------------------------------------------------------
// Update the BVH tree
template <typename T>
void ElasticRodCD<T>::UpdateBVHTree ()
{
    if ( m_BVHTree->Root() )    UpdateBVHNodeRecursively( m_BVHTree->Root() );
}

//-----------------------------------------------------------------------------
// Update the BVH nodes recursively
template <typename T>
void ElasticRodCD<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();
        BVHNodeLeafElasticRodNode<T> * pBVNode = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node );
        Vector3<T> const & pos1 = pBVNode->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition();
        Vector3<T> const & pos2 = pBVNode->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition();
        pBVNode->SetRadius( (pos2-pos1).Length()/2.0 + m_pParameters->Radius );
        pBVNode->SetCenter( (pos2+pos1) / 2.0 );
    }
    else {
        node->Update();
    }
}

//-----------------------------------------------------------------------------
// Check self intersection at next level recursively.
// Assume a BVH node either has both children or none.
template <typename T>
void ElasticRodCD<T>::CheckSelfIntersectionsNextLevelRecursively ( BVHNode<T> * node1, BVHNode<T> * node2 )
{
    CheckSelfIntersectionsRecursively( node1, node2 );
    if ( node1->IsNotLeaf() )   CheckSelfIntersectionsNextLevelRecursively( node1->Child(0), node1->Child(1) );
    if ( node2->IsNotLeaf() )   CheckSelfIntersectionsNextLevelRecursively( node2->Child(0), node2->Child(1) );
}

//-----------------------------------------------------------------------------
// Check self intersection recursively
template <typename T>
T ElasticRodCD<T>::CheckSelfIntersectionsRecursively ( BVHNode<T> * node1, BVHNode<T> * node2 )
{
    T LinkDiameter = 2.0 * m_pParameters->Radius + m_pParameters->Offset_CD_Self;
    T LinkStick = LinkDiameter * 1.1;

    T ScaledLinkDiameter = LinkDiameter * m_pParameters->KselfCDR_ScaleDistance;

    T scale = m_pParameters->KselfCDR / ( m_pParameters->TimeStep * m_pParameters->TimeStep );
    T scaleStick = m_pParameters->KselfStick / ( m_pParameters->TimeStep * m_pParameters->TimeStep );

    // Skip this test and return the highest value if a node is a leaf node or they are siblings.
    if ( node1->IsLeaf() && node2->IsLeaf() && abs(node1->GetID()-node2->GetID()) == 1 ) {
        return DBL_MAX;
    }

    // Test overlap between node1 and a group of nodes from node2 on
    T overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2 );
    if ( overlap < DBL_MIN ) {
        if ( node2->IsLeaf() ) {    // case: node2 is a leaf node
            if ( node1->IsNotLeaf() ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(0), node2 );
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(1), node2 );
            }
        }
        else {                      // case: node2 is not a leaf node
            overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2->Child(0) );
            if ( overlap < DBL_MIN ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(0), node2->Child(0) );
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(1), node2->Child(0) );
            }
            overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2->Child(1) );
            if ( overlap < DBL_MIN ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(0), node2->Child(1) );
                /*overlap =*/ CheckSelfIntersectionsRecursively( node1->Child(1), node2->Child(1) );
            }
        }
    }

    // Both nodes are leaf nodes, then do collision response
    if ( node1->IsLeaf() && node2->IsLeaf() ) {

        T distance;
        Vector3<T> U;
        BVHNodeLeafElasticRodNode<T> * Node1 = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node1 );
        BVHNodeLeafElasticRodNode<T> * Node2 = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node2 );

        CGMath<T>::FindClosestDistanceBetweenTwoLineSegments(
            Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition(), Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition(), 
            Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition(), Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition(), 
            distance, U
        );

        //
        // If collide
        //
        if ( distance < ScaledLinkDiameter ) {
            U *= distance * scale / 2.0;
            Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetForce( Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetForce() - U * Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass() );
            Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].SetForce( Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetForce() - U * Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetMass() );
            Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetForce( Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetForce() + U * Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass() );
            Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].SetForce( Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetForce() + U * Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetMass() );

            #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
            AddSelfCDExtraDataForALinkPair( node1->GetID(), node2->GetID() );
            #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION
        }
        // Make them stick together
        else if ( distance < LinkStick ) {
            U *= (LinkDiameter - distance) * scaleStick / 2.0;
            Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetForce( Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetForce() - U * Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass() );
            Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].SetForce( Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetForce() - U * Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetMass() );
            Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].SetForce( Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetForce() + U * Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetMass() );
            Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].SetForce( Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetForce() + U * Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetMass() );

            #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
            AddSelfCDExtraDataForALinkPair( node1->GetID(), node2->GetID() );
            #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION
        }
    }
    return overlap;
}




//-----------------------------------------------------------------------------
#ifdef  TAPs_ADD_IMPLICIT_OBJECTS
//-----------------------------------------------------------------------------
// Collision response with an implicit sphere
template <typename T>
void ElasticRodCD<T>::CollisionResponseWithImplicitObject_Sphere ( ImplicitObject_Sphere<T> * const pImpObj )
{
    #ifdef  TAPs_SHOW_COLLISION_STATE
        //pImpObj->IsCollided = false;
    #endif//TAPs_SHOW_COLLISION_STATE

    Vector3<T> sphere_center( pImpObj->GetTransformedCenter() );
    T sphere_radius = pImpObj->GetTransformedRadius();

    std::vector< BVHNode<T> * > & nodeERs       = GetBVHTree()->GetListOfCollidedNodes();
    T combinedRadius = m_pParameters->Radius + m_pParameters->Offset_CD_ImpObj_Sphere + sphere_radius;
    T distance, u;
    Vector3<T> Vd;
    BVHNodeLeafElasticRodNode<T> * nodeER;
    ElasticRodNode<T> * erNode_1;
    //ElasticRodNode<T> * erNode_2;

    T scale = m_pParameters->K_CDRwImpObj_Sphere / (m_pParameters->TimeStep * m_pParameters->TimeStep);

    // CDR suture's points against the implicit sphere
    for ( unsigned int i = 0; i < nodeERs.size(); ++i ) {
        nodeER = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( nodeERs[i] );
        erNode_1 = nodeER->GetPtrToPrimitive_1();
        //erNode_2 = nodeER->GetPtrToPrimitive_2();

        // Point 1
        Vd = erNode_1->Centerline[*m_pIdxCurr].GetPosition() - sphere_center;
        distance = Vd.Length();
        u = combinedRadius - distance;
        if ( u > 0 ) {
            //erNode_1->ExternalForce += Vd * ( u / distance ) * m_pParameters->K_CDRwImpObj_Sphere;
            erNode_1->ExternalForce += u/distance * scale * erNode_1->Centerline[*m_pIdxCurr].GetMass() * Vd;
            #ifdef  TAPs_SHOW_COLLISION_STATE
                pImpObj->IsCollided = true;
            #endif//TAPs_SHOW_COLLISION_STATE
        }
        // Make it sticky
        else if ( u > -m_pParameters->Radius ) {
            //Vector3<T> stickyDirection = Vd * (sphere_radius/distance);
            erNode_1->ExternalForce -= u/distance * scale * erNode_1->Centerline[*m_pIdxCurr].GetMass() * Vd;
            //erNode_1->ExternalForce = ( stickyDirection - Vd ) * 20;
            erNode_1->Centerline[*m_pIdxCurr].SetVelocity( 0, 0, 0 );
        }

        // SKIPPED to avoid duplicating the replusive force
        // Point 2
        //Vd = erNode_2->Centerline[*m_pIdxCurr].GetPosition() - sphere_center;
        //distance = Vd.Length();
        //u = combinedRadius - distance;
        //if ( u > 0 ) {
        //  erNode_2->ExternalForce += Vd * ( u / distance ) * m_pParameters->K_CDRwImpObj_Sphere;

        //  #ifdef  TAPs_SHOW_COLLISION_STATE
        //      pImpObj->IsCollided = true;
        //  #endif//TAPs_SHOW_COLLISION_STATE
        //}
    }
}

//-----------------------------------------------------------------------------
// Collision response with an implicit torus
template <typename T>
void ElasticRodCD<T>::CollisionResponseWithImplicitObject_Torus ( ImplicitObject_Torus<T> * const pImpObj )
{
    Vector3<T> torus_direction( pImpObj->GetTransformedAxis() );
    Vector3<T> torus_center( pImpObj->GetTransformedCenter() );
    T torus_inner_radius, torus_outer_radius;
    pImpObj->GetTransformedRadii( torus_inner_radius, torus_outer_radius );

    std::vector< BVHNode<T> * > & nodeERs   = GetBVHTree()->GetListOfCollidedNodes();
    T combinedRadius = m_pParameters->Radius + m_pParameters->Offset_CD_ImpObj_Torus + torus_inner_radius;
    T inner_radius = torus_outer_radius - torus_inner_radius;
    T outer_radius = torus_outer_radius + torus_inner_radius;
    T length, distance, u;
    Vector3<T> P, Vd, ProjPt, MedianAxisPt, Force;
    BVHNodeLeafElasticRodNode<T> * nodeER;
    ElasticRodNode<T> * erNode_1;
    //ElasticRodNode<T> * erNode_2;

    #ifdef  TAPs_SHOW_COLLISION_STATE
        //pImpObj->IsCollided = false;
    #endif//TAPs_SHOW_COLLISION_STATE

    T scale = m_pParameters->K_CDRwImpObj_Torus / (m_pParameters->TimeStep * m_pParameters->TimeStep);

    // CDR suture's points against the implicit torus
    for ( unsigned int i = 0; i < nodeERs.size(); ++i ) {

        nodeER = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( nodeERs[i] );
        erNode_1 = nodeER->GetPtrToPrimitive_1();
        //erNode_2 = nodeER->GetPtrToPrimitive_2();

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

        // STEP:
        // 1) Project the test point on to the torus plane.
        // 2) Use the project point to find the point on the torus median axis.
        // 3) Find the distance from the test point to the median axis point.
        // 4) If the distance is less than the combined radius, then push the test point out of it.

        // Point 1
        P = erNode_1->Centerline[*m_pIdxCurr].GetPosition();
        CGMath<T>::FindProjectedPointOnAPlane( P, torus_center, torus_direction, ProjPt );
        MedianAxisPt = (ProjPt - torus_center);
        length = MedianAxisPt.Length();
        //if ( length > Math<T>::ZERO  ) {
        if ( inner_radius <= length && length <= outer_radius ) {
            MedianAxisPt *= torus_outer_radius / length;
            MedianAxisPt += torus_center;
            Vd = P - MedianAxisPt;
            distance = Vd.Length();
            u = combinedRadius - distance;
            if ( u > 0 ) {
                //erNode_1->ExternalForce += Vd * ( u / distance ) * m_pParameters->K_CDRwImpObj_Torus;
                erNode_1->ExternalForce += u/distance * scale * erNode_1->Centerline[*m_pIdxCurr].GetMass() * Vd;

                #ifdef  TAPs_SHOW_COLLISION_STATE
                    pImpObj->IsCollided = true;
                #endif//TAPs_SHOW_COLLISION_STATE
            }
            // Make it sticky
            else if ( u > -m_pParameters->Radius ) {
                //Vector3<T> stickyDirection = Vd * (torus_inner_radius/distance);
                erNode_1->ExternalForce -= u/distance * scale * erNode_1->Centerline[*m_pIdxCurr].GetMass() * Vd;
                //erNode_1->ExternalForce = ( stickyDirection - Vd ) * 20;
                erNode_1->Centerline[*m_pIdxCurr].SetVelocity( 0, 0, 0 );
            }
        }

        // SKIPPED to avoid duplicating the replusive force
        // Point 2
        //P = erNode_2->Centerline[*m_pIdxCurr].GetPosition();
        //CGMath<T>::FindProjectedPointOnAPlane( P, torus_center, torus_direction, ProjPt );
        //MedianAxisPt = (ProjPt - torus_center);
        //length = MedianAxisPt.Length();
        //if ( length > Math<T>::ZERO ) {
        //  MedianAxisPt *= torus_outer_radius / length;
        //  MedianAxisPt += torus_center;
        //  Vd = P - MedianAxisPt;
        //  distance = Vd.Length();
        //  u = combinedRadius - distance;
        //  if ( u > 0 ) {
        //      erNode_2->ExternalForce += Vd * ( u / distance ) * m_pParameters->K_CDRwImpObj_Torus;
        //      #ifdef  TAPs_SHOW_COLLISION_STATE
        //          pImpObj->IsCollided = true;
        //      #endif//TAPs_SHOW_COLLISION_STATE
        //  }
        //}
    }
}
//-----------------------------------------------------------------------------
#endif//TAPs_ADD_IMPLICIT_OBJECTS
//-----------------------------------------------------------------------------




//-----------------------------------------------------------------------------
#ifdef  TAPs_SIM_WITH_POSITION_BASED_DYNAMICS
//-----------------------------------------------------------------------------
// This function assumes no intersections between immediate adjacent links.
template <typename T>
void ElasticRodCD<T>::CDRforSelfIntersections_PBD ()
{
    CheckSelfIntersectionsNextLevelRecursively_PBD( m_BVHTree->Root()->Child(0), m_BVHTree->Root()->Child(1) );
}
//-----------------------------------------------------------------------------
// Update the BVH tree
template <typename T>
void ElasticRodCD<T>::UpdateBVHTree_PBD ()
{
    if ( m_BVHTree->Root() )    UpdateBVHNodeRecursively_PBD( m_BVHTree->Root() );
}
//-----------------------------------------------------------------------------
// Update the BVH nodes recursively
template <typename T>
void ElasticRodCD<T>::UpdateBVHNodeRecursively_PBD ( BVHNode<T> * node )
{
    if ( node->Child(0) ) {
        UpdateBVHNodeRecursively_PBD( node->Child(0) );
    }
    if ( node->Child(1) ) {
        UpdateBVHNodeRecursively_PBD( node->Child(1) );
    }
    //----------------------------------------------------------------
    if ( node->Child(0) == NULL && node->Child(1) == NULL ) {
        int id = node->GetID();
        BVHNodeLeafElasticRodNode<T> * pBVNode = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node );
        Vector3<T> const & pos1 = pBVNode->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].GetPosition();
        Vector3<T> const & pos2 = pBVNode->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].GetPosition();
        pBVNode->SetRadius( (pos2-pos1).Length()/2.0 + m_pParameters->Radius );
        pBVNode->SetCenter( (pos2+pos1) / 2.0 );
    }
    else {
        node->Update();
    }
}
//-----------------------------------------------------------------------------
// Check self intersection at next level recursively.
// Assume a BVH node either has both children or none.
template <typename T>
void ElasticRodCD<T>::CheckSelfIntersectionsNextLevelRecursively_PBD ( BVHNode<T> * node1, BVHNode<T> * node2 )
{
    CheckSelfIntersectionsRecursively_PBD( node1, node2 );
    if ( node1->IsNotLeaf() )   CheckSelfIntersectionsNextLevelRecursively_PBD( node1->Child(0), node1->Child(1) );
    if ( node2->IsNotLeaf() )   CheckSelfIntersectionsNextLevelRecursively_PBD( node2->Child(0), node2->Child(1) );
}
//-----------------------------------------------------------------------------
// Check self intersection recursively
template <typename T>
T ElasticRodCD<T>::CheckSelfIntersectionsRecursively_PBD ( BVHNode<T> * node1, BVHNode<T> * node2 )
{
    T LinkDiameter = 2.0 * m_pParameters->Radius + m_pParameters->Offset_CD_Self;
    T ScaledLinkDiameter = LinkDiameter * m_pParameters->KselfCDR_ScaleDistance;

    // Skip this test and return the highest value if a node is a leaf node or they are siblings.
    if ( node1->IsLeaf() && node2->IsLeaf() && abs(node1->GetID()-node2->GetID()) == 1 ) {
        return DBL_MAX;
    }

    // Test overlap between node1 and a group of nodes from node2 on
    T overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2 );
    if ( overlap < DBL_MIN ) {
        if ( node2->IsLeaf() ) {    // case: node2 is a leaf node
            if ( node1->IsNotLeaf() ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(0), node2 );
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(1), node2 );
            }
        }
        else {                      // case: node2 is not a leaf node
            overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2->Child(0) );
            if ( overlap < DBL_MIN ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(0), node2->Child(0) );
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(1), node2->Child(0) );
            }
            overlap = node1->TestOverlapSphereWithSphere_NoPrimitiveTests( node2->Child(1) );
            if ( overlap < DBL_MIN ) {
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(0), node2->Child(1) );
                /*overlap =*/ CheckSelfIntersectionsRecursively_PBD( node1->Child(1), node2->Child(1) );
            }
        }
    }

    // Both nodes are leaf nodes, then do collision response
    if ( node1->IsLeaf() && node2->IsLeaf() ) {
        T distance;
        Vector3<T> U;
        //int i = node1->GetID();
        //int j = node2->GetID();
        BVHNodeLeafElasticRodNode<T> * Node1 = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node1 );
        BVHNodeLeafElasticRodNode<T> * Node2 = dynamic_cast<BVHNodeLeafElasticRodNode<T> *>( node2 );

        CGMath<T>::FindClosestDistanceBetweenTwoLineSegments(
            Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].GetPosition(), Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].GetPosition(), 
            Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].GetPosition(), Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].GetPosition(), 
            distance, U
        );
        // If collide
        if ( distance < ScaledLinkDiameter ) {
            U *= distance * m_pParameters->KselfCDR;
            CollisionResponseByUpdatingGeometric_PBD( U, Node1, Node2 );
        }
    }
    return overlap;
}
//-----------------------------------------------------------------------------
// Collision response by geometrically updating the positions of collided segments
template <typename T>
void ElasticRodCD<T>::CollisionResponseByUpdatingGeometric_PBD (
    Vector3<T> & displacementVector,        
    BVHNodeLeafElasticRodNode<T> * Node1,   
    BVHNodeLeafElasticRodNode<T> * Node2    
)
{
    //std::cout << "Node1: " << Node1 << "\n";
    //std::cout << "Node2: " << Node2 << "\n";
    Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].SetPosition( Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].GetPosition() - displacementVector );
    Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].SetPosition( Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].GetPosition() - displacementVector );
    Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].SetPosition( Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxNext].GetPosition() + displacementVector );
    Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].SetPosition( Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxNext].GetPosition() + displacementVector );
}
//-----------------------------------------------------------------------------
#endif//TAPs_SIM_WITH_POSITION_BASED_DYNAMICS
//-----------------------------------------------------------------------------




//=============================================================================
#ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodCD<T>::ResetSelfCDExtraData ()
{
    for ( int i = 0; i < m_pParameters->NumOfNodes-1; ++i ) {
        for ( int j = 0; j < m_pParameters->NumOfNodes-1; ++j ) {
            SelfCDExtraDataTrackStatus[i][j] = false;
        }
    }
    ListOfSelfCDExtraData.clear();
}
//-----------------------------------------------------------------------------
template <typename T>
bool ElasticRodCD<T>::InitSelfCDExtraData ()
{
    SelfCDExtraDataTrackStatus = new bool*[m_pParameters->NumOfNodes-1];
    if ( !SelfCDExtraDataTrackStatus ) return false;

    for ( int i = 0; i < m_pParameters->NumOfNodes-1; ++i ) {
        SelfCDExtraDataTrackStatus[i] = new bool[m_pParameters->NumOfNodes-1];
        if ( !SelfCDExtraDataTrackStatus[i] ) {
            return false;
        }
    }
    return true;
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodCD<T>::DeleteSelfCDExtraData ()
{
    for ( int i = 0; i < m_pParameters->NumOfNodes-1; ++i ) {
        delete [] SelfCDExtraDataTrackStatus[i];
    }
    delete [] SelfCDExtraDataTrackStatus;
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodCD<T>::AddSelfCDExtraDataForALinkPair ( unsigned int id1, unsigned int id2 )
{
    unsigned int diff = id1 - id2;
    if ( diff > 1 || diff < -1 ) {
        if ( SelfCDExtraDataTrackStatus[id1][id2] == false ) {
            SelfCDExtraDataTrackStatus[id1][id2] = true;
            SelfCDExtraData<T> data( id1, id2, m_pParameters->Radius );
            data.LinkCenter_A = ( (*m_pNodeList)[data.LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition() + (*m_pNodeList)[data.LinkID_A].Centerline[*m_pIdxCurr].GetPosition() ) * T(0.5);
            data.LinkCenter_B = ( (*m_pNodeList)[data.LinkID_B+1].Centerline[*m_pIdxCurr].GetPosition() + (*m_pNodeList)[data.LinkID_B].Centerline[*m_pIdxCurr].GetPosition() ) * T(0.5);
            data.AverageCenter = ( data.LinkCenter_A + data.LinkCenter_B ) * T(0.5);
            ListOfSelfCDExtraData.push_back( data );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodCD<T>::UpdateSelfCDExtraData ()
{
    std::list< SelfCDExtraData<T> >::iterator it = ListOfSelfCDExtraData.begin();
    std::list< SelfCDExtraData<T> >::iterator it_erase;
    while ( it != ListOfSelfCDExtraData.end() ) {
        it->LinkCenter_A = ( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition() + (*m_pNodeList)[it->LinkID_A].Centerline[*m_pIdxCurr].GetPosition() ) * T(0.5);
        it->LinkCenter_B = ( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxCurr].GetPosition() + (*m_pNodeList)[it->LinkID_B].Centerline[*m_pIdxCurr].GetPosition() ) * T(0.5);
        it->AverageCenter = ( it->LinkCenter_A + it->LinkCenter_B ) * T(0.5);

        T separateThreshold = it->AverageRadius * 20;
        T distance = ( it->LinkCenter_A - it->LinkCenter_B ).Length();

        //std::cout << "separateThreshold: " << separateThreshold << "\n";
        //std::cout << "distance: " << distance << "\n";

        if ( distance > separateThreshold ) {
            it_erase = it;
            ++it;
            ListOfSelfCDExtraData.erase( it_erase );
        }
        else {
            ++it;
        }
    }
}
//template <typename T>
//void ElasticRodCD<T>::PreventMissedCDWithSelfCDExtraData ()
//{
//  bool bRestore = false;
//
//  std::list< SelfCDExtraData<T> >::iterator it = ListOfSelfCDExtraData.begin();
//  while ( it != ListOfSelfCDExtraData.end() ) {
//      Vector3<T> NewLinkCenter_A = ( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].GetPosition() + (*m_pNodeList)[it->LinkID_A].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
//      Vector3<T> NewLinkCenter_B = ( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].GetPosition() + (*m_pNodeList)[it->LinkID_B].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
//      Vector3<T> NewAverageCenter = ( it->LinkCenter_A + it->LinkCenter_B ) * T(0.5);
//
//      Vector3<T> V0 = it->LinkCenter_A - it->AverageCenter;
//      Vector3<T> V1 = NewLinkCenter_A - NewAverageCenter;
//
//      T angle = V0.Normalized() * V1.Normalized();
//
//      std::cout << "angle: " << angle << "\n";
//
//      // If angle is greater than 90 degrees, then restore the previous position
//      if ( angle < T(0) ) {
//          bRestore = true;
//          //std::cout << "New value: " << (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition();
//          //std::cout << "OLD value: " << (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].GetPosition();
//
//          //(*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition() );
//          //(*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxCurr].GetPosition() );
//          //(*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxCurr].GetPosition() );
//          //(*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxCurr].GetPosition() );
//      }
//
//      ++it;
//  }
//
//  if ( bRestore ) {
//
//      //std::list< SelfCDExtraData<T> >::iterator it = ListOfSelfCDExtraData.begin();
//      //while ( it != ListOfSelfCDExtraData.end() ) {
//      //  (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition() );
//      //  (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxCurr].GetPosition() );
//      //  (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxCurr].GetPosition() );
//      //  (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxCurr].GetPosition() );
//      //  ++it;
//      //}
//
//      //for ( unsigned int i = 0; i < (*m_pNodeList).size(); ++i ) {
//      //  (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition() );
//      //}
//  }
//}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodCD<T>::CheckAndRestoreWithSelfCDExtraData ( unsigned int nodeID )
{
    std::list< SelfCDExtraData<T> >::iterator it = ListOfSelfCDExtraData.begin();
    while ( it != ListOfSelfCDExtraData.end() ) {

        if ( 
            it->LinkID_A == nodeID || it->LinkID_B == nodeID
            ||
            it->LinkID_A+1 == nodeID || it->LinkID_B+1 == nodeID
        )
        {
            Vector3<T> NewLinkCenter_A = ( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].GetPosition() + (*m_pNodeList)[it->LinkID_A].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
            Vector3<T> NewLinkCenter_B = ( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].GetPosition() + (*m_pNodeList)[it->LinkID_B].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
            Vector3<T> NewAverageCenter = ( it->LinkCenter_A + it->LinkCenter_B ) * T(0.5);

            Vector3<T> V0 = it->LinkCenter_A - it->AverageCenter;
            Vector3<T> V1 = NewLinkCenter_A - NewAverageCenter;

            T angle = V0.Normalized() * V1.Normalized();

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

            //T angleThs = T(0);                // 90 degrees
            //T angleThs = T(0.258819045);  // 75 degrees
            //T angleThs = T(0.5);          // 60 degrees
            T angleThs = T(0.707106781);    // 45 degrees
            //T angleThs = T(0.866025403);  // 30 degrees
            //T angleThs = T(0.965925826);  // 15 degrees
            unsigned int offset = 4;        // number of extra points on each direction

            if ( angle < angleThs ) {
                //std::cout << "New value: " << (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition();
                //std::cout << "OLD value: " << (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].GetPosition();

                (*m_pNodeList)[it->LinkID_A  ].Orientation[*m_pIdxNext] = (*m_pNodeList)[it->LinkID_A  ].Orientation[*m_pIdxCurr];
                (*m_pNodeList)[it->LinkID_A+1].Orientation[*m_pIdxNext] = (*m_pNodeList)[it->LinkID_A+1].Orientation[*m_pIdxCurr];
                (*m_pNodeList)[it->LinkID_B  ].Orientation[*m_pIdxNext] = (*m_pNodeList)[it->LinkID_B  ].Orientation[*m_pIdxCurr];
                (*m_pNodeList)[it->LinkID_B+1].Orientation[*m_pIdxNext] = (*m_pNodeList)[it->LinkID_B+1].Orientation[*m_pIdxCurr];

                (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxCurr].GetPosition() );
                (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxCurr].GetPosition() );
                (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxCurr].GetPosition() );
                (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxCurr].GetPosition() );

                (*m_pNodeList)[it->LinkID_A  ].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                (*m_pNodeList)[it->LinkID_A+1].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                (*m_pNodeList)[it->LinkID_B  ].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                (*m_pNodeList)[it->LinkID_B+1].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );

                // A
                for ( unsigned int i = it->LinkID_A-offset; i < it->LinkID_A; ++i ) {
                    if ( i >= 0 ) {
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition() );
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                        (*m_pNodeList)[i].Orientation[*m_pIdxNext] = (*m_pNodeList)[i].Orientation[*m_pIdxCurr];
                    }
                }
                for ( unsigned int i = it->LinkID_A+2; i <= it->LinkID_A+offset; ++i ) {
                    if ( i < static_cast<unsigned int>( m_pParameters->NumOfNodes ) ) {
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition() );
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                        (*m_pNodeList)[i].Orientation[*m_pIdxNext] = (*m_pNodeList)[i].Orientation[*m_pIdxCurr];
                    }
                }
                // B
                for ( unsigned int i = it->LinkID_B-offset; i < it->LinkID_B; ++i ) {
                    if ( i >= 0 ) {
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition() );
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                        (*m_pNodeList)[i].Orientation[*m_pIdxNext] = (*m_pNodeList)[i].Orientation[*m_pIdxCurr];
                    }
                }
                for ( unsigned int i = it->LinkID_B+2; i <= it->LinkID_B+offset; ++i ) {
                    if ( i < static_cast<unsigned int>( m_pParameters->NumOfNodes ) ) {
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition() );
                        (*m_pNodeList)[i].Centerline[*m_pIdxNext].SetVelocity( 0, 0, 0 );
                        (*m_pNodeList)[i].Orientation[*m_pIdxNext] = (*m_pNodeList)[i].Orientation[*m_pIdxCurr];
                    }
                }

            }
        }
        ++it;
    }
}
//-----------------------------------------------------------------------------
#endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION
//=============================================================================




//-----------------------------------------------------------------------------

#if defined(__gl_h_) || defined(__GL_H__)
//=============================================================================
template <typename T>
void ElasticRodCD<T>::Draw () const
{
    if ( m_BVHTree ) {
        m_BVHTree->Draw();
    }

    /*
    // DEBUG -- Draw tool shaft line
    glPushMatrix();
    glTranslatef( 0, 1, -1 );

    glPushAttrib( GL_ALL_ATTRIB_BITS );
    //glDisable( GL_DEPTH_TEST );
    glDisable( GL_LIGHTING );
    glLineWidth( 5 );
    glBegin( GL_LINES );
    glColor3f( 1, 0, 0 );
    glVertex3fv( d_ShaftSPt.GetDataFloat() );
    glColor3f( 0, 1, 0 );
    glVertex3fv( d_ShaftEPt.GetDataFloat() );
    glEnd();

    // Draw number of wrapping loops as a set of points
    glPointSize( 10 );
    glColor3f( 0, 1, 1 );
    glBegin( GL_POINTS );
    glColor3f( 0.5, 0.5, 0.5 );
    glVertex3f( -0.5, 0.0, 0 );
    glColor3f( 0, 1, 1 );
    for ( int i = 0; i < d_NumOfWrappingLoops; ++i ) {
        glVertex3f( i*0.5, 0.0, 0 );
    }
    glEnd();

    // Draw node positions
    glPointSize( 3 );
    glColor3f( 0, 0, 1 );
    glBegin( GL_LINE_STRIP );
    for ( std::vector< Vector3<T> >::const_iterator it = d_NodePositions.begin(); it != d_NodePositions.end(); ++it ) {
        glVertex3fv( it->GetDataFloat() );
    }
    glEnd();

    glPopAttrib();
    glPopMatrix();
    //*/
}

#ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
    template <typename T>
    void ElasticRodCD<T>::DrawExtraSelfCDData () const
    {
        std::list< SelfCDExtraData<T> >::const_iterator it = ListOfSelfCDExtraData.begin();
        while ( it != ListOfSelfCDExtraData.end() ) {
            it->Draw();
            ++it;
        }
    }
#endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

//=============================================================================
#endif // #if defined(__gl_h_) || defined(__GL_H__)

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