TAPsElasticRodKR.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsElasticRodKR.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (08/31/2009)
UPDATE          (03/23/2011)
******************************************************************************/
#include "TAPsElasticRodKR.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>
ElasticRodKR<T>::ElasticRodKR ()
    : m_pIdxCurr( 0 )
    , m_pIdxNext( 0 )
    , m_pParameters( NULL )
    , m_pNodeList( NULL )
    , m_BVHTree( NULL )
    
    , m_pListOfLockedKnots( NULL )
    , m_iMinPtIdThatIsInRecordedKnots( -1 )
    , m_iMaxPtIdThatIsInRecordedKnots( -1 )

    , m_ucMaxNumKnotsAllowed( 0 )
    , m_ucKnotCount( 0 )
    , m_uiCounterThreshold( 0 )
    , m_uiCounterForFixingKnot( 0 )
    , m_tWaitTime( 0 )
    , m_puiLengthThresholds( NULL )
{
    m_pOGLUsefulObj = new OpenGL::OpenGLUsefulObj<T>( GLU_FILL, 8, 8, Vector4<T>( 0, 1, 0, 1 ) );
}

// Destructor
template <typename T>
ElasticRodKR<T>::~ElasticRodKR ()
{
    if ( m_pOGLUsefulObj )  delete m_pOGLUsefulObj;
    DeleteKR();
}

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

// Create collision detection process
template <typename T>
void ElasticRodKR<T>::CreateKR (
    int *                       pIdxCurr,       
    int *                       pIdxNext,       
    ElasticRodParameters<T> *   pParameters,    
    SetOfElasticRodNodes *      pNodeList,      
    BVHTree<T> *    bvhTree,            
    unsigned char   numOfNotAllowed     
)
{
    m_pIdxCurr    = pIdxCurr;
    m_pIdxNext    = pIdxNext;
    m_pParameters = pParameters;
    m_pNodeList   = pNodeList;
    m_BVHTree = bvhTree;

    // Initialize this KR
    Init( numOfNotAllowed );
}

// Delete collision detection process
template <typename T>
void ElasticRodKR<T>::DeleteKR ()
{
    DeleteData();
}

// Build the BVH tree for the elastic rod
//template <typename T>
//void ElasticRodKR<T>::BuildBVHTree ()
//{}


// Find projection direction from the set of three points
template <typename T>
Vector3<T> ElasticRodKR<T>::GetProjDirection () const
{
    return TAPs::CGMath<T>::NormalOfTriangle(
                m_NodesForProjPoints[0]->Centerline[*m_pIdxCurr].GetPosition(),
                m_NodesForProjPoints[1]->Centerline[*m_pIdxCurr].GetPosition(),
                m_NodesForProjPoints[2]->Centerline[*m_pIdxCurr].GetPosition()
            );
}


// Create the knot control's data
template <typename T>
void ElasticRodKR<T>::CreateData ( unsigned char numKnotAllowed )
{
    // Set the three points for finding projection direction
    int i = 1;
    SetOfElasticRodNodes::iterator node = m_pNodeList->begin();
    m_NodesForProjPoints[0] = &(*node);     // the fist point
    for ( ; i < m_pParameters->NumOfNodes/2; ++i )  ++node;
    m_NodesForProjPoints[1] = &(*node);     // the middle point
    for ( ; i < m_pParameters->NumOfNodes; ++i )    ++node;
    m_NodesForProjPoints[2] = &(*node);     // the last point

    m_iKnotStartPt = m_iKnotEndPt = -1; 
    m_iMaxCrossingPairs = (m_pParameters->NumOfNodes-1)/2;
    m_iNumCrossingPairs = 0;
    m_iaCrossingCounters = new int[ m_pParameters->NumOfNodes-1 ];
    m_iaDowkerNotation = new int[ m_iMaxCrossingPairs ];

    m_ucMaxNumKnotsAllowed = numKnotAllowed;
    m_pListOfLockedKnots = new IC_LockedKnot[ m_ucMaxNumKnotsAllowed ];
    m_puiLengthThresholds = new unsigned int[ m_ucMaxNumKnotsAllowed ];
    {
        m_tWaitTime = 2.0;//5.0;                // set wait time
        m_uiCounterThreshold = 10;//25;     // act as a timer for fixing the current knot
        m_uiMinLengthThreshold = 0;     // start at zero
        m_puiLengthThresholds[0] = 20;//10; // length threshold for half knot -- basic knot
                                        // extra length threshold for double knot will be added by m_puiLengthThresholds[0]/2
        m_puiLengthThresholds[1] = 12;//8;  // extra length threshold for double knot
        // i.e. for double knot the length threshold is 18 (of half knot) + 8

        // The rest of the threshold lengths
        for ( int i = 2; i < m_ucMaxNumKnotsAllowed; ++i ) {
            m_puiLengthThresholds[i] =  8;
        }
    }

    Reset();
}


// Delete the knot control's data
template <typename T>
void ElasticRodKR<T>::DeleteData ()
{
    if ( m_iaCrossingCounters ) {
        delete [] m_iaCrossingCounters;
        m_iaCrossingCounters = NULL;
    }
    if ( m_iaDowkerNotation ) {
        delete [] m_iaDowkerNotation;
        m_iaDowkerNotation = NULL;
    }

    if ( m_pListOfLockedKnots ) {
        delete [] m_pListOfLockedKnots;
        m_pListOfLockedKnots = NULL;
    }

    if ( m_puiLengthThresholds ) {
        delete [] m_puiLengthThresholds;
        m_puiLengthThresholds = NULL;
    }
}


// Initialize this knot control
template <typename T>
void ElasticRodKR<T>::Init ( unsigned char numKnotAllowed )
{
    DeleteData();
    CreateData( numKnotAllowed );
}


// Reset this knot control
template <typename T>
void ElasticRodKR<T>::Reset ()
{
    m_ucKnotCount = 0;
    for ( int i = 0; i < m_ucMaxNumKnotsAllowed; ++i ) {
        m_pListOfLockedKnots[i].Reset();

    }
    m_uiCounterForFixingKnot = 0;   // reset counter
    m_uiMinLengthThreshold = 0;     // reset min length threshold
    ResetElapsedTime();
    m_uiLengthAccumulateTarget = m_puiLengthThresholds[0];      // reset accumulated target length
    m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED; // reset tracker

    #ifdef  TAPs_ADD_ANIMATED_KNOTS
    m_ListOfAnimatedKnot.clear();
    //DoneAnimatingKnot();
    #endif//TAPs_ADD_ANIMATED_KNOTS

    m_iMinPtIdThatIsInRecordedKnots = -1;
    m_iMaxPtIdThatIsInRecordedKnots = -1;
}


template <typename T>
void ElasticRodKR<T>::GetCurrentTrackingKnot_StartAndEndLinkIDs (
    int & startLink,    
    int & endLink       
)
{
    if ( m_ucTrackingKnot == Data::DatabaseDowkerNotation::UNDEFINED ) {
        startLink = endLink = -1;
    }
    else {
        startLink = m_KnotLinkStartTempRecord;
        endLink = m_KnotLinkEndTempRecord;
    }
}


template <typename T>
int ElasticRodKR<T>::ConverseDowkerNotationToKnotName ( std::string * knotName )
{
    // Find the knot that match with the Dowker notation
    int knotID = Data::DatabaseDowkerNotation::FindKnotOfDowkerNotation( m_iNumCrossingPairs, m_iaDowkerNotation, knotName );

    // Include the Dowker notation in the return string
    if ( knotName ) {
        if ( knotID >= 0 ) {
            *knotName = Data::DatabaseDowkerNotation::GetStrEntry( knotID );
        }
        else {
            *knotName = "n/a";
        }
    }
    return knotID;  // return -1 if not found, otherwise return the knotID
}


// Check if a part of the elastic rod forms the surgeon knot
template <typename T>
std::string ElasticRodKR<T>::CheckSurgeonsKnot ()
{
    std::string str = "";
    static std::string name;

    //
    // DEBUG -- For checking knot lock thresholds
    //
    //std::stringstream ss;
    //ss << "Knot_Fixing_Threshold (" << m_uiCounterThreshold << "): " << m_uiCounterForFixingKnot;
    //ss << "  Knot_Waiting_Time_Threshold (" << m_tWaitTime << "): " << m_tAccTime << "  ";
    //str += ss.str();

    // Recognize the knot
    //*
    if ( m_ucTrackingKnot == Data::DatabaseDowkerNotation::UNDEFINED ) {

        CheckSelfCrossingForKnotRecognition( GetProjDirection() );
        int start = GetKnotStartPt();
        int end = GetKnotEndPt();

        // Check if a new knot is being formed
        m_ucTrackingKnot = ConverseDowkerNotationToKnotName( &name );

        // Initially record the knot length (start and end)
        if ( m_ucTrackingKnot != Data::DatabaseDowkerNotation::UNDEFINED ) {
            m_KnotLinkStartTempRecord = start;
            m_KnotLinkEndTempRecord   = end;
        }

        // Recognize the knot -- Total Knots is One
        if ( m_ucKnotCount == 1 ) {
            str += GetNameOfKnot( 0, false );   // 2nd parameter: true with (-)/(+) for direction
            str += " Knot";
            //str = "One Knot";
        }

        // Recognize the knot -- Total Knots is Two
        else if ( m_ucKnotCount == 2 ) {
            //-----------------------------------
            // Check Surgeon's Knot (NEG-then-POS or POS-then-NEG)
            if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG )
            ) {
                str += "Surgeon's Knot";
            }
            //-----------------------------------
            // Check Surgeon's Knot (NEG-then-NEG or POS-then-POS)
            else if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Granny Double Knot";
            }
            //-----------------------------------
            // Check Square Knot (NEG-then-POS or POS-then-NEG)
            else if ( 
                    // neg half then pos half equals square knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS )
                ||
                    // pos half then neg half equals square knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG )
            ) {
                str += "Square Knot";
            }
            //-----------------------------------
            // Check Square Knot (NEG-then-NEG or POS-then-POS)
            else if ( 
                    // neg half then pos half equals square knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos half then neg half equals square knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Granny Knot";
            }
            //-----------------------------------
            // Unrecognized Knot Combination
            else {
                char s[16];
                str += _itoa( m_ucKnotCount, s, 10 );
                str += " knot combination: ";

                //*
                #ifdef  TAPs_STRAND_DEBUG
                for ( int i = 0; i < m_ucKnotCount; ++i ) {
                    str += GetNameOfKnot( i, true );    // 2nd parameter: true with (-)/(+) for direction
                    if ( i < m_ucKnotCount-1 ) str += ", ";
                }
                #endif//TAPs_STRAND_DEBUG
                //*/
                str += GetStrNameForKnotCombination();
            }
        }

        // Recognize the knot -- Total Knots is Three
        else if ( m_ucKnotCount == 3 ) {
            //-----------------------------------
            // Check Surgeon's Knot (NEG-then-POS-then-NEG or POS-then-NEG-then-POS)
            if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Square Surgeon's Knot";
            }
            //-----------------------------------
            // Check Surgeon's Knot (NEG-then-NEG-then-NEG or POS-then-POS-then-POS)
            else if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::DOUBLE_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Granny Granny Double Knot";
            }
            //-----------------------------------
            // Check Square's Knot (NEG-then-POS-then-NEG or POS-then-NEG-then-POS)
            else if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Square Square Knot";
            }
            //-----------------------------------
            // Check Square's Knot (NEG-then-NEG-then-NEG or POS-then-POS-then-POS)
            else if (
                    // neg double then pos half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_NEG
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_NEG 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_NEG )
                ||
                    // pos double then neg half equals surgeon's knot
                    (    m_pListOfLockedKnots[0].ID == Data::DatabaseDowkerNotation::HALF_POS
                      && m_pListOfLockedKnots[1].ID == Data::DatabaseDowkerNotation::HALF_POS 
                      && m_pListOfLockedKnots[2].ID == Data::DatabaseDowkerNotation::HALF_POS )
            ) {
                str += "Granny Granny Knot";
            }
            //-----------------------------------
            // Unrecognized Knot Combination
            else {
                char s[16];
                str += _itoa( m_ucKnotCount, s, 10 );
                str += " knot combination: ";

                //*
                #ifdef  TAPs_STRAND_DEBUG
                for ( int i = 0; i < m_ucKnotCount; ++i ) {
                    str += GetNameOfKnot( i, true );    // 2nd parameter: true with (-)/(+) for direction
                    if ( i < m_ucKnotCount-1 ) str += ", ";
                }
                #endif//TAPs_STRAND_DEBUG
                //*/
                str += GetStrNameForKnotCombination();
            }
        }

        // Recognize the knot -- Total Knots is greater than three
        else {
            char s[16];
            str += _itoa( m_ucKnotCount, s, 10 );
            str += " knot combination: ";

            //*
            #ifdef  TAPs_STRAND_DEBUG
            for ( int i = 0; i < m_ucKnotCount; ++i ) {
                str += GetNameOfKnot( i, true );    // 2nd parameter: true with (-)/(+) for direction
                if ( i < m_ucKnotCount-1 ) str += ", ";
            }
            #endif//TAPs_STRAND_DEBUG
            //*/
            str += GetStrNameForKnotCombination();
        }
    }
    //*/

    // Track the current knot
    else {
        CheckSelfCrossingForKnotRecognition( GetProjDirection() );
        int start = GetKnotStartPt();
        int end = GetKnotEndPt();

        // Keep the new knot length
        if ( start > 0 || end > 0 ) {
            m_KnotLinkStartTempRecord = start;
            m_KnotLinkEndTempRecord   = end;
        }

        //-------------------------------------------------
        // Change tracking knot -- for a more complex knot

        // Current tracked knot is a half knot
        if (    m_ucTrackingKnot == Data::DatabaseDowkerNotation::HALF_NEG
             || m_ucTrackingKnot == Data::DatabaseDowkerNotation::HALF_POS )
        {
            // Check the current knot
            std::string name2;
            int knot = ConverseDowkerNotationToKnotName( &name2 );

            // The current knot is not a half knot
            if ( knot != Data::DatabaseDowkerNotation::UNDEFINED ) {
                // REMARK:
                // This change allows the knot to switch freely between 
                // HALF_NEG and HALF_POS due to the result knot id from 
                // Dowker notation.
                // This change is not allowed here.
                // So that the HALF knot (direction) will not be switched!
                //if (   knot == Data::DatabaseDowkerNotation::HALF_NEG
                //    || knot == Data::DatabaseDowkerNotation::HALF_POS )
                //{ 
                //  // Change from half knot to half knot
                //  str = name = name2;
                //  m_ucTrackingKnot = knot;
                //}
                //else 

                if (  knot == Data::DatabaseDowkerNotation::DOUBLE_NEG
                   || knot == Data::DatabaseDowkerNotation::DOUBLE_POS )
                {   
                    // Change from half knot to double knot
                    str += name = name2;
                    m_ucTrackingKnot = knot;
                    // Add extra length threshold for the knot change
                    m_uiLengthAccumulateTarget += m_puiLengthThresholds[0] / 2;
                }
            }
        }

        //-------------------------------------------------
        // A new knot is formed or being tracked
        if ( m_ucTrackingKnot != Data::DatabaseDowkerNotation::UNDEFINED ) {

            /*
            // Recovering from incorrect collision detection
            // Case when there was a knot then there is no crossing pairs
            if ( m_iNumCrossingPairs == 0 ) {
                m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED;
                str = "Recovering from an error!";

                m_uiCounterForFixingKnot = 0;
                ResetElapsedTime();

                // DEBUG
                //std::cout << "m_iNumCrossingPairs was ZERO!\n" << std::endl;
            }
            //*/

            // Recovering from incorrect collision detection
            // Case when there was a knot then there is no crossing pairs
            if ( m_iNumCrossingPairs == 0 ) {

                // RECORD THE KNOT -- SPECIAL FOR BROKEN SIMULATION OF THE KNOT
                //-------------------------------------------------
                int knotLength = m_KnotLinkEndTempRecord - m_KnotLinkStartTempRecord;
                // Since the knot tracking is broken, then the locked knot length must be m_puiLengthThresholds[m_ucKnotCount]

                std::stringstream ss;
                ss << "knotLength < m_puiLengthThresholds[m_ucKnotCount]?: " << knotLength << " < " << m_puiLengthThresholds[m_ucKnotCount] << "?";
                wxMessageDialog( NULL, ss.str(), "Info", wxOK );
                std::cout << ss.str() << "\n";

                if ( knotLength < static_cast<int>( m_puiLengthThresholds[m_ucKnotCount] ) ) {
                    int diff = m_puiLengthThresholds[m_ucKnotCount] - knotLength;
                    if ( diff % 2 == 0 ) {
                        diff /= 2;
                        m_KnotLinkStartTempRecord -= diff;
                        m_KnotLinkEndTempRecord -= diff;
                    }
                    else {
                        diff /= 2;
                        m_KnotLinkStartTempRecord -= diff+1;
                        m_KnotLinkEndTempRecord -= diff;
                    }
                    // Record knot ID and increase knot count
                    RecordKnot( m_KnotLinkStartTempRecord-1, m_KnotLinkEndTempRecord+1, m_ucTrackingKnot );
                    // Reset counter and set the new accumulated target length for next knot
                    m_uiCounterForFixingKnot = 0;
                    ResetElapsedTime();
                    if ( static_cast<unsigned int>( knotLength ) > m_uiMinLengthThreshold ) {
                        m_uiMinLengthThreshold = knotLength;    // new min length
                    }
                    m_uiLengthAccumulateTarget = m_uiMinLengthThreshold + m_puiLengthThresholds[m_ucKnotCount];
                    // Reset tracking knot
                    m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED;
                    str += "";
                    //-------------------------------
                    ResetStartTime();
                    return str;
                }
                else {
                    m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED;
                    str += "Recovering from an error!";

                    m_uiCounterForFixingKnot = 0;
                    ResetElapsedTime();
                }
                //-------------------------------------------------

            }

            char val[16];
            str += "Tracking Knot# ";
            str += _itoa(m_ucKnotCount+1, val, 10);
            str += " (" + name + ")";

            str += " ID: ";
            str += _itoa(m_ucTrackingKnot, val, 10);

            // DEBUG
            //std::cout << "end-start: " << end-start << " = " << end << " - " << start << "\n";
            //std::cout << "TargetLength: " << m_uiLengthAccumulateTarget << "\n";
            //std::cout << "CounterForFixingKnot: " << m_uiCounterForFixingKnot << "\n";

            //-----------------------------------
            // Skip invalid length
            if ( end < 0 || start < 0 ) {
                //++m_uiCounterForFixingKnot;
//              start = m_KnotLinkStartTempRecord;
//              end   = m_KnotLinkEndTempRecord;
            }

            //-----------------------------------
            // Track the first knot
            if ( m_ucKnotCount == 0 ) {
                int knotLength = end-start;
                if ( knotLength >= static_cast<int>( m_uiMinLengthThreshold ) || static_cast<int>( m_uiCounterForFixingKnot ) <= m_uiCounterThreshold )
                {
                    if ( knotLength < static_cast<int>( m_uiLengthAccumulateTarget ) ) {
                        // Reset counter and set new accumulated target length
                        m_uiCounterForFixingKnot = 0;
                        m_uiLengthAccumulateTarget = knotLength;
                        ResetElapsedTime();
                    }
                    // LOCK KNOT
                    else if ( m_pParameters->EnableKR_LockKnots && ( knotLength <= static_cast<int>( m_uiLengthAccumulateTarget ) + 2 ) ) {
                        if ( ++m_uiCounterForFixingKnot >= m_uiCounterThreshold ) {

                            // RECORD THE KNOT
                            //-------------------------------------------------
                            // Record knot ID and increase knot count
                            RecordKnot( start-1, end+1, m_ucTrackingKnot );
                            // Reset counter and set the new accumulated target length for next knot
                            m_uiCounterForFixingKnot = 0;
                            ResetElapsedTime();
                            if ( knotLength >= static_cast<int>( m_uiMinLengthThreshold ) ) {
                                m_uiMinLengthThreshold = knotLength;    // new min length
                            }
                            m_uiLengthAccumulateTarget = m_uiMinLengthThreshold + m_puiLengthThresholds[m_ucKnotCount];
                            // Reset tracking knot
                            m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED;
                            str += "";
                            //-------------------------------------------------
                        }
                    }
                    //*/
                    else {
                        ++m_uiCounterForFixingKnot;
                        AddElapsedTime();
                    }
                }
            }
            //-----------------------------------
            // Tracking the second, third, and so on
            else if ( 0 < m_ucKnotCount && m_ucKnotCount < m_ucMaxNumKnotsAllowed ) {
                int knotLength = end-start;
                if ( knotLength >= static_cast<int>( m_uiMinLengthThreshold ) || static_cast<int>( m_uiCounterForFixingKnot ) <= m_uiCounterThreshold ) {
                    if ( knotLength < static_cast<int>( m_uiLengthAccumulateTarget ) ) {
                        // Reset counter and set new accumulated target length
                        m_uiCounterForFixingKnot = 0;
                        m_uiLengthAccumulateTarget = knotLength;
                        ResetElapsedTime();
                    }
                    else if ( ( knotLength <= static_cast<int>( m_uiLengthAccumulateTarget ) + 2 ) || 

                        ( IsTimeLimitReached() && knotLength <= static_cast<int>( m_uiLengthAccumulateTarget ) + 4 ) ) {

                        //(m_uiCounterForFixingKnot >= 125) ) {

                        #ifdef  TAPs_STRAND_DEBUG
                        if ( IsTimeLimitReached() ) {
                            std::cout << "Elapsed Time: " << m_tAccTime << "\n";
                        }
                        #endif//TAPs_STRAND_DEBUG

                        // LOCK KNOT
                        if ( m_pParameters->EnableKR_LockKnots && ++m_uiCounterForFixingKnot >= m_uiCounterThreshold ) {

                            // RECORD THE KNOT
                            //-------------------------------------------------
                            // Record knot ID and increase knot count
                            RecordKnot( start-1, end+1, m_ucTrackingKnot );
                            // Reset counter and set the new accumulated target length for next knot
                            m_uiCounterForFixingKnot = 0;
                            ResetElapsedTime();
                            if ( knotLength >= static_cast<int>( m_uiMinLengthThreshold ) ) {
                                m_uiMinLengthThreshold = knotLength;    // new min length
                            }
                            m_uiLengthAccumulateTarget = m_uiMinLengthThreshold + m_puiLengthThresholds[m_ucKnotCount];
                            // Reset tracking knot
                            m_ucTrackingKnot = Data::DatabaseDowkerNotation::UNDEFINED;
                            str += "";
                            //-------------------------------------------------

                        }
                        //*/
                    }
                    else {
                        ++m_uiCounterForFixingKnot;
                        AddElapsedTime();
                    }
                }
            }
        }
        //std::cout << "Acc Time: " << m_tAccTime << "\n";
    }
    ResetStartTime();
    return str;
}


// Record a knot
template <typename T>
void ElasticRodKR<T>::RecordKnot ( int startLink, int endLink, int knotID )
{
    int startLoopLink = ( endLink - startLink )/2 + startLink;
    int endLoopLink = startLoopLink + 1;

    RecordKnot( startLink, startLoopLink, endLoopLink, endLink, knotID );

}


// Record a knot with flexible knot loop part
template <typename T>
void ElasticRodKR<T>::RecordKnot ( int startLink, int startLoopLink, int endLoopLink, int endLink, int knotID )
{
    //
    // Store the min and max of point IDs that belong to recorded knots
    //
    if ( m_iMinPtIdThatIsInRecordedKnots > startLink || m_iMinPtIdThatIsInRecordedKnots < 0 ) {
        m_iMinPtIdThatIsInRecordedKnots = startLink;
    }
    if ( m_iMaxPtIdThatIsInRecordedKnots < endLink ) {
        m_iMaxPtIdThatIsInRecordedKnots = endLink;
    }

    //
    // Record the knot
    //
    m_pListOfLockedKnots[m_ucKnotCount].ID = knotID;
    m_pListOfLockedKnots[m_ucKnotCount].StartLinkID = startLink;
    m_pListOfLockedKnots[m_ucKnotCount].StartLoopLinkID = startLoopLink;
    m_pListOfLockedKnots[m_ucKnotCount].EndLoopLinkID = endLoopLink;
    m_pListOfLockedKnots[m_ucKnotCount].EndLinkID = endLink;
    ++m_ucKnotCount;
    //
    // Set the status of all links forming the knot to "CLUSTERED"
    int count = 0;
    SetOfElasticRodNodes::iterator  currNode  = m_pNodeList->begin();
    while ( currNode != m_pNodeList->end() ) {
        if (    ( startLink <= count && count <= startLoopLink )
            ||  ( endLoopLink <= count && count <= endLink ) )
        {
            currNode->SimFlags.SetSimulationConstraints( Enum::AddOn::CLUSTERED );

            // DEBUG
            //currNode->SimFlags.SetSimulationConstraints( Enum::AddOn::FIXED );
        }
        ++count;
        ++currNode;
    }
}


// Create a knot (shape) from a set of links
template <typename T>
void ElasticRodKR<T>::CombineTwoKnots (
    int startLockedKnotID,                          
    Data::DatabaseDowkerNotation::KNOT knotType,    
    Matrix4x4<T> const & orientation                
)
{
    const T angleDeg = 30;

    T angle = angleDeg * Math<T>::DEG_TO_RAD;
    T cos_X = cos( angle );
    T sin_Y = sin( angle );
    T dx = cos_X * m_pParameters->LinkLength;
    T dy = sin_Y * m_pParameters->LinkLength;
    T dy2 = dy * 2; // for raising the 2nd knot in y-direction
    T dz = m_pParameters->Radius;
    T dz2 = dz * 2;
    T x1, x2, y1, y2, z1, z2, z1_2, z2_2;
    int numOfCrossings_1;   // the 1st knot adjacent to the combined knot's loop
    int numOfCrossings_2;   // the 2nd knot on top of the 1st knot

    //
    // Setting up the 1st knot
    //

    // REMARK:
    // From the view point the 2nd knot is 180 degree rotated from the first knot.
    // Therefore, z2 and z2_2 values for the 2nd knot have to be multiplied by -1.
    // Hence, for example, HALF_NEG_FOLLOWED_BY_HALF_POS which has 
    // the 1st knot of HALF_NEG followed by the 2nd knot of HALF_POS.
    // The z values for forming the 1st knot are set same as setting up a HALF_NEG knot,
    // but the z values of the 2nd knot are set same as setting up a HALF_NEG knot, 
    // due to the 180 degree rotation.

    // Set suture's node offsets for creating the knot shape
    switch ( knotType ) {

        case Data::DatabaseDowkerNotation::DOUBLE_NEG_FOLLOWED_BY_HALF_NEG:
            numOfCrossings_1 = 5;
            z1 = dz;
            z1_2 = dz2;
            numOfCrossings_2 = 3;
            z2 = -dz;
            z2_2 = -dz2;
            break;

        case Data::DatabaseDowkerNotation::DOUBLE_NEG_FOLLOWED_BY_HALF_POS:
            numOfCrossings_1 = 5;
            z1 = dz;
            z1_2 = dz2;
            numOfCrossings_2 = 3;
            z2 = dz;
            z2_2 = dz2;
            break;

        case Data::DatabaseDowkerNotation::DOUBLE_POS_FOLLOWED_BY_HALF_NEG:
            numOfCrossings_1 = 5;
            z1 = -dz;
            z1_2 = -dz2;
            numOfCrossings_2 = 3;
            z2 = -dz;
            z2_2 = -dz2;
            break;

        case Data::DatabaseDowkerNotation::DOUBLE_POS_FOLLOWED_BY_HALF_POS:
            numOfCrossings_1 = 5;
            z1 = -dz;
            z1_2 = -dz2;
            numOfCrossings_2 = 3;
            z2 = dz;
            z2_2 = dz2;
            break;

        case Data::DatabaseDowkerNotation::HALF_NEG_FOLLOWED_BY_HALF_NEG:
            numOfCrossings_1 = 3;
            z1 = dz;
            z1_2 = dz2;
            numOfCrossings_2 = 3;
            z2 = -dz;
            z2_2 = -dz2;
            break;

        case Data::DatabaseDowkerNotation::HALF_POS_FOLLOWED_BY_HALF_NEG:
            numOfCrossings_1 = 3;
            z1 = -dz;
            z1_2 = -dz2;
            numOfCrossings_2 = 3;
            z2 = -dz;
            z2_2 = -dz2;
            break;

        case Data::DatabaseDowkerNotation::HALF_POS_FOLLOWED_BY_HALF_POS:
            numOfCrossings_1 = 3;
            z1 = -dz;
            z1_2 = -dz2;
            numOfCrossings_2 = 3;
            z2 = dz;
            z2_2 = dz2;
            break;

        case Data::DatabaseDowkerNotation::HALF_NEG_FOLLOWED_BY_HALF_POS:
        default:    // same as HALF_NEG_FOLLOWED_BY_HALF_POS
            numOfCrossings_1 = 3;
            z1 = dz;
            z1_2 = dz2;
            numOfCrossings_2 = 3;
            z2 = dz;
            z2_2 = dz2;
            break;
    }
    y1 = dy;
    y2 = dy;
    x1 = dx * numOfCrossings_1;
    x2 = -dx * numOfCrossings_2;
    int twiceNumOfCrossings_1 = numOfCrossings_1 * 2;
    int twiceNumOfCrossings_2 = numOfCrossings_2 * 2;

    // DEBUG
    //std::cout << "twiceNumOfCrossings_1: " << twiceNumOfCrossings_1 << "\n";
    //std::cout << "twiceNumOfCrossings_2: " << twiceNumOfCrossings_2 << "\n";

    //int knotShapeLength = twiceNumOfCrossings_1 + twiceNumOfCrossings_2;
    //int startLoopLinkID = startLinkID + knotShapeLength;
    //int endLoopLinkID = endLinkID - knotShapeLength;
    //int knotIDStart_1 = startLoopLinkID - twiceNumOfCrossings_1;
    //int knotIDEnd_1 = endLoopLinkID + twiceNumOfCrossings_1;
    //int knotIDStart_2 = knotIDStart_1 - twiceNumOfCrossings_2;
    //int knotIDEnd_2 = knotIDEnd_1 + twiceNumOfCrossings_2;

    int startLoopLinkID = m_pListOfLockedKnots[startLockedKnotID].StartLoopLinkID;
    int endLoopLinkID = m_pListOfLockedKnots[startLockedKnotID].EndLoopLinkID;
    int knotIDStart_1 = startLoopLinkID - twiceNumOfCrossings_1;
    int knotIDEnd_1 = endLoopLinkID + twiceNumOfCrossings_1;
    int knotIDStart_2 = knotIDStart_1 - twiceNumOfCrossings_2;
    int knotIDEnd_2 = knotIDEnd_1 + twiceNumOfCrossings_2;


    //
    // Calculate translation and orientation for the animated knot
    //
    //if ( m_pListOfLockedKnots[startLockedKnotID].GetStatusCalKnotPosition() ) {
        Vector3<T> center   = (   (*m_pNodeList)[knotIDStart_2-1].Centerline[*m_pIdxNext].GetPosition() 
                                + (*m_pNodeList)[knotIDEnd_2+1].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
        //Vector3<T> direction  = (   (*m_pNodeList)[knotIDStart-1].Centerline[*m_pIdxNext].GetPosition() 
        //                          - (*m_pNodeList)[knotIDEnd+1].Centerline[*m_pIdxNext].GetPosition() ).GetUnit();
        //Matrix4x4<T> orientation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( CGMath<T>::VectorX, direction );
    //}

    //
    // Create the 1st knot shape from the constrained links
    //

    int s = knotIDStart_1;
    int e = knotIDEnd_1 - twiceNumOfCrossings_1;
    Vector3<T> posS, posE;
    for ( int i = 0; i <= twiceNumOfCrossings_1;  )
    {
        {
            posS = (orientation * Vector4<T>( x1, y1, z1, 1)).GetVector3() + center;
            posE = (orientation * Vector4<T>( x1,-y1,-z1, 1)).GetVector3() + center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x1 -= dx;

        if ( i < twiceNumOfCrossings_1 )
        {
            posS = (orientation * Vector4<T>( x1, 0, z1_2, 1)).GetVector3() + center;
            posE = (orientation * Vector4<T>( x1, 0,-z1_2, 1)).GetVector3() + center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x1 -= dx;
        y1 = -y1;
        z1 = -z1;
        z1_2 = -z1_2;
    }

    //
    // Create the 2nd knot shape from the constrained links
    //
    //Matrix4x4<T> orientation2 = orientation * CGMath<T>::CreateRotationMatrix4x4( 1, 0, 0, 180 );
    center[1] += dy2;
    s = knotIDStart_2;
    e = knotIDEnd_2 - twiceNumOfCrossings_2;
    for ( int i = 0; i <= twiceNumOfCrossings_2;  )
    {
        {
            posS = (orientation * Vector4<T>( x2, y2, z2, 1)).GetVector3() + center;
            posE = (orientation * Vector4<T>( x2,-y2,-z2, 1)).GetVector3() + center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x2 += dx;

        if ( i < twiceNumOfCrossings_2 )
        {
            posS = (orientation * Vector4<T>( x2, 0, z2_2, 1)).GetVector3() + center;
            posE = (orientation * Vector4<T>( x2, 0,-z2_2, 1)).GetVector3() + center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x2 += dx;
        y2 = -y2;
        z2 = -z2;
        z2_2 = -z2_2;
    }

    //s = startLoopLinkID + 1;
    //e = endLoopLinkID - 1;
    //int m = ( e - s ) / 2;
    //...

    //
    // Combined locked knots into one locked knot, start at startLockedKnotID
    //
    {
        int numOfCombinedKnots = m_ucKnotCount - startLockedKnotID;
        if ( numOfCombinedKnots <= 1 )  return;
        numOfCombinedKnots = 2;

        m_ucKnotCount -= numOfCombinedKnots - 1;
        m_pListOfLockedKnots[startLockedKnotID].ID              = knotType;
        m_pListOfLockedKnots[startLockedKnotID].StartLinkID     = knotIDStart_2;
        m_pListOfLockedKnots[startLockedKnotID].StartLoopLinkID = startLoopLinkID;
        m_pListOfLockedKnots[startLockedKnotID].EndLoopLinkID   = endLoopLinkID;
        m_pListOfLockedKnots[startLockedKnotID].EndLinkID       = knotIDEnd_2;

        int i = knotIDStart_2;
        for ( ; i < startLoopLinkID; ++i ) {
            (*m_pNodeList)[i].SimFlags.SetSimulationConstraints( Enum::AddOn::CLUSTERED );
        }
        for ( ; i <= endLoopLinkID; ++i ) {
            (*m_pNodeList)[i].SimFlags.ClearSimulationConstraints( Enum::AddOn::CLUSTERED );
        }
        for ( ; i <= knotIDEnd_2; ++i ) {
            (*m_pNodeList)[i].SimFlags.SetSimulationConstraints( Enum::AddOn::CLUSTERED );
        }
    }
    /*
    // For multiple knots
    {
        int numOfCombinedKnots = m_ucKnotCount - startLockedKnotID;
        if ( numOfCombinedKnots <= 1 )  return;

        int i = startLockedKnotID;
        int startLinkID     = m_pListOfLockedKnots[i].StartLinkID;
        int startLoopLinkID = m_pListOfLockedKnots[i].StartLoopLinkID;
        int endLoopLinkID   = m_pListOfLockedKnots[i].EndLoopLinkID;
        int endLinkID       = m_pListOfLockedKnots[i].EndLinkID;
        for ( ++i; i < startLockedKnotID + numOfCombinedKnots; ++i ) {
            if ( m_pListOfLockedKnots[i].StartLinkID        < startLinkID )         startLinkID     = m_pListOfLockedKnots[i].StartLinkID;
            if ( m_pListOfLockedKnots[i].StartLoopLinkID    > startLoopLinkID )     startLoopLinkID = m_pListOfLockedKnots[i].StartLoopLinkID;
            if ( m_pListOfLockedKnots[i].EndLoopLinkID      < endLoopLinkID )       endLoopLinkID   = m_pListOfLockedKnots[i].EndLoopLinkID;
            if ( m_pListOfLockedKnots[i].EndLinkID          > endLinkID )           endLinkID       = m_pListOfLockedKnots[i].EndLinkID;
        }
        m_ucKnotCount -= numOfCombinedKnots - 1;
        m_pListOfLockedKnots[startLockedKnotID].ID              = knotType;
        m_pListOfLockedKnots[startLockedKnotID].StartLinkID     = startLinkID;
        m_pListOfLockedKnots[startLockedKnotID].StartLoopLinkID = startLoopLinkID-2;
        m_pListOfLockedKnots[startLockedKnotID].EndLoopLinkID   = endLoopLinkID+2;
        m_pListOfLockedKnots[startLockedKnotID].EndLinkID       = endLinkID;
    }
    //*/
}


// Constraint knots (forcing all links forming each knot to move as a unit)
template <typename T>
void ElasticRodKR<T>::ConstrainKnots ()
{
    // For each locked knot
    for ( int i = 0; i < m_ucKnotCount; ++i ) {
        Quaternion<T> ori_o( 0, 0, 0, 0 );
        Quaternion<T> ori_n( 0, 0, 0, 0 );
        Vector3<T>    pos_o( 0, 0, 0 );
        Vector3<T>    pos_n( 0, 0, 0 );
        Vector3<T>    vel_o( 0, 0, 0 );
        Vector3<T>    vel_n( 0, 0, 0 );
        int div = 0;

        // There is at least one pickd point
        bool IsThereAtLeastOneEnforcedPosPt = false;

        // Sum up
        //for ( int n = m_pListOfLockedKnots[i].StartLinkID; n <= m_pListOfLockedKnots[i].EndLinkID; ++n ) {
        for ( int n = m_pListOfLockedKnots[i].StartLinkID; n <= m_pListOfLockedKnots[i].StartLoopLinkID; ++n ) {
            //if ( !(*m_pNodeList)[n].SimFlags.CheckSimulationConstraints( Enum::AddOn::FIXED ) )
            {
                ori_o += (*m_pNodeList)[n].Orientation[*m_pIdxCurr];
                ori_n += (*m_pNodeList)[n].Orientation[*m_pIdxNext];
                pos_o += (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetPosition();
                pos_n += (*m_pNodeList)[n].Centerline[*m_pIdxNext].GetPosition();
                vel_o += (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetVelocity();
                vel_n += (*m_pNodeList)[n].Centerline[*m_pIdxNext].GetVelocity();
                ++div;

                if ( (*m_pNodeList)[n].UseEnforcedPosition )    IsThereAtLeastOneEnforcedPosPt = true;
            }
        }
        for ( int n = m_pListOfLockedKnots[i].EndLoopLinkID; n <= m_pListOfLockedKnots[i].EndLinkID; ++n ) {
            //if ( !(*m_pNodeList)[n].SimFlags.CheckSimulationConstraints( Enum::AddOn::FIXED ) )
            {
                ori_o += (*m_pNodeList)[n].Orientation[*m_pIdxCurr];
                ori_n += (*m_pNodeList)[n].Orientation[*m_pIdxNext];
                pos_o += (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetPosition();
                pos_n += (*m_pNodeList)[n].Centerline[*m_pIdxNext].GetPosition();
                vel_o += (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetVelocity();
                vel_n += (*m_pNodeList)[n].Centerline[*m_pIdxNext].GetVelocity();
                ++div;

                if ( (*m_pNodeList)[n].UseEnforcedPosition )    IsThereAtLeastOneEnforcedPosPt = true;
            }
        }

        // Averaging
        if ( div > 0 ) {
            ori_o *= T(1)/div;
            ori_n *= T(1)/div;
            pos_o /= div;
            pos_n /= div;
            vel_o /= div;
            vel_n /= div;
            if ( m_pListOfLockedKnots[i].DoLockLocation && !IsThereAtLeastOneEnforcedPosPt ) {
                pos_n += m_pListOfLockedKnots[i].Center - pos_o;
            }
            Quaternion<T> ori_change = ori_n - ori_o;
            Vector3<T> pos_change = pos_n - pos_o;
            Vector3<T> vel_change = vel_n - vel_o;
            //for ( int n = m_pListOfLockedKnots[i].StartLinkID; n <= m_pListOfLockedKnots[i].EndLinkID; ++n ) {
            for ( int n = m_pListOfLockedKnots[i].StartLinkID; n <= m_pListOfLockedKnots[i].StartLoopLinkID; ++n ) {
                (*m_pNodeList)[n].Centerline[*m_pIdxNext].SetVelocity( (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetVelocity() + vel_change );
                (*m_pNodeList)[n].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetPosition() + pos_change );
                (*m_pNodeList)[n].Orientation[*m_pIdxNext] = (*m_pNodeList)[n].Orientation[*m_pIdxCurr] + ori_change;
                (*m_pNodeList)[n].Orientation[*m_pIdxNext].Normalized();
            }
            for ( int n = m_pListOfLockedKnots[i].EndLoopLinkID; n <= m_pListOfLockedKnots[i].EndLinkID; ++n ) {
                (*m_pNodeList)[n].Centerline[*m_pIdxNext].SetVelocity( (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetVelocity() + vel_change );
                (*m_pNodeList)[n].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[n].Centerline[*m_pIdxCurr].GetPosition() + pos_change );
                (*m_pNodeList)[n].Orientation[*m_pIdxNext] = (*m_pNodeList)[n].Orientation[*m_pIdxCurr] + ori_change;
                (*m_pNodeList)[n].Orientation[*m_pIdxNext].Normalized();
            }
        }
    }
}


/*
// Get an approximate center of knot i
template <typename T>
void ElasticRodKR<T>::GetApproxKnotCenterAndOrientation ( unsigned int i, Vector3<T> &center, Quaternion<T> &orientation )
{
    if ( i >= GetKnotCount() ) return;

    // Averaging all positions as an approximate center
    int div = 0;
    for ( int n = m_pListOfLockedKnots[i].StartLinkID; n <= m_pListOfLockedKnots[i].EndLinkID; ++n )
    {
        center += (*m_pNodeList)[n].Centerline[*m_pIdxNext].GetPosition();
        ++div;
    }
    // Averaging
    if ( div > 0 ) {
        center /= div;
    }
}
//*/


// Return the name of a knot
template <typename T>
std::string ElasticRodKR<T>::GetNameOfKnot ( unsigned int i, bool withPosOrNegPostfix )
{
    if ( i >= m_ucKnotCount )   return "n\a";
    if ( withPosOrNegPostfix ) {
        if ( m_pListOfLockedKnots[i].ID % 2 == 0 ) {
            return Data::DatabaseDowkerNotation::GetKnotName( m_pListOfLockedKnots[i].ID ) + "(-)";
        }
        else {
            return Data::DatabaseDowkerNotation::GetKnotName( m_pListOfLockedKnots[i].ID ) + "(+)";
        }
    }
    return Data::DatabaseDowkerNotation::GetKnotName( m_pListOfLockedKnots[i].ID );
}


// Return the name of a knot combination
template <typename T>
std::string ElasticRodKR<T>::GetStrNameForKnotCombination ()
{
    std::string str;
    for ( int i = 1; i < m_ucKnotCount; ++i ) {
        if ( m_pListOfLockedKnots[i-1].ID == 0 ) {
            if ( m_pListOfLockedKnots[i].ID == 0 ) {
                str = "Granny " + str;
            }
            else if ( m_pListOfLockedKnots[i].ID == 1 ) {
                str = "Square " + str;
            }
        }
        else if ( m_pListOfLockedKnots[i-1].ID == 1 ) {
            if ( m_pListOfLockedKnots[i].ID == 1 ) {
                str = "Granny " + str;
            }
            else if ( m_pListOfLockedKnots[i].ID == 0 ) {
                str = "Square " + str;
            }
        }
    }
    return str + " Knot";
}


template <typename T>
bool ElasticRodKR<T>::SetKnotControlThreshold ( enum Threshold thdID, T val )
{
    if ( val < 0.0 )    return false;
    switch ( thdID ) {
        case THRESHOLD_FOR_FIXING_KNOT:
            m_uiCounterThreshold = static_cast< unsigned int >( val );
            return true;
        case THRESHOLD_WAIT_TIME:
            m_tWaitTime = val;
            return true;
        case LENGTH_THRESHOLD_0:
            m_puiLengthThresholds[0] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_1:
            m_puiLengthThresholds[1] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_2:
            m_puiLengthThresholds[2] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_3:
            m_puiLengthThresholds[3] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_4:
            m_puiLengthThresholds[4] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_5:
            m_puiLengthThresholds[5] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_6:
            m_puiLengthThresholds[6] = static_cast< unsigned int >( val );
            return true;
        case LENGTH_THRESHOLD_7:
            m_puiLengthThresholds[7] = static_cast< unsigned int >( val );
            return true;
    }
    return false;
}


template <typename T>
T ElasticRodKR<T>::GetKnotControlThreshold ( enum Threshold thdID ) const
{
    switch ( thdID ) {
        case THRESHOLD_FOR_FIXING_KNOT:
            return static_cast<T>( m_uiCounterThreshold );
        case THRESHOLD_WAIT_TIME:
            return static_cast<T>( m_tWaitTime );
        case LENGTH_THRESHOLD_0:
            return static_cast<T>( m_puiLengthThresholds[0] );
        case LENGTH_THRESHOLD_1:
            return static_cast<T>( m_puiLengthThresholds[1] );
        case LENGTH_THRESHOLD_2:
            return static_cast<T>( m_puiLengthThresholds[2] );
        case LENGTH_THRESHOLD_3:
            return static_cast<T>( m_puiLengthThresholds[3] );
        case LENGTH_THRESHOLD_4:
            return static_cast<T>( m_puiLengthThresholds[4] );
        case LENGTH_THRESHOLD_5:
            return static_cast<T>( m_puiLengthThresholds[5] );
        case LENGTH_THRESHOLD_6:
            return static_cast<T>( m_puiLengthThresholds[6] );
        case LENGTH_THRESHOLD_7:
            return static_cast<T>( m_puiLengthThresholds[7] );
    }
    return -1;
}


template <typename T>
int ElasticRodKR<T>::KnotRecognition ()
{
    return CheckSelfCrossingForKnotRecognition( GetProjDirection() );
}


template <typename T>
int ElasticRodKR<T>::CheckSelfCrossingForKnotRecognition ( Vector3<T> const & projectionDirection )
{
#ifdef  TAPs_NO_DOWKER_KNOT_CHECKING
    // NO CHECKING FOR KNOTS BY DOWKER NOTATION
    return -1;
#endif//TAPs_NO_DOWKER_KNOT_CHECKING

    // Reset
    for ( int i = 0; i < m_iMaxCrossingPairs; ++i ) m_iaDowkerNotation[i] = 0;
    m_crossings.clear();
    for ( int i = 0; i < m_pParameters->NumOfNodes-1; ++i ) m_iaCrossingCounters[i] = 0;

    // Find Crossings
    CheckSelfCrossingsNextLevelRecursively( m_BVHTree->Root()->Child(0), 
                                            m_BVHTree->Root()->Child(1), 
                                            projectionDirection );
    // For Recording Dowker Notation
    std::vector<int> crosses;
    std::vector<int> crossesID;

    //std::cout << "m_crossings.size(): " << m_crossings.size() << std::endl;

    //return -1;

    std::list< IC_Crossing >::const_iterator it = m_crossings.begin();
    while ( it != m_crossings.end() ) {
        // crossesID records the node number that has another node cross over or under it.
        // crosses   records the crossing identified by node number that crosses over the crossesID.
        // E.g. real cross#  1   2   3   4   5   6 
        //      crossesID:  30  31  32  74  77  79
        //      crosses:   -74  77 -79  30 -31  32
        //      results in the Dowker notation below
        //      Dowker#:    (1,4)   (3,6)   (5,2)
        //      which is a half knot
        crossesID.push_back( (*it).id );
        crosses.push_back( (*it).number );
        ++it;
    }

    int numOfCrosses = static_cast<int>( crosses.size() );

    if ( numOfCrosses == 0 ) {      // Not a knot
        m_iKnotStartPt = m_iKnotEndPt = -1;
        m_iNumCrossingPairs = 0;
    }
    else if ( numOfCrosses % 2 == 0 ) {

        //std::cout << "numOfCrosses % 2 == 0" << "\tm_iMaxCrossingPairs: " << m_iMaxCrossingPairs << std::endl;

        m_iNumCrossingPairs = numOfCrosses/2;
        assert( m_iNumCrossingPairs <= m_iMaxCrossingPairs );
        int idx = 0;
        for ( int i = 0, odd = 1; i < numOfCrosses; i+=2, odd+=2 ) {
            for ( int j = 1, even = 2; j < numOfCrosses; j+=2, even+=2 ) {
                if ( crossesID[i] == abs( crosses[j] ) ) {

                    //std::cout << "idx: " << idx << std::endl;

                    //iaDowkerNotation[ idx++ ] = odd;
                    if ( crosses[j] < 0 )
                        m_iaDowkerNotation[ idx++ ] = -even;
                    else
                        m_iaDowkerNotation[ idx++ ] =  even;
                }
            }
        }

        m_iKnotStartPt = crossesID[0];
        m_iKnotEndPt   = crossesID[numOfCrosses-1];

        //std::cout << "END: numOfCrosses % 2 == 0" << std::endl;
    }
    else {  // Not a knot
        m_iKnotStartPt = m_iKnotEndPt = -1;
        m_iNumCrossingPairs = numOfCrosses/2;
    }

    //*
    // DEBUG
    debug_crosses   = crosses;
    debug_crossesID = crossesID;
    std::cout << Debug_Str();
    //*/

    // Find the knot that match with the Dowker notation
    int knotID = ConverseDowkerNotationToKnotName();
    if ( knotID >= 0 ) {
        return knotID;
    }

    /*
    // Check Clumped
    if ( m_iNumCrossingPairs > 1 ) {
        if ( m_ucTrackingKnot != Data::DatabaseDowkerNotation::UNDEFINED ) {
            T sphereRadius = m_tLinkLength * 10.0;
            CheckClumped( m_iKnotStartPt, m_iKnotEndPt, sphereRadius );
            if ( IsClumped() ) {
                #ifdef TAPs_STRAND_DEBUG
                std::cout << "CLUMPED: true (" << m_iKnotStartPt << "," << m_iKnotEndPt << ")" << "\n";
                #endif//TAPs_STRAND_DEBUG

                m_uiCounterForFixingKnot = m_uiCounterThreshold+1;

                #ifdef TAPs_STRAND_DEBUG
                std::cout << "RET: " << m_ucTrackingKnot << "\n";
                #endif//TAPs_STRAND_DEBUG

                return m_ucTrackingKnot;
            }
        }
        else {
            //std::cout << "CLUMPED: false (" << m_iKnotStartPt << "," << m_iKnotEndPt << ")" << std::endl;
        }
    }
    //*/

    return knotID;
}



//=============================================================================
// START: Fns for CheckSelfCrossingForKnotRecognition function
//-----------------------------------------------------------------------------
// CheckSelfCrossingsNextLevelRecursively
// Assume no intersections between immediate adjacent links
// Assume a BVH node either has both children or none, therefore 
//   checking either it has a left or right child is enough to determine 
//   whether it is a leaf node or not.
template <typename T>
void ElasticRodKR<T>::CheckSelfCrossingsNextLevelRecursively ( 
            BVHNode<T> * node1, BVHNode<T> * node2, 
            Vector3<T> const & projectionDirection 
)
{
    CheckSelfCrossingsRecursively( node1, node2, projectionDirection );
    if ( node1->Child(0) )
        CheckSelfCrossingsNextLevelRecursively( node1->Child(0), node1->Child(1), projectionDirection );
    if ( node2->Child(0) )
        CheckSelfCrossingsNextLevelRecursively( node2->Child(0), node2->Child(1), projectionDirection );
}
//-----------------------------------------------------------------
// CheckSelfCrossingsRecursively
// Assume no intersections between immediate adjacent links
// Use the fact that the ids of sphere bvh leaf nodes are the same as 
//   the link id and 
template <typename T>
T ElasticRodKR<T>::CheckSelfCrossingsRecursively (
            BVHNode<T> * node1, BVHNode<T> * node2, 
            Vector3<T> const & projectionDirection 
)
{
    const T LinkDiameter = static_cast<T>(2.0) * m_pParameters->Radius;
    const T epsilon = m_pParameters->Radius * static_cast<T>(0.05); // for collision response
    //-------------------------------------------------------------
    if ( !node1->Child(0) && !node2->Child(0) && 
        ( abs(node1->GetID() - node2->GetID()) == 1 ) )
    {
        return DBL_MAX;
    }
    //-------------------------------------------------------------
    T overlap = TestOverlapCircle( node1, node2, projectionDirection );
    //-------------------------------------------------------------
    if ( overlap < DBL_MIN ) {
        if ( node2->Child(0) == NULL ) { //&& node2->RightChild() == NULL ) {
            if ( node1->Child(0) ) {
                overlap = CheckSelfCrossingsRecursively( node1->Child(0), node2, projectionDirection );
            }
            if ( node1->Child(1) ) {
                overlap = CheckSelfCrossingsRecursively( node1->Child(1), node2, projectionDirection );
            }
        }
        else {
            if ( node2->Child(0) ) {
                overlap = TestOverlapCircle( node1, node2->Child(0), projectionDirection );
                if ( overlap < DBL_MIN ) {
                    if ( node1->Child(0) ) {
                        overlap = CheckSelfCrossingsRecursively( node1->Child(0), node2->Child(0), projectionDirection );
                    }
                    if ( node1->Child(1) ) {
                        overlap = CheckSelfCrossingsRecursively( node1->Child(1), node2->Child(0), projectionDirection );
                    }
                }
            }
            if ( node2->Child(1) ) {
                overlap = TestOverlapCircle( node1, node2->Child(1), projectionDirection );
                if ( overlap < DBL_MIN ) {
                    if ( node1->Child(0) ) {
                        overlap = CheckSelfCrossingsRecursively( node1->Child(0), node2->Child(1), projectionDirection );
                    }
                    if ( node1->Child(1) ) {
                        overlap = CheckSelfCrossingsRecursively( node1->Child(1), node2->Child(1), projectionDirection );
                    }
                }
            }
        }
        //---------------------------------------------------------
        if ( node1->Child(0) == NULL && //node1->Child(1) == NULL &&
            node2->Child(0) == NULL ) //&& node2->Child(1) == NULL )
        {
            CheckCrossings( node1, node2, projectionDirection );
        }
    }
    //-------------------------------------------------------------
    return overlap;
}
//-----------------------------------------------------------------
template <typename T>
T ElasticRodKR<T>::TestOverlapCircle ( 
            BVHNode<T> * node1, BVHNode<T> * node2, 
            Vector3<T> const & projectionDirection 
)
{
    Vector3<T> C1 = node1->GetCenter();
    Vector3<T> C2 = node2->GetCenter();


    C1.SetZ( 0 );
    C2.SetZ( 0 );
    return (C1 - C2).Length() - ( node1->GetRadius() + node2->GetRadius() );
}
//-----------------------------------------------------------------
template <typename T>
void ElasticRodKR<T>::CheckCrossings ( 
            BVHNode<T> * node1, BVHNode<T> * node2, 
            Vector3<T> const & projectionDirection 
)
{
    int i = node1->GetID();
    int j = node2->GetID();

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

    // Skip if a node is clustered
    if ( Node1->GetPtrToPrimitive_1()->SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) )  return;
    if ( Node1->GetPtrToPrimitive_2()->SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) )  return;
    if ( Node2->GetPtrToPrimitive_1()->SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) )  return;
    if ( Node2->GetPtrToPrimitive_2()->SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) )  return;

    Matrix3x3<T> rotationMatrix = CGMath<T>::CreateRotationMatrix3x3FromVectorAtoVectorB( 
        Vector3<T>( 0, 0, 1 ), projectionDirection );

    // the start and end points of link i and link j
    Vector3<T> I1 = Node1->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition();
    Vector3<T> I2 = Node1->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition();
    Vector3<T> J1 = Node2->GetPtrToPrimitive_1()->Centerline[*m_pIdxCurr].GetPosition();
    Vector3<T> J2 = Node2->GetPtrToPrimitive_2()->Centerline[*m_pIdxCurr].GetPosition();

    // Link# i
    Vector3<T> P1 = rotationMatrix * I1;
    Vector3<T> P2 = rotationMatrix * I2;
    // Link# j
    Vector3<T> Q1 = rotationMatrix * J1;
    Vector3<T> Q2 = rotationMatrix * J2;

    // Project in z-direction
    Vector3<T> p1( P1[0], P1[1], 0 );
    Vector3<T> p2( P2[0], P2[1], 0 );
    Vector3<T> q1( Q1[0], Q1[1], 0 );
    Vector3<T> q2( Q2[0], Q2[1], 0 );

    bool isTouched;
    T pt, qt;   // parameter for the 1st & 2nd line
                // where intersection point = p1 + pt(p2-p1)
                // where intersection point = q1 + qt(q2-q1)
    CGMath<T>::FindIntersectionLineSegmentLineSegment( 
            p1, p2, q1, q2,     // I/P
            pt, qt,             // O/P
            isTouched           // O/P
    );
    if ( 0 <= pt && pt <= 1 && 0 <= qt && qt <= 1 ) {
        Vector3<T> P = P1 + pt*( P2 - P1 );
        Vector3<T> Q = Q1 + qt*( Q2 - Q1 );

        // i+1 and j+1 are for making the link# to be one-based instead of zero-based
        if ( P[2] > Q[2] ) {
            InsertToList( -(j), i );
            InsertToList( +(i), j );
        }
        else {
            InsertToList( +(j), i );
            InsertToList( -(i), j );
        }
    }
}
//-----------------------------------------------------------------------------
// END: Fns for CheckSelfCrossingForKnotRecognition function
//=============================================================================




//=============================================================================
// START: Animating Knot Tying
#ifdef  TAPs_ADD_ANIMATED_KNOTS
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodKR<T>::ProcessKnotAnimation ()
{
    Data::DatabaseDowkerNotation::KNOT knotType;
    int knotStartLinkID, knotEndLinkID, knotNumOfCrossings;
    int tightenKnotID = ConstrainAnimatedKnots( knotType, knotStartLinkID, knotEndLinkID, knotNumOfCrossings );
    if ( tightenKnotID < 0 ) return;

    // Record the tighten knot
    //
    int twiceNumOfCrossings = knotNumOfCrossings * 2;
    RecordKnot(
        knotStartLinkID,
        knotStartLinkID + twiceNumOfCrossings,
        knotEndLinkID - twiceNumOfCrossings,
        knotEndLinkID, 
        knotType
    );

    //
    // Fix the combined knot
    //
    if ( GetKnotCount() == 2 ) {
        int knotID_1 = m_pListOfLockedKnots[0].ID;
        int knotID_2 = m_pListOfLockedKnots[1].ID;

        knotType = Data::DatabaseDowkerNotation::UNDEFINED;

        if ( knotID_1 == Data::DatabaseDowkerNotation::HALF_NEG ) {
            if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_NEG ) {
                knotType = Data::DatabaseDowkerNotation::HALF_NEG_FOLLOWED_BY_HALF_NEG;
            }
            else if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_POS ) {
                knotType = Data::DatabaseDowkerNotation::HALF_NEG_FOLLOWED_BY_HALF_POS;
            }
        }
        else if ( knotID_1 == Data::DatabaseDowkerNotation::HALF_POS ) {
            if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_NEG ) {
                knotType = Data::DatabaseDowkerNotation::HALF_POS_FOLLOWED_BY_HALF_NEG;
            }
            else if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_POS ) {
                knotType = Data::DatabaseDowkerNotation::HALF_POS_FOLLOWED_BY_HALF_POS;
            }
        }
        else if ( knotID_1 == Data::DatabaseDowkerNotation::DOUBLE_NEG ) {
            if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_NEG ) {
                knotType = Data::DatabaseDowkerNotation::DOUBLE_NEG_FOLLOWED_BY_HALF_NEG;
            }
            else if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_POS ) {
                knotType = Data::DatabaseDowkerNotation::DOUBLE_NEG_FOLLOWED_BY_HALF_POS;
            }
        }
        else if ( knotID_1 == Data::DatabaseDowkerNotation::DOUBLE_POS ) {
            if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_NEG ) {
                knotType = Data::DatabaseDowkerNotation::DOUBLE_POS_FOLLOWED_BY_HALF_NEG;
            }
            else if ( knotID_2 == Data::DatabaseDowkerNotation::HALF_POS ) {
                knotType = Data::DatabaseDowkerNotation::DOUBLE_POS_FOLLOWED_BY_HALF_POS;
            }
        }

        /*
        // DEBUG
        std::cout << "knotID_1: " << knotID_1 << "\n";
        std::cout << "knotID_2: " << knotID_2 << "\n";
        std::cout << "knotType: " << knotType << "\n";
        //*/

        CombineTwoKnots(
            0,              // start id of locked knots to be combined
            knotType,
            m_OrientationForCombinedKnot
        );
    }

}
//-----------------------------------------------------------------------------
// Constraint and animate knots (forcing all links forming each animated knot to move as a unit)
// Each animated knot will be tighten until it is locked and becomes a permanent knot.
template <typename T>
int ElasticRodKR<T>::ConstrainAnimatedKnots (
    Data::DatabaseDowkerNotation::KNOT & knotTypeOfTheFinishTightenKnot,    
    int & knotIDStartOfTheFinishTightenKnot,    
    int & knotIDEndOfTheFinishTightenKnot,      
    int & numOfCrossingsOfTheFinishTightenKnot  
)
{
    //    \             /\             /\            /
    //     --         --  --         --  --        --
    //       \       /      \       /      \      /
    //        --   --        --   --       --   --
    //          \ /            \ /           \ / <------ angle
    //    ----------------------0----------------------
    //          / \            / \           / \
    //        --   --        --   --       --   --
    //       /       \      /       \      /      \
    //     --         --  --         --  --       --
    //    /             \/             \/           \
    //
    //
    //

    if ( m_ListOfAnimatedKnot.size() <= 0 ) return -1;

    int returnID = -1;

    //static T offsetAngleDeg = -0.01;
    //angleDeg -= offsetAngleDeg;
    //if ( angleDeg < -90 || angleDeg > 90 )        offsetAngleDeg *= -1;

    // For each animated knot
    int knotNumber = 0;
    for ( std::vector< AnimatedKnot >::iterator knot = m_ListOfAnimatedKnot.begin(); knot != m_ListOfAnimatedKnot.end(); ++knot )
    {

        //SetTheShapeOfAnimatedKnot( *knot );
        //return -1;


        //
        // Setting up the knot
        //
        if ( knot->StepsLeftForSettingKnot > 0 ) {
            --knot->StepsLeftForSettingKnot;
            SetTheShapeOfAnimatedKnot( *knot );
        }
        else {


            //
            // Move the crossing pairs by avaraging
            //

            Quaternion<T> ori_o( 0, 0, 0, 0 );
            Quaternion<T> ori_n( 0, 0, 0, 0 );
            Vector3<T>    pos_o( 0, 0, 0 );
            Vector3<T>    pos_n( 0, 0, 0 );
            Vector3<T>    vel_o( 0, 0, 0 );
            Vector3<T>    vel_n( 0, 0, 0 );
            int div = 0;

            int size = knot->NumOfCrossings * 2;
            int s = knot->KnotIDStart;
            int e = knot->KnotIDEnd - size;
            for ( int i = 0; i <= size; ++i, ++s, ++e ) {
                if ( !(*m_pNodeList)[s].SimFlags.CheckSimulationConstraints( Enum::AddOn::FIXED ) ) {
                    ori_o += (*m_pNodeList)[s].Orientation[*m_pIdxCurr];
                    ori_n += (*m_pNodeList)[s].Orientation[*m_pIdxNext];
                    pos_o += (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetPosition();
                    pos_n += (*m_pNodeList)[s].Centerline[*m_pIdxNext].GetPosition();
                    vel_o += (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetVelocity();
                    vel_n += (*m_pNodeList)[s].Centerline[*m_pIdxNext].GetVelocity();
                    ++div;
                }
                if ( !(*m_pNodeList)[e].SimFlags.CheckSimulationConstraints( Enum::AddOn::FIXED ) ) {
                    ori_o += (*m_pNodeList)[e].Orientation[*m_pIdxCurr];
                    ori_n += (*m_pNodeList)[e].Orientation[*m_pIdxNext];
                    pos_o += (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetPosition();
                    pos_n += (*m_pNodeList)[e].Centerline[*m_pIdxNext].GetPosition();
                    vel_o += (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetVelocity();
                    vel_n += (*m_pNodeList)[e].Centerline[*m_pIdxNext].GetVelocity();
                    ++div;
                }
            }

            // Averaging
            if ( div > 0 ) {
                ori_o *= T(1)/div;
                ori_n *= T(1)/div;
                pos_o /= div;
                pos_n /= div;
                vel_o /= div;
                vel_n /= div;
                Quaternion<T> ori_change = ori_n - ori_o;
                Vector3<T> pos_change = pos_n - pos_o;
                Vector3<T> vel_change = vel_n - vel_o;
                s = knot->KnotIDStart;
                e = knot->KnotIDEnd - size;

                for ( int i = 0; i <= size; ++i, ++s, ++e ) {
                    (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetVelocity( (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetVelocity() + vel_change );
                    (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetPosition() + pos_change );
                    (*m_pNodeList)[s].Orientation[*m_pIdxNext] = (*m_pNodeList)[s].Orientation[*m_pIdxCurr] + ori_change;
                    (*m_pNodeList)[s].Orientation[*m_pIdxNext].Normalized();
                    (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetVelocity( (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetVelocity() + vel_change );
                    (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetPosition() + pos_change );
                    (*m_pNodeList)[e].Orientation[*m_pIdxNext] = (*m_pNodeList)[e].Orientation[*m_pIdxCurr] + ori_change;
                    (*m_pNodeList)[e].Orientation[*m_pIdxNext].Normalized();
                }
            }

            //
            // Tighten the knot
            //
            const T lenThs = 0.5;
            bool bReachStartNodeIDLimit = false;
            bool bReachEndNodeIDLimit = false;
            //int lookAheadIdOffset = size;
            //if ( GetKnotCount() > 0 ) {
            //  lookAheadIdOffset += 2;
            //}
            int lookAheadIdOffset = 1;
            int knotSizeLimit = knot->KnotSizeLimit + (lookAheadIdOffset << 1); // adjust knot size limit by lookAheadOffset*2

            int knotSize = knot->KnotIDEnd - knot->KnotIDStart;

            /*
            // DEBUG
            std::stringstream ss;
            ss << "knotSize: " << knotSize << "; knot->KnotSizeLimit: " << knot->KnotSizeLimit;
            ss << ";  StartD: " << knot->KnotIDStart << "<" << knot->KnotIDStart_Limit;
            ss << ";  EndID: " << knot->KnotIDEnd << ">" << knot->KnotIDEnd_Limit;
            gDebugStr[11] = ss.str();
            //*/

            if ( knot->KnotIDStart < knot->KnotIDStart_Limit - size && knotSize > knotSizeLimit ) {
                int check = knot->KnotIDStart + lookAheadIdOffset;
                if (    ( (*m_pNodeList)[check].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) == false )
                    &&  ( (*m_pNodeList)[check].SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) == false )
                ) {
                    s = knot->KnotIDStart;
                    T lenS_0 = ( (*m_pNodeList)[s-1].Centerline[*m_pIdxCurr].GetPosition() - (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetPosition() ).Length();
                    T lenS_1 = ( (*m_pNodeList)[s+1].Centerline[*m_pIdxCurr].GetPosition() - (*m_pNodeList)[s].Centerline[*m_pIdxCurr].GetPosition() ).Length();
                    if ( lenS_0 - lenS_1 > lenS_1 * lenThs )
                    {
                        ++knot->KnotIDStart;
                        //knot->StepsLeftForSettingKnot = m_pParameters->NumOfNodes * T(0.05);
                        knot->StepsLeftForSettingKnot = 1;
                    }
                }
                else {
                    bReachStartNodeIDLimit = true;
                }
            }
            else {
                bReachStartNodeIDLimit = true;
            }
            //
            //
            //
            if ( knot->KnotIDEnd > knot->KnotIDEnd_Limit + size && knotSize > knotSizeLimit ) {
                int check = knot->KnotIDEnd - lookAheadIdOffset;
                if (    ( (*m_pNodeList)[check].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) == false )
                    &&  ( (*m_pNodeList)[check].SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) == false )
                ) {
                    e = knot->KnotIDEnd;
                    T lenE_0 = ( (*m_pNodeList)[e-1].Centerline[*m_pIdxCurr].GetPosition() - (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetPosition() ).Length();
                    T lenE_1 = ( (*m_pNodeList)[e+1].Centerline[*m_pIdxCurr].GetPosition() - (*m_pNodeList)[e].Centerline[*m_pIdxCurr].GetPosition() ).Length();
                    if ( lenE_1 - lenE_0 > lenE_0 * lenThs ) {
                        --knot->KnotIDEnd;
                        //knot->StepsLeftForSettingKnot = m_pParameters->NumOfNodes * T(0.05);
                        knot->StepsLeftForSettingKnot = 1;
                    }
                }
                else {
                    bReachEndNodeIDLimit = true;
                }
            }
            else {
                bReachEndNodeIDLimit = true;
            }

            //
            // If the knot is tighten, then record the knot.
            //
            if ( bReachStartNodeIDLimit && bReachEndNodeIDLimit ) {
                knot->IsKnotTight = true;

                #ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
                *(knot->pKnotTightenStatus) = true;
                #endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02

                //
                // Rearrange the knot
                //
                if ( GetKnotCount() <= 0 ) {
                    FinalizeTheShapeOfAnimatedKnot( *knot );
                }

                returnID = knot->GetID();
                knotIDStartOfTheFinishTightenKnot = knot->KnotIDStart;
                knotIDEndOfTheFinishTightenKnot = knot->KnotIDEnd;
                numOfCrossingsOfTheFinishTightenKnot = knot->NumOfCrossings;
                knotTypeOfTheFinishTightenKnot = knot->KnotType;

                m_ListOfAnimatedKnot.erase( knot );

                return returnID;

                break;
            }
            else {
                //SetTheShapeOfAnimatedKnot( *knot );
            }
        }
    }
    return returnID;
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodKR<T>::SetTheShapeOfAnimatedKnot ( AnimatedKnot & knot )
{
    // Variables
    const T angleDeg = 30;
    T angle = angleDeg * Math<T>::DEG_TO_RAD;
    T cos_X = cos( angle );
    T sin_Y = sin( angle );
    T dx = cos_X * m_pParameters->LinkLength;
    T dy = sin_Y * m_pParameters->LinkLength;
    T dz = m_pParameters->Radius;
    T dz2 = dz * 2;
    T x, y, z, z2;

    // Set suture's node offsets for creating the knot shape
    x = dx * knot.NumOfCrossings;
    switch ( knot.KnotType ) {
        case Data::DatabaseDowkerNotation::HALF_NEG:
        case Data::DatabaseDowkerNotation::DOUBLE_NEG:
            y = dy;
            z = dz;
            z2 = dz2;
            break;
        case Data::DatabaseDowkerNotation::HALF_POS:
        case Data::DatabaseDowkerNotation::DOUBLE_POS:
            y =  dy;
            z = -dz;
            z2 = -dz2;
            break;
        default:
            y = dy;
            z = dz;
            z2 = dz2;
            break;
    }

    //
    // Calculate translation and orientation for the animated knot
    //
    if ( knot.GetStatusCalKnotPosition() ) {
        knot.Center = (   (*m_pNodeList)[knot.KnotIDStart-1].Centerline[*m_pIdxNext].GetPosition() 
                        + (*m_pNodeList)[knot.KnotIDEnd+1].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
        Vector3<T> direction    = (   (*m_pNodeList)[knot.KnotIDStart-1].Centerline[*m_pIdxNext].GetPosition() 
                                    - (*m_pNodeList)[knot.KnotIDEnd+1].Centerline[*m_pIdxNext].GetPosition() ).GetUnit();
        if ( !knot.IsOrientationFixed ) {
            knot.Orientation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( CGMath<T>::VectorX, direction );
        }
    }

    // Create a knot shape from the constrained links
    int size = knot.NumOfCrossings * 2;
    int s = knot.KnotIDStart;
    int e = knot.KnotIDEnd - size;
    Vector3<T> posS, posE;
    for ( int i = 0; i <= size;  )
    {
        //if ( i > 0 )
        {
            posS = (knot.Orientation * Vector4<T>( x, y, z, 1)).GetVector3() + knot.Center;
            posE = (knot.Orientation * Vector4<T>( x,-y,-z, 1)).GetVector3() + knot.Center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x -= dx;

        if ( i < size )
        {
            posS = (knot.Orientation * Vector4<T>( x, 0, z2, 1)).GetVector3() + knot.Center;
            posE = (knot.Orientation * Vector4<T>( x, 0,-z2, 1)).GetVector3() + knot.Center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x -= dx;
        y = -y;
        z = -z;
        z2 = -z2;

    }
    //knot.Orientation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( CGMath<T>::VectorZ, direction );
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodKR<T>::FinalizeTheShapeOfAnimatedKnot ( AnimatedKnot & knot )
{
    // Variables
    const T angleDeg = 30;
    T angle = angleDeg * Math<T>::DEG_TO_RAD;
    T cos_X = cos( angle );
    T sin_Y = sin( angle );
    T dx = cos_X * m_pParameters->LinkLength;
    T dy = sin_Y * m_pParameters->LinkLength;
    T dz = m_pParameters->Radius;
    T dz2 = dz * 2;
    T x, y, z, z2;

    // Set suture's node offsets for creating the knot shape
    x = dx * knot.NumOfCrossings;
    switch ( knot.KnotType ) {
        case Data::DatabaseDowkerNotation::HALF_NEG:
        case Data::DatabaseDowkerNotation::DOUBLE_NEG:
            y = dy;
            z = dz;
            z2 = dz2;
            break;
        case Data::DatabaseDowkerNotation::HALF_POS:
        case Data::DatabaseDowkerNotation::DOUBLE_POS:
            y =  dy;
            z = -dz;
            z2 = -dz2;
            break;
        default:
            y = dy;
            z = dz;
            z2 = dz2;
            break;
    }

    //
    // Calculate translation and orientation for the animated knot
    //
    if ( knot.GetStatusCalKnotPosition() ) {
        knot.Center = (   (*m_pNodeList)[knot.KnotIDStart-1].Centerline[*m_pIdxNext].GetPosition() 
                        + (*m_pNodeList)[knot.KnotIDEnd+1].Centerline[*m_pIdxNext].GetPosition() ) * T(0.5);
        Vector3<T> direction    = (   (*m_pNodeList)[knot.KnotIDStart-1].Centerline[*m_pIdxNext].GetPosition() 
                                    - (*m_pNodeList)[knot.KnotIDEnd+1].Centerline[*m_pIdxNext].GetPosition() ).GetUnit();
        if ( !knot.IsOrientationFixed ) {
            knot.Orientation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( CGMath<T>::VectorX, direction );
        }
    }

    //
    // Create a knot shape from the constrained links
    //
    int size = knot.NumOfCrossings * 2;
    knot.KnotIDStart = knot.KnotIDStart_Limit - size;
    knot.KnotIDEnd = knot.KnotIDEnd_Limit + size;
    int s = knot.KnotIDStart;
    int e = knot.KnotIDEnd - size;
    Vector3<T> posS, posE;
    for ( int i = 0; i <= size;  )
    {
        //if ( i > 0 )
        {
            posS = (knot.Orientation * Vector4<T>( x, y, z, 1)).GetVector3() + knot.Center;
            posE = (knot.Orientation * Vector4<T>( x,-y,-z, 1)).GetVector3() + knot.Center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x -= dx;

        if ( i < size )
        {
            posS = (knot.Orientation * Vector4<T>( x, 0, z2, 1)).GetVector3() + knot.Center;
            posE = (knot.Orientation * Vector4<T>( x, 0,-z2, 1)).GetVector3() + knot.Center;
            (*m_pNodeList)[s].Centerline[*m_pIdxNext].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxNext].SetPosition( posE );
            (*m_pNodeList)[s].Centerline[*m_pIdxCurr].SetPosition( posS );
            (*m_pNodeList)[e].Centerline[*m_pIdxCurr].SetPosition( posE );
        }
        ++i; ++s; ++e;
        x -= dx;
        y = -y;
        z = -z;
        z2 = -z2;

    }
    //knot.Orientation = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( CGMath<T>::VectorZ, direction );
}
//-----------------------------------------------------------------------------
// Add an animated knot -- used with knot tying by wrapping loops
template <typename T>
bool ElasticRodKR<T>::AddAnimatedKnot (
#ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
    bool * pKnotTightenStatus,                  
#endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02
    int grabbedNodeID,                          
    Data::DatabaseDowkerNotation::KNOT knot,    
    int startNodeIDLimit,                       
    int endNodeIDLimit,                         
    bool bCalKnotPosition,                      
    bool bEndIsGrabbed,                         
    int offset,                                 
    Matrix4x4<T> * pOrientation                 
)
{
    // Clear the previous animated knot
    //DoneAnimatingKnot();

    // The suture tail is grabbed, so check only nodes from the suture head to
    // an offset of number of points from the grabbed point
    if ( bEndIsGrabbed ) {

        // Find the closest node to the grabbed node
        Vector3<T> position = (*m_pNodeList)[grabbedNodeID].Centerline[*m_pIdxCurr].GetPosition();
    #ifndef _WINDOWS_
        //T shortestDist = std::numeric_limits<T>::max();
    #else
        //T shortestDist = Math<T>::MAX;
    #endif
        //T shortestDist = std::numeric_limits<T>::max();
        T shortestDist = 5E32;
        
        int closestNodeID = 1;  // not zero to avoid boundary error, since ConstrainAnimatedKnots fn use this value minus by 1 for indexing
        for ( int i = 0; i < grabbedNodeID - offset; ++i ) {
            Vector3<T> pos2 = (*m_pNodeList)[i].Centerline[*m_pIdxCurr].GetPosition();
            T dist = (position - pos2).Length();
            if ( dist < shortestDist ) {
                shortestDist = dist;
                closestNodeID = i;
            }
            //std::cout << "i" << i << std::endl;
        }

        /*
        // DEBUG
        std::stringstream ss;
        ss << "grabbedNodeID: " << grabbedNodeID;
        ss << " closestNodeID: " << closestNodeID;
        gDebugStr[10] = ss.str();
        //*/

        // Set number of crossing pairs according to the knot type
        int numOfCrossings = 3;
        switch ( knot ) {
        case Data::DatabaseDowkerNotation::HALF_NEG:
            numOfCrossings = 3;
            break;
        case Data::DatabaseDowkerNotation::HALF_POS:
            numOfCrossings = 3;
            break;
        case Data::DatabaseDowkerNotation::DOUBLE_NEG:
            numOfCrossings = 5;
            break;
        case Data::DatabaseDowkerNotation::DOUBLE_POS:
            numOfCrossings = 5;
            break;
        default:
            numOfCrossings = 3;
            break;
        }

        int knotSizeLimit = numOfCrossings*4;

        // The knot could not be added if the knot size is smaller than the knot size limit
        if ( grabbedNodeID - closestNodeID < knotSizeLimit ) return false;

        //std::cout << "closestNodeID: " << closestNodeID << std::endl;

        // Create an animated knot
        m_ListOfAnimatedKnot.push_back( AnimatedKnot() );
        m_ListOfAnimatedKnot.back().SetStatusCalKnotPosition( bCalKnotPosition );

        #ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
            *pKnotTightenStatus = false;
            m_ListOfAnimatedKnot.back().pKnotTightenStatus = pKnotTightenStatus;
        #endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02

        // Set the animated knot's properties
        {
            int idA = closestNodeID;
            int idB = grabbedNodeID;
            int twiceNumOfCrossings = numOfCrossings*2;
            int id_at_half_loop = ( idB - idA - twiceNumOfCrossings ) / 2;
            int knot_tighten_half_length = numOfCrossings * 2;  // *2 for the number of links reserved for half knot loop

            m_ListOfAnimatedKnot.back().KnotIDStart = idA;
            m_ListOfAnimatedKnot.back().KnotIDEnd   = idB;

            m_ListOfAnimatedKnot.back().KnotSizeLimit = knotSizeLimit;
            m_ListOfAnimatedKnot.back().NumOfCrossings = numOfCrossings;
            m_ListOfAnimatedKnot.back().KnotType = knot;

            if ( pOrientation ) {
                m_ListOfAnimatedKnot.back().IsOrientationFixed = true;
                m_ListOfAnimatedKnot.back().Orientation = *pOrientation;
            }

            // Set the start limit id
            if ( startNodeIDLimit >= 0 ) {
                //if ( idA <= startNodeIDLimit && startNodeIDLimit <= idB ) {
                    m_ListOfAnimatedKnot.back().KnotIDStart_Limit = startNodeIDLimit;
                //}
                //else {
                //}
            }
            else {
                m_ListOfAnimatedKnot.back().KnotIDStart_Limit = id_at_half_loop - knot_tighten_half_length;
            }
            
            // Set the end limit id
            if ( endNodeIDLimit >= 0 ) {
                //if ( idA <= endNodeIDLimit && endNodeIDLimit <= idB ) {
                    m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = endNodeIDLimit;
                //}
                //else {
                //}
            }
            else {
                m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = id_at_half_loop + knot_tighten_half_length;
            }

            // Adjust the start and end limit ids due to the existing locked knots
            for ( int i = 0; i < m_ucKnotCount; ++i ) {
                int sid = m_pListOfLockedKnots[i].StartLinkID - twiceNumOfCrossings;
                int eid = m_pListOfLockedKnots[i].EndLinkID   + twiceNumOfCrossings;
                if ( m_ListOfAnimatedKnot.back().KnotIDStart <= sid && sid < m_ListOfAnimatedKnot.back().KnotIDStart_Limit ) {
                    m_ListOfAnimatedKnot.back().KnotIDStart_Limit = sid;
                }
                if ( m_ListOfAnimatedKnot.back().KnotIDEnd_Limit < sid && sid <= m_ListOfAnimatedKnot.back().KnotIDEnd ) {
                    m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = eid;
                }
            }
            // Set the number of simulation loops for setting up the knot
            //m_ListOfAnimatedKnot.back().StepsLeftForSettingKnot = m_pParameters->NumOfNodes;
            m_ListOfAnimatedKnot.back().StepsLeftForSettingKnot = 1;

            //*
            // DEBUG
            {
                std::stringstream ss;
                ss << "after add, start--end IDs: " << m_ListOfAnimatedKnot.back().KnotIDStart 
                    << "--" << m_ListOfAnimatedKnot.back().KnotIDEnd  
                    << " : (" << m_ListOfAnimatedKnot.back().KnotIDStart_Limit 
                    << ", " << m_ListOfAnimatedKnot.back().KnotIDEnd_Limit << ")"
                    << "; knot size limit: " << m_ListOfAnimatedKnot.back().KnotSizeLimit;
                gDebugStr[9] = ss.str();
            }
            //*/
        }
    }
    else {
        std::cout << __FILE__ << ":" << __LINE__ << ": (!bEndIsGrabbed) does not implemented/supported!" << std::endl;
    }
    return true;
} //END: Add an animated knot -- used with knot tying by wrapping loops

//-----------------------------------------------------------------------------
// Add an animated knot -- used with knot tying by Tyco Endo Stitch Device
template <typename T>
bool ElasticRodKR<T>::AddAnimatedKnot (
#ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
    bool * pKnotTightenStatus,                  
#endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02
    Data::DatabaseDowkerNotation::KNOT knot,    
    int startNodeIDLimit,                       
    int startLoopNodeIDLimit,                   
    int endLoopNodeIDLimit,                     
    int endNodeIDLimit,                         
    bool bCalKnotPosition,                      
    bool bEndIsGrabbed,                         
    Matrix4x4<T> * pOrientation                 
)
{
    if      ( startNodeIDLimit < 0 )    startNodeIDLimit = 1;
    else if ( endNodeIDLimit < 0  )     endNodeIDLimit = m_pParameters->NumOfNodes - 2;

    // The suture tail is grabbed
    if ( bEndIsGrabbed ) {

        // Set number of crossing pairs according to the knot type
        int numOfCrossings = 3;
        switch ( knot ) {
        case Data::DatabaseDowkerNotation::HALF_NEG:
            numOfCrossings = 3;
            break;
        case Data::DatabaseDowkerNotation::HALF_POS:
            numOfCrossings = 3;
            break;
        case Data::DatabaseDowkerNotation::DOUBLE_NEG:
            numOfCrossings = 5;
            break;
        case Data::DatabaseDowkerNotation::DOUBLE_POS:
            numOfCrossings = 5;
            break;
        default:
            numOfCrossings = 3;
            break;
        }

        int knotSizeLimit = numOfCrossings*4;

        //knotSizeLimit = numOfCrossings;

        // The knot could not be added if the knot size is smaller than the knot size limit
        if ( endNodeIDLimit - startNodeIDLimit < knotSizeLimit ) return false;

        //std::cout << "closestNodeID: " << closestNodeID << std::endl;

        // Create an animated knot
        m_ListOfAnimatedKnot.push_back( AnimatedKnot() );
        m_ListOfAnimatedKnot.back().SetStatusCalKnotPosition( bCalKnotPosition );

        #ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
            *pKnotTightenStatus = false;
            m_ListOfAnimatedKnot.back().pKnotTightenStatus = pKnotTightenStatus;
        #endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02

        // Set the animated knot's properties
        {
            int idA = startNodeIDLimit;
            int idB = endNodeIDLimit;
            int twiceNumOfCrossings = numOfCrossings*2;
            int id_at_half_loop = ( idB - idA - twiceNumOfCrossings ) / 2;
            int knot_tighten_half_length = numOfCrossings * 2;  // *2 for the number of links reserved for half knot loop

            m_ListOfAnimatedKnot.back().KnotIDStart = idA;
            m_ListOfAnimatedKnot.back().KnotIDEnd   = idB;

            m_ListOfAnimatedKnot.back().NumOfCrossings = numOfCrossings;
            m_ListOfAnimatedKnot.back().KnotType = knot;

            if ( pOrientation ) {
                m_ListOfAnimatedKnot.back().IsOrientationFixed = true;
                m_ListOfAnimatedKnot.back().Orientation = *pOrientation;
            }

            //
            // Set the start limit id, end limit id, and knot size limit
            //
            if ( startLoopNodeIDLimit < 0 || endLoopNodeIDLimit < 0 || ( startLoopNodeIDLimit >= endLoopNodeIDLimit ) ) {
                m_ListOfAnimatedKnot.back().KnotIDStart_Limit = startNodeIDLimit;
                m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = endNodeIDLimit;
                m_ListOfAnimatedKnot.back().KnotSizeLimit = knotSizeLimit;
            }
            else {
                m_ListOfAnimatedKnot.back().KnotIDStart_Limit = startLoopNodeIDLimit;
                m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = endLoopNodeIDLimit;
                m_ListOfAnimatedKnot.back().KnotSizeLimit = endLoopNodeIDLimit - startLoopNodeIDLimit + 1;
            }

            // Adjust the start and end limit ids due to the existing locked knots
            for ( int i = 0; i < m_ucKnotCount; ++i ) {
                int sid = m_pListOfLockedKnots[i].StartLinkID - twiceNumOfCrossings;
                int eid = m_pListOfLockedKnots[i].EndLinkID   + twiceNumOfCrossings;
                if ( m_ListOfAnimatedKnot.back().KnotIDStart <= sid && sid < m_ListOfAnimatedKnot.back().KnotIDStart_Limit ) {
                    m_ListOfAnimatedKnot.back().KnotIDStart_Limit = sid;
                }
                if ( m_ListOfAnimatedKnot.back().KnotIDEnd_Limit < sid && sid <= m_ListOfAnimatedKnot.back().KnotIDEnd ) {
                    m_ListOfAnimatedKnot.back().KnotIDEnd_Limit = eid;
                }
            }
            // Set the number of simulation loops for setting up the knot
            //m_ListOfAnimatedKnot.back().StepsLeftForSettingKnot = m_pParameters->NumOfNodes;
            m_ListOfAnimatedKnot.back().StepsLeftForSettingKnot = 1;

            //*
            // DEBUG
            {
                std::stringstream ss;
                ss << "after add, start--end IDs: " << m_ListOfAnimatedKnot.back().KnotIDStart 
                    << "--" << m_ListOfAnimatedKnot.back().KnotIDEnd  
                    << " : (" << m_ListOfAnimatedKnot.back().KnotIDStart_Limit 
                    << ", " << m_ListOfAnimatedKnot.back().KnotIDEnd_Limit << ")"
                    << "; knot size limit: " << m_ListOfAnimatedKnot.back().KnotSizeLimit;
                gDebugStr[9] = ss.str();
            }
            //*/
        }
    }
    else {
        std::cout << __FILE__ << ":" << __LINE__ << ": (!bEndIsGrabbed) does not implemented/supported!" << std::endl;
    }
    return true;
} //END: Add an animated knot -- used with knot tying by Tyco Endo Stitch Device
//-----------------------------------------------------------------------------
#endif//TAPs_ADD_ANIMATED_KNOTS
// END: Animating Knot Tying
//=============================================================================




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

    //std::cout << "KR -> Draw\n";

    /*
    // Draw animated knot info
    {
        for ( unsigned int k = 0; k < m_ListOfAnimatedKnot.size(); ++k ) {

            int S = m_ListOfAnimatedKnot[k].KnotIDStart;
            int E = m_ListOfAnimatedKnot[k].KnotIDEnd;

            // Draw points
            glPointSize( 10 );
            glDisable( GL_DEPTH_TEST );
            glDisable( GL_LIGHTING );
            glBegin( GL_POINTS );

            for ( int i = 0; i <= m_ListOfAnimatedKnot[k].NumOfCrossings*2; ++i ) {
                glColor3f( 1, 0, 0 );
                glVertex3fv( (*m_pNodeList)[S+i].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
                glColor3f( 0, 0, 1 );
                glVertex3fv( (*m_pNodeList)[E-i].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );

                //std::cout << "S: " << S+i << "\n";
                //std::cout << "E: " << E-i << "\n";
            }
            glEnd();

            // Draw link lines
            glColor3f( 0, 0, 1 );
            glLineWidth( 5 );
            glBegin( GL_LINE_STRIP );
            for ( int i = 0; i <= m_ListOfAnimatedKnot[k].NumOfCrossings*2; ++i ) {
                glColor3f( 1, 0, 0 );
                glVertex3fv( (*m_pNodeList)[S+i].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
            }
            glEnd();
            glBegin( GL_LINE_STRIP );
            for ( int i = 0; i <= m_ListOfAnimatedKnot[k].NumOfCrossings*2; ++i ) {
                glColor3f( 0, 0, 1 );
                glVertex3fv( (*m_pNodeList)[E-i].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
            }
            glEnd();
        }
    }
    //*/

    /*
    // Draw animated knot info
    {
        // Draw points
        glPointSize( 5 );
        glBegin( GL_POINTS );
        for ( int i = 0; i < m_AnimatedKnot.NumOfPairs; ++i ) {
            glColor3f( 1, 0, 0 );
            glVertex3fv( (*m_pNodeList)[m_AnimatedKnot.ListANodes[i]].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
            glColor3f( 0, 0, 1 );
            glVertex3fv( (*m_pNodeList)[m_AnimatedKnot.ListBNodes[i]].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
        }
        glEnd();

        // Draw link lines
        glBegin( GL_LINES );
        glColor3f( 0, 0, 1 );
        glLineWidth(3);
        for ( int i = 0; i < m_AnimatedKnot.NumOfPairs; ++i ) {
            glColor3f( 1, 0, 0 );
            glVertex3fv( (*m_pNodeList)[m_AnimatedKnot.ListANodes[i]].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
            glColor3f( 0, 0, 1 );
            glVertex3fv( (*m_pNodeList)[m_AnimatedKnot.ListBNodes[i]].Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
        }
        glEnd();
    }
    //*/

    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T>
void ElasticRodKR<T>::DrawForDebug () const
{
    //if ( m_BVHTree ) {
    //  m_BVHTree->Draw();
    //}

    Vector3<T> direction = GetProjDirection();

    // Draw the three points for fiding a projection direction
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glPushMatrix();
    glEnable( GL_COLOR_MATERIAL );
    glColor3f( 0, 1, 0 );
    glPointSize( 25 );
    glBegin( GL_LINE_LOOP );
        glVertex3fv( m_NodesForProjPoints[0]->Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
        glVertex3fv( m_NodesForProjPoints[1]->Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
        glVertex3fv( m_NodesForProjPoints[2]->Centerline[*m_pIdxCurr].GetPosition().GetDataFloat() );
    glEnd();
    glTranslatef( 
        m_NodesForProjPoints[1]->Centerline[*m_pIdxCurr].GetPosition()[0],
        m_NodesForProjPoints[1]->Centerline[*m_pIdxCurr].GetPosition()[1],
        m_NodesForProjPoints[1]->Centerline[*m_pIdxCurr].GetPosition()[2]
    );
    m_pOGLUsefulObj->DrawOneHeadArrow( direction, 0.2 );
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
//=============================================================================
#endif // #if defined(__gl_h_) || defined(__GL_H__)

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