TAPsModelElasticRod.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsModelElasticRod.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (07/07/2009)
UPDATE          (03/21/2011)
******************************************************************************/
#include "TAPsModelElasticRod.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
//=============================================================================
template <typename T> int   ModelElasticRod<T>::g_NumOfElasticRods = 0;
//template <typename T> bool    ModelElasticRod<T>::g_IsGLSLInit = false;
//-----------------------------------------------------------------------------
// Constructor
template <typename T>
ModelElasticRod<T>::ModelElasticRod (
    int iNoLinks,               // # of links/segments
    T tRadius,                  // radius
    T tTotalLength,             // total length
    T tTotalMass,               // total mass
    Vector3<T> & posOfVertex0,  // position of the 1st node
    ShapeInitializationParameters ShapeParameters   // initialization parameters for shape

#ifdef  TAPs_USE_CUDA
    , bool  bUseCUDAsPLHM       // utilizing CUDA's page-locked host memory
#endif//TAPs_USE_CUDA
) :
    IdxCurr(0), IdxPrev(1), IdxNext(IdxPrev),   // for data accessing
    m_vStartPos( posOfVertex0 ),                // the position of the first point

    // For drawing by OpenGL
#if defined(__gl_h_) || defined(__GL_H__)
    m_GLVertexArray( 0 ),
    m_drawNV( 8 ),
    m_drawCrossSectionVertices( NULL ),
    m_drawCrossSectionNormals( NULL ),
    m_drawCrossSectionTexCoords( NULL ),
    vertices( NULL ),
    normals( NULL ),
    texCoords( NULL ),
    sVertices( NULL ),
    m_pVertexData( NULL )
#endif//defined(__gl_h_) || defined(__GL_H__)

{
    m_Parameters.NumOfNodes     = iNoLinks+1;
    m_Parameters.Radius         = tRadius;
    m_Parameters.TotalLength    = tTotalLength;
    m_Parameters.TotalMass      = tTotalMass;

    m_Parameters.Kstretch_modulus   = 5000;
    m_Parameters.Kbend_modulus      = 250;
    m_Parameters.Kshear_modulus     = 250;

    m_Parameters.Ps=1;
    m_Parameters.Pb[0]=m_Parameters.Pb[1]=m_Parameters.Pb[2]=0;
    m_Parameters.Kt=0;
    m_Parameters.Kr[0]=m_Parameters.Kr[1]=m_Parameters.Kr[2]=0;
    m_Parameters.Dt=0;
    m_Parameters.Dr[0]=m_Parameters.Dr[1]=m_Parameters.Dr[2]=0;
    m_Parameters.Kconstraint_3rdDirAlignTangent=0.5;
    m_Parameters.Kvdamping=0.9;
    m_Parameters.MaterialDensity = m_Parameters.TotalMass / ( m_Parameters.Radius * m_Parameters.Radius * Math<T>::PI * m_Parameters.TotalLength );

    m_Parameters.Kgravity       = -9.81;
#ifdef  TAPs_ADD_RESTING_LEVEL_Y
    m_Parameters.RestingLevel   = -1000.0;
#endif//TAPs_ADD_RESTING_LEVEL_Y

    // For collision detection
    m_Parameters.KselfCDR = 0.15;
    m_Parameters.KselfStick = 0.05;
    m_Parameters.K_CDRwBV_Cylinder  = 1;
    m_Parameters.K_CDRwBV_Sphere    = 0.01;
    m_Parameters.K_CDRwTriangle     = 1;
    // Offsets
    m_Parameters.Offset_CD_Self         = 0;//m_Parameters.Radius/2;
    m_Parameters.Offset_CD_Triangle     = m_Parameters.Radius/2;
    m_Parameters.Offset_CD_BV_Sphere    = m_Parameters.Radius/2;
    m_Parameters.Offset_CD_BV_Cylinder  = m_Parameters.Radius/2;

    // For collision detection with implicit objects
    m_Parameters.K_CDRwImpObj_Sphere    = 1;
    m_Parameters.K_CDRwImpObj_Torus     = 1;
    m_Parameters.Offset_CD_ImpObj_Sphere    = m_Parameters.Radius/2;
    m_Parameters.Offset_CD_ImpObj_Torus     = m_Parameters.Radius/2;
    
    SetMaterialProperties( m_Parameters.Kstretch_modulus, m_Parameters.Kbend_modulus, m_Parameters.Kshear_modulus, m_Parameters.MaterialDensity );

    #if defined(__gl_h_) || defined(__GL_H__)
        InitGL();
    #endif

    // Initialize the elastic rod's configuration
    InitShapeParameters = ShapeParameters;
    Initialize( InitShapeParameters.StartShape );

    // Initialize collision detection unit
    m_CD.CreateCD( &IdxCurr, &IdxNext, &m_Parameters, &m_Nodes );

    // Initialize CUDA for computation
    #ifdef  TAPs_USE_CUDA
    try {
        //m_Parameters.EnableCUDA = true;
        m_CUDA.CreateCUDA( &IdxCurr, &IdxPrev, &IdxNext, &m_Parameters, &m_Nodes, bUseCUDAsPLHM );
    }
    catch ( char * e ) {
        std::cout << e << std::endl;
        exit(-1);
    }
    #endif//TAPs_USE_CUDA

    // Initialize knot recognition unit
    #ifdef  TAPs_ADD_KNOT_RECOGNITION
        unsigned char numOfKnotAllowed = 8;
        m_KR.CreateKR( &IdxCurr, &IdxNext, &m_Parameters, &m_Nodes, m_CD.GetBVHTree(), numOfKnotAllowed );
    #endif//TAPs_ADD_KNOT_RECOGNITION

    // Assign an ID for this elastic rod and count the number of created elastic rods
    m_ID = g_NumOfElasticRods++;


    // Set all point to slidable
    for ( int i = 0; i < GetNumberOfPoints(); ++i ) {
        GetListOfNodes()[i].SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::SLIDABLE );
    }
}


// Destructor
template <typename T>
ModelElasticRod<T>::~ModelElasticRod ()
{
    #if defined(__gl_h_) || defined(__GL_H__)
        ClearGL();
    #endif
}


// Destructor
template <typename T>
ModelElasticRod<T> * ModelElasticRod<T>::CopySegment ( int startLink, int endLink )
{
    if ( startLink > endLink )  return NULL;
    if ( startLink < 0  )               startLink = 0;
    if ( endLink > GetNumberOfLinks()-1 )   endLink = GetNumberOfLinks()-1;

    int iNoLinks = endLink - startLink + 1;
    T ratio = static_cast<T>( iNoLinks ) / GetNumberOfLinks();
    T tTotalLength  = GetTotalLength() / ratio;
    T tTotalMass    = GetTotalMass() / ratio;
    ModelElasticRod<T> * pER = new  ModelElasticRod(
                                        iNoLinks,
                                        GetRadius(),
                                        tTotalLength,
                                        tTotalMass
                                    );

    // Copy the parameter properties
    pER->m_Parameters = m_Parameters;
    pER->m_Parameters.NumOfNodes    = iNoLinks + 1;
    pER->m_Parameters.TotalLength   = tTotalLength;
    pER->m_Parameters.TotalMass     = tTotalMass;

    // Copy the shape and simulation properties
    SetOfElasticRodNodes::const_iterator orig = GetListOfNodes().begin();
    SetOfElasticRodNodes::iterator dest = pER->GetListOfNodes().begin();
    int c = 0;
    while ( c < startLink ) {
        ++orig;
        ++c;
    }

    for ( ; c <= endLink+1; ++c ) {
        *dest = *orig;
        ++orig;
        ++dest;
    }

    return pER;
}


// Return this object info as a string
template <typename T>
std::string ModelElasticRod<T>::StrInfo () const
{
    std::stringstream output;
    output  <<  "Pointer: " << this << "\t"
            <<  "TAPs::ModelElasticRod<"
            <<  typeid(T).name() << "> Class:";
    //-------------------------------------------------
    output  << "\n\tLength: " << m_Parameters.TotalLength
            << "\n\tRadius: " << m_Parameters.Radius
            << "\n\tMass:   " << m_Parameters.TotalMass
            << "\n\tSimulation Info:"
            << "\n\t\tnumber of Links is " << m_Parameters.NumOfNodes-1 << " where each link's length is " << m_Parameters.LinkLength
            << "\n\t\tstretch modulus (Es): " << m_Parameters.Kstretch_modulus
            << "\n\t\tbend modulus (Eb):    " << m_Parameters.Kbend_modulus
            << "\n\t\tshear modulus (G):    " << m_Parameters.Kshear_modulus
            << "\n\t\tmaterial density:     " << m_Parameters.MaterialDensity
            << "\n\t\tpotential stretch constant (Ps): " << m_Parameters.Ps << " (Ps = Es*pi*r^2)"
            << "\n\t\tpotential bend constant (Pb):    " << m_Parameters.Pb[0] << "  " << m_Parameters.Pb[1] << "  " << m_Parameters.Pb[2] 
                        << " (Pb[0] = Pb[1] = Eb*pi*r^2*0.25; Pb[2] = G*pi*r^2*0.5)"
            << "\n\t\tkinetic translational constant (Kt): " << m_Parameters.Kt
            << "\n\t\tkinetic rotational constant (Kr): " << m_Parameters.Kr[0] << "  " << m_Parameters.Kr[1] << "  " << m_Parameters.Kr[2]
            << "\n\t\ttranslational dissipation constant (Dt): " << m_Parameters.Dt
            << "\n\t\trotational dissipation constant (Dr): " << m_Parameters.Dr[0] << "  " << m_Parameters.Dr[1] << "  " << m_Parameters.Dr[2]
            << "\n\t\tconstaint constant (Kconstraint_3rdDirAlignTangent): " << m_Parameters.Kconstraint_3rdDirAlignTangent
            << "\n\t\tvelocity damping constant (Kvdamping): " << m_Parameters.Kvdamping
            << "\n";
    
    return output.str();
}

// Get the stretch Young's modulus
template <typename T>
T ModelElasticRod<T>::GetStretchModulus () const
{
    return m_Parameters.Kstretch_modulus;
}

// Set the stretch Young's modulus
template <typename T>
void ModelElasticRod<T>::SetStretchModulus ( T k )
{
    m_Parameters.Kstretch_modulus = k;
    m_Parameters.Ps = m_Parameters.Kstretch_modulus * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius;
}

// Get the bend Young's modulus
template <typename T>
T ModelElasticRod<T>::GetBendModulus () const
{
    return m_Parameters.Kbend_modulus;
}

// Set the bend Young's modulus
template <typename T>
void ModelElasticRod<T>::SetBendModulus ( T k )
{
    m_Parameters.Kbend_modulus = k;
    m_Parameters.Pb[0] = m_Parameters.Pb[1] = m_Parameters.Kbend_modulus * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius * 0.25;
}

// Get the shear modulus
template <typename T>
T ModelElasticRod<T>::GetShearModulus () const
{
    return m_Parameters.Kshear_modulus;
}

// Set the shear modulus
template <typename T>
void ModelElasticRod<T>::SetShearModulus ( T k )
{
    m_Parameters.Kshear_modulus = k;
    m_Parameters.Pb[2] = m_Parameters.Kshear_modulus * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius * 0.5;
}

// Get the material density
template <typename T>
T ModelElasticRod<T>::GetMaterialDensity () const
{
    return m_Parameters.MaterialDensity;
}

// Set the material density
template <typename T>
void ModelElasticRod<T>::SetMaterialDensity ( T material_density )
{
    m_Parameters.MaterialDensity = material_density;
}

// Set the elastic rod's material properties
template <typename T>
void ModelElasticRod<T>::SetMaterialProperties ( 
    T stretch_modulus, T bend_modulus, T shear_modulus, T material_density )
{
    SetStretchModulus( stretch_modulus );
    SetBendModulus( bend_modulus );
    SetShearModulus( shear_modulus );
    SetMaterialDensity( material_density );
}

// Initialize
template <typename T>
void ModelElasticRod<T>::Initialize ( enum ShapeInitializationParameters::Shape shape ) throw(...)
{
    // Set the number of elastic rod nodes
    try {
        m_Nodes.resize( m_Parameters.NumOfNodes );
    }
    catch ( std::bad_alloc & e ) {
        std::cerr << StrInfo() << " couldn't allocate the elastic rod of node size: " << m_Parameters.NumOfNodes 
            << "[" << e.what() << "]" << std::endl;
        throw( e );
    }

    InitializeElasticRodNodes( shape );
}

// Initialize both centerlines and orientations
template <typename T>
void ModelElasticRod<T>::InitializeElasticRodNodes ( enum ShapeInitializationParameters::Shape shape )
{
    m_Parameters.MassOfPoint = m_Parameters.TotalMass / (m_Parameters.NumOfNodes);
    m_Parameters.LinkLength  = m_Parameters.TotalLength / (m_Parameters.NumOfNodes-1); 

    //Vector3<T> position = m_vStartPos;

    //
    // SET A STRAIGHT LINE -- for centerlines
    //
    if ( shape == ShapeInitializationParameters::STRAIGHT_SHAPE ) {
        unsigned int id = 0;
        Vector3<T> position( m_vStartPos );
        //Vector3<T> offsetPos( m_vStartPos - position );
        Vector3<T> offset( -m_Parameters.LinkLength, 0, 0 );

        //position += offset;   // need this, otherwise runtime error with "w.exe > t.txt"

        SetOfElasticRodNodes::iterator it = m_Nodes.begin();
        while ( it != m_Nodes.end() ) {
            it->ID = id++;  // assign id to the node
            it->Centerline[IdxCurr].SetMass( m_Parameters.MassOfPoint );
            it->Centerline[IdxCurr].SetSize( m_Parameters.Radius );
            it->Centerline[IdxCurr].SetPosition( position );

            it->CenterlineRestLength = m_Parameters.LinkLength;

            //std::cout << "it->CenterlineRestLength: " << it->CenterlineRestLength << "\n";

            position += offset;
            it->Centerline[IdxPrev] = it->Centerline[IdxCurr];  // Initialize the previous centerline
            ++it;
        }
    }
    //
    // SET A HELIX -- for centerlines
    //
    else if ( shape == ShapeInitializationParameters::HELIX_SHAPE ) {
        unsigned int id = 0;
        T a = InitShapeParameters.Helix_Radius; // helix radius
        T b = InitShapeParameters.Helix_Pitch;  // pitch := 2*pi*b
        T t = 0.0;
        Vector3<T> position( a*cos(t), a*sin(t), b*t );
        Vector3<T> offsetPos( m_vStartPos - position );
        position += offsetPos;

        // Find the parameter value that make the next point close to the link length
        T errThreshold = 1E-6;
        T linkLenMax = m_Parameters.LinkLength + errThreshold;
        T linkLenMin = m_Parameters.LinkLength - errThreshold;
        T prevMaxS = 100;
        T prevMinS = 0;
        T s;

        //std::cout << "linkLenMax: " << linkLenMax << "\n";
        //std::cout << "linkLenMin: " << linkLenMin << "\n";

        T closestS = 0;
        //T err = Math<T>::MAX;
        //T err = std::numeric_limits<T>::max();
        T err = 5E32;

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

        bool bFound = false;
        int count = 100;
        Vector3<T> helixOrg( a, 0, 0 );
        do {
            s = ( prevMaxS - prevMinS )*0.5 + prevMinS;
            Vector3<T> point( a*cos(s), a*sin(s), b*s );
            T length = ( point - helixOrg ).Length();

            //std::cout << "prevMaxS, s, prevMinS: " << prevMaxS << "\t" << s << "\t" << prevMinS << "\n";
            //std::cout << "length: " << length << "\n";

            if      ( length < linkLenMin ) prevMinS = s;
            else if ( length > linkLenMax ) prevMaxS = s;
            else bFound = true;

            T newErr = fabs( length - m_Parameters.LinkLength );
            if ( newErr < err ) {
                err = newErr;
                closestS = s;
            }

        } while ( !bFound && --count > 0 );

        if ( !bFound )  s = closestS;

        SetOfElasticRodNodes::iterator it = m_Nodes.begin();
        while ( it != m_Nodes.end() ) {
            it->ID = id++;  // assign id to the node
            it->Centerline[IdxCurr].SetMass( m_Parameters.MassOfPoint );
            it->Centerline[IdxCurr].SetSize( m_Parameters.Radius );
            it->Centerline[IdxCurr].SetPosition( position );

            // next position
            //t += it->CenterlineRestLength;

            t += s;
            Vector3<T> next( a*cos(t), a*sin(t), b*t );
            //next.Normalized();
            //next *= it->CenterlineRestLength * ++count;

            it->CenterlineRestLength = (next + offsetPos - position).Length();

            position = next + offsetPos;

            //std::cout << "id: " << id-1 << "\t" << "it->CenterlineRestLength: " << it->CenterlineRestLength << "\n";

            it->Centerline[IdxPrev] = it->Centerline[IdxCurr];  // Initialize the previous centerline
            ++it;
        }
    }
    //*/


    // Set orientation
    SetOfElasticRodNodes::iterator it = m_Nodes.begin();
    for ( int i = 1; i < m_Parameters.NumOfNodes; ++i ) {
        // FIGURE IT OUT!!!
        // The orientation must have d3 points in the direction of 
        // Centerline_next - Centerline_current

        if ( shape == ShapeInitializationParameters::STRAIGHT_SHAPE ) {
            // SET A STRAIGHT LINE
            it->Orientation[IdxCurr].SetRIJK( 0, 1, 0, -1 );    // results in d3 is <-1,0,0>
        }
        else if ( shape == ShapeInitializationParameters::HELIX_SHAPE ) {
            // SET A HELIX
            Vector3<T> ptA = it->Centerline[IdxCurr].GetPosition();
            ++it;
            Vector3<T> ptB = it->Centerline[IdxCurr].GetPosition();
            --it;
            Matrix3x3<T> rotMat = CGMath<T>::CreateRotationMatrix3x3FromVectorAtoVectorB( ptA, ptB );
            it->Orientation[IdxCurr].SetByRotationMatrix3x3( rotMat );
        }
        it->Orientation[IdxCurr].Normalized();  // Initialize the previous orientation
        it->Orientation[IdxPrev] = it->Orientation[IdxCurr];
        ++it;
    }

    ComputeAndStoreCenterlineRestLengths();
    ComputeAndStoreOrientationRestLengths();

    //*
    it = m_Nodes.begin();
    // Compute the intrinsic strain rate of the orientations (except the last node)
    for ( int i = 1; i < m_Parameters.NumOfNodes-1; ++i ) {
        ComputeIntrinsicStrainRate( it );
        ++it;
    }
    //*/
}

// Switch data accessing (previous becomes current and current becomes previous)
template <typename T>
void ModelElasticRod<T>::SwitchDataAccessing ()
{
    int tmp = IdxCurr;
    IdxCurr = IdxPrev;
    IdxPrev = tmp;
}

// Compute and store the rest lengths of the orientation segments
template <typename T>
void ModelElasticRod<T>::ComputeAndStoreCenterlineRestLengths ()
{
    // CenterlineRestLength = (currCenterline - prevCenterline).Length()
    SetOfElasticRodNodes::iterator currNode = m_Nodes.begin();
    SetOfElasticRodNodes::iterator nextNode = currNode;
    ++nextNode;

    while ( nextNode != m_Nodes.end() ) {
        currNode->CenterlineRestLength = ComputeLengthOfCenterlineSegment( 
            currNode->Centerline[IdxCurr], nextNode->Centerline[IdxCurr] );
        ++currNode;
        ++nextNode;
    }
}

// Compute and store the rest lengths of the orientation segments
template <typename T>
void ModelElasticRod<T>::ComputeAndStoreOrientationRestLengths ()
{
    SetOfElasticRodNodes::iterator currNode = m_Nodes.begin();
    SetOfElasticRodNodes::iterator nextNode = currNode;
    ++nextNode;
    SetOfElasticRodNodes::iterator nextOfNextNode = nextNode;
    ++nextOfNextNode;

    while ( nextOfNextNode != m_Nodes.end() ) {
        currNode->OrientationRestLength = ComputeLengthOfOrientationSegment( 
            currNode->Centerline[IdxCurr], nextNode->Centerline[IdxCurr], nextOfNextNode->Centerline[IdxCurr] );
        ++currNode;
        ++nextNode;
        ++nextOfNextNode;
    }
}

// Reset
template <typename T>
void ModelElasticRod<T>::Reset ()
{
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        currNode->Reset();
        ++currNode;
    }

    Initialize( InitShapeParameters.StartShape );

    m_CD.Reset();

    #ifdef  TAPs_ADD_KNOT_RECOGNITION
        m_KR.Reset();
    #endif//TAPs_ADD_KNOT_RECOGNITION
}

// Get the position of the start point
template <typename T>
Vector3<T> ModelElasticRod<T>::GetPosOfStartPt ()
{
    return m_Nodes.front().Centerline[IdxCurr].GetPosition();
}

// Get the position of the end point
template <typename T>
Vector3<T> ModelElasticRod<T>::GetPosOfEndPt ()
{
    return m_Nodes.back().Centerline[IdxCurr].GetPosition();
}

// Set the position of the start point
template <typename T>
void ModelElasticRod<T>::SetPosOfStartPt ( Vector3<T> const & pos, Quaternion<T> const & ori )
{
    m_Nodes.front().Centerline[IdxPrev].SetPosition( m_Nodes.front().Centerline[IdxCurr].GetPosition() );
    m_Nodes.front().Centerline[IdxCurr].SetPosition( pos );
}

// Set the position of the end point
template <typename T>
void ModelElasticRod<T>::SetPosOfEndPt ( Vector3<T> const & pos, Quaternion<T> const & ori )
{
    m_Nodes.back().Centerline[IdxPrev].SetPosition( m_Nodes.back().Centerline[IdxCurr].GetPosition() );
    m_Nodes.back().Centerline[IdxCurr].SetPosition( pos );
}

// Move the position of the start point
template <typename T>
void ModelElasticRod<T>::MovePosOfStartPt ( Vector3<T> const & pos, Quaternion<T> const & ori )
{
    m_Nodes.front().Centerline[IdxPrev].SetPosition( m_Nodes.front().Centerline[IdxCurr].GetPosition() );
    m_Nodes.front().Centerline[IdxCurr].SetPosition( pos + m_Nodes.front().Centerline[IdxCurr].GetPosition() );
}

// Move the position of the end point
template <typename T>
void ModelElasticRod<T>::MovePosOfEndPt ( Vector3<T> const & pos, Quaternion<T> const & ori )
{
    m_Nodes.back().Centerline[IdxPrev].SetPosition( m_Nodes.back().Centerline[IdxCurr].GetPosition() );
    m_Nodes.back().Centerline[IdxCurr].SetPosition( pos + m_Nodes.back().Centerline[IdxCurr].GetPosition() );
}

// Clear external forces
template <typename T>
void ModelElasticRod<T>::ClearExternalForces ()
{
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        (currNode->ExternalForce).Clear();
        ++currNode;
    }
}

// Clear external forces
template <typename T>
void ModelElasticRod<T>::ClearExternalForces_Reserved ()
{
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        (currNode->ExternalForce_Reserved).Clear();
        ++currNode;
    }
}

// Collision Detection and Response with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ModelElasticRod<T>::CDRwith (
    MultiBoundingVolume<T> * const pMBVObj,     
    TransformationSupport<T> * const pTransform 
)
{
    return m_CD.CDRwith( pMBVObj, pTransform );
}

// Collision Detection and Response with an MBV (Multi-Bounding Volume) object
template <typename T>
bool ModelElasticRod<T>::CDRwith (
    TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pObj,    
    TransformationSupport<T> * const pTransform             
)
{
    return m_CD.CDRwith( pObj, pTransform );
}

// Clear internal forces
template <typename T>
void ModelElasticRod<T>::ClearInternalForces ()
{
    //std::cout << "ClearInternalForces\n";

    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        currNode->Centerline[IdxCurr].SetForce( 0, 0, 0 );
        //currNode->Centerline[IdxCurr].SetVelocity( 0, 0, 0 );
        currNode->Torque.SetRIJK( 0, 0, 0, 0 );
        ++currNode;
    }
}

// Store internal forces
template <typename T>
void ModelElasticRod<T>::StoreInternalForces ()
{
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        currNode->OldInternalForce = currNode->Centerline[IdxCurr].GetForce();
        currNode->OldInternalTorque = currNode->Torque;
        ++currNode;
    }
}

// Restore internal forces
template <typename T>
void ModelElasticRod<T>::RestoreInternalForces ()
{
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
    while ( currNode != m_Nodes.end() ) {
        currNode->Centerline[IdxCurr].SetForce( currNode->OldInternalForce );
        currNode->Torque = currNode->OldInternalTorque;
        ++currNode;
    }
}


// Advance Simulation by time step
template <typename T>
void ModelElasticRod<T>::AdvanceSimulation ()
{
    #if ER_TIMING_ADV_SIM != 0
        // TIMING
        static int times = -1;
        ++times;
        clock_t startTime, endTime;
        static clock_t compTime = 0;
        clock_t advSimStartTime, advSimEndTime;
        static clock_t advSimTime = 0;
        static bool isFirstRun = true;
        advSimStartTime = clock();
    #endif

    for ( unsigned int i = 0; i < m_Parameters.SimIterationTimes; ++i )
    {
        #if ER_TIMING_ADV_SIM != 0
        startTime = clock();    // TIMING
        #endif

        #ifdef  TAPs_USE_CUDA
        if ( m_Parameters.EnableCUDA ) {
            //m_CUDA.AdvanceSimulation ( tCurrentTime, tNextTime, numOfSubSteps );
            m_CUDA.AdvanceSimulation ();

            // IMPORTANT!!!
            // Switch data accessing (previous becomes current and current becomes previous)
            SwitchDataAccessing();

            #if ER_TIMING_ADV_SIM != 0
                endTime = clock();  // TIMING
                if ( isFirstRun ) {
                    isFirstRun = false;
                }
                else {
                    compTime += endTime - startTime;    // TIMING
                }
            #endif
        }
        else
        #endif//TAPs_USE_CUDA
        {
            // A simulation loop runs on CPU.
            AdvSimLoop();

            #if ER_TIMING_ADV_SIM != 0
                endTime = clock();  // TIMING
                if ( isFirstRun ) {
                    isFirstRun = false;
                }
                else {
                    compTime += endTime - startTime;    // TIMING
                }
            #endif
        }

        #if ER_TIMING_ADV_SIM != 0
            advSimEndTime = endTime = clock();  // TIMING
            if ( isFirstRun ) {
                isFirstRun = false;
            }
            else {
                advSimTime += advSimEndTime - advSimStartTime;  // TIMING
            }

            #ifdef  TAPs_USE_CUDA
            if ( m_Parameters.EnableCUDA ) {
                if ( times == ER_TIMING_ADV_SIM_STOP ) {
                    std::cout << "SIM (GPU) ACC TIME (for " << times << " times): " << 1000.0 * compTime / CLOCKS_PER_SEC << " ms.\n";
                    std::cout << "ADV SIM ACC TIME (for " << times << " times): " << 1000.0 * advSimTime / CLOCKS_PER_SEC << " ms.\n";
                    std::cout << std::endl;
                    exit(-1);
                }
            }
            else
            #endif//TAPs_USE_CUDA
            {
                if ( times == ER_TIMING_ADV_SIM_STOP ) {
                    std::cout << "SIM (CPU) ACC TIME (for " << times << " times): " << 1000.0 * compTime / CLOCKS_PER_SEC << " ms.\n";
                    std::cout << "ADV SIM ACC TIME (for " << times << " times): " << 1000.0 * advSimTime / CLOCKS_PER_SEC << " ms.\n";
                    std::cout << std::endl;
                    exit(-1);
                }
            }
        #endif

    } // END: for ( unsigned int i = 0; i < m_Parameters.SimIterationTimes; ++i ) {}
}

// Advance simulation loop (on CPU)
template <typename T>
void ModelElasticRod<T>::AdvSimLoop ()
{


    #if ER_TIMING_ADV_SIM != 0
        // TIMING
        static bool isFirstRun = true;
        static int times = 0;
        static clock_t compTime = 0;
        clock_t startTime, endTime;
    #endif

    #if ER_TIMING_ADV_SIM != 0
        startTime = clock();
    #endif

    #if ER_TIMING_ADV_SIM_DETAILS != 0
        // TIMING
        static clock_t T_clear_forces = 0;  // time for clear forces (both centerlines and orientations)
        static clock_t T_clear_forces_MAX = 0;      // MAX time for clear forces (both centerlines and orientations)
        static clock_t T_clear_forces_MIN = 777;    // MIN time for clear forces (both centerlines and orientations)
        clock_t Start_T_clear_forces = 0;   // start time for collision detection update BVH
        clock_t End_T_clear_forces = 0;     // end time for collision detection update BVH

        // Timing for the inner sim loop
        static clock_t T_inner_sim_loop = 0;
        static clock_t T_inner_sim_loop_MAX = 0;
        static clock_t T_inner_sim_loop_MIN = 777;
        clock_t Start_T_inner_sim_loop = 0;
        clock_t End_T_inner_sim_loop = 0;

        static clock_t T_CD_update = 0; // time for collision detection update BVH
        static clock_t T_CD_self = 0;   // time for collision detection self intersections
        static clock_t T_FC_centerline = 0;     // time for force computation of centerlines
        static clock_t T_FC_orientation = 0;    // time for force computation of orientations
        static clock_t T_Int_centerline = 0;    // time for integration of centerlines
        static clock_t T_Int_orientations = 0;  // time for integration of orientations
        static clock_t T_CD_update_MAX = 0; // MAX time for collision detection update BVH
        static clock_t T_CD_self_MAX = 0;   // MAX time for collision detection self intersections
        static clock_t T_FC_centerline_MAX = 0;     // MAX time for force computation of centerlines
        static clock_t T_FC_orientation_MAX = 0;    // MAX time for force computation of orientations
        static clock_t T_Int_centerline_MAX = 0;    // MAX time for integration of centerlines
        static clock_t T_Int_orientations_MAX = 0;  // MAX time for integration of orientations
        static clock_t T_CD_update_MIN = 777;   // MIN time for collision detection update BVH
        static clock_t T_CD_self_MIN = 777; // MIN time for collision detection self intersections
        static clock_t T_FC_centerline_MIN = 777;       // MIN time for force computation of centerlines
        static clock_t T_FC_orientation_MIN = 777;  // MIN time for force computation of orientations
        static clock_t T_Int_centerline_MIN = 777;  // MIN time for integration of centerlines
        static clock_t T_Int_orientations_MIN = 777;    // MIN time for integration of orientations
        clock_t Start_T_CD_update = 0;  // start time for collision detection update BVH
        clock_t Start_T_CD_self = 0;    // start time for collision detection self intersections
        clock_t Start_T_FC_centerline = 0;      // start time for force computation of centerlines
        clock_t Start_T_FC_orientation = 0; // start time for force computation of orientations
        clock_t Start_T_Int_centerline = 0; // start time for integration of centerlines
        clock_t Start_T_Int_orientations = 0;   // start time for integration of orientations
        clock_t End_T_CD_update = 0;    // end time for collision detection update BVH
        clock_t End_T_CD_self = 0;  // end time for collision detection self intersections
        clock_t End_T_FC_centerline = 0;        // end time for force computation of centerlines
        clock_t End_T_FC_orientation = 0;   // end time for force computation of orientations
        clock_t End_T_Int_centerline = 0;   // end time for integration of centerlines
        clock_t End_T_Int_orientations = 0; // end time for integration of orientations
    #endif

    #if ER_TIMING_ADV_SIM_DETAILS != 0
        Start_T_clear_forces = clock();
    #endif

    ClearInternalForces();

    #if ER_TIMING_ADV_SIM_DETAILS != 0
        End_T_clear_forces = clock();
        if ( !isFirstRun ) {
            clock_t timediff = End_T_clear_forces - Start_T_clear_forces;
            T_clear_forces += timediff;
            if ( T_clear_forces_MAX < timediff )    T_clear_forces_MAX = timediff;
            if ( T_clear_forces_MIN > timediff )    T_clear_forces_MIN = timediff;
            if ( times >= ER_TIMING_ADV_SIM_STOP-1 ) {
                std::cout << "Time to clear forces (both centerlines and orientations) (for " << times << " times): " << 1000.0 * T_clear_forces / CLOCKS_PER_SEC 
                    << " ms with (MIN & MAX: " << T_clear_forces_MIN << " & " << T_clear_forces_MAX << ").\n";
            }
        }
    #endif

    #ifdef  TAPs_SIM_WITH_POSITION_BASED_DYNAMICS
    #else //TAPs_SIM_WITH_POSITION_BASED_DYNAMICS
        // Update the tree and check for self intersection (resolving collision by setting internal forces)
        UpdateBVHTree();
        CDRforSelfIntersections();
    #endif//TAPs_SIM_WITH_POSITION_BASED_DYNAMICS

    #if ER_TIMING_ADV_SIM_DETAILS != 0
        Start_T_inner_sim_loop = clock();
    #endif

    #ifdef  TAPs_ADD_KNOT_RECOGNITION
        //m_KR.ComputeStickPairForce();
    #endif//TAPs_ADD_KNOT_RECOGNITION

    // Simulation Loop
    int count = 0;
    SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();

    while ( currNode != m_Nodes.end() ) {

        // Computing forces for centerline (except the last node)
        if ( ++count < m_Parameters.NumOfNodes ) {
    
            // Compute the constraint force for both centerlines and the quaternion
            ComputeConstraintForceOfCenterlineAndOrientation( currNode );

            ComputeStretchForceOfCenterline( currNode );

            ComputeBendTorqueOfQuaternion( currNode );

        #ifdef  USE_CORDE_SIM
            ComputeTranslationalDissipationForceOfCenterline( currNode );
            ComputeRotationalDissipationTorqueOfQuaternion( currNode );
            ComputeKineticForceOfCenterline( currNode );
            ComputeKineticTorqueOfQuaternion( currNode );
        #endif//USE_CORDE_SIM
        }

        // For each centerline
        ComputeGravitationalForceOfCenterline( currNode );
        //ComputeGravitationalVelocityOfCenterline( h, currNode );
        ComputeDampingVelocityOfCenterline( currNode );

        #ifdef  TAPs_ADVANCED_SIMULATION
        if ( !currNode->SimFlags.CheckSimulationConstraints( Enum::AddOn::FIXED ) )
        #endif//TAPs_ADVANCED_SIMULATION
        {
            //-----------------------------------------------------------
            // SEMI-IMPLICIT EULER SCHEME FOR CENTERLINE
            //-----------------------------------------------------------
            // Since force will be clear before starting the force computation for the next time step,
            // maintain the force information can be neglected.

            // The statement below is replaced by the one without currNode->Centerline[IdxCurr].GetVelocity()
            //currNode->Centerline[IdxNext].SetVelocity( h/currNode->Centerline[IdxCurr].GetMass() 
            //  * (currNode->Centerline[IdxCurr].GetForce() + currNode->ExternalForce + currNode->ExternalForce_Reserved) );


            #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
                //m_CD.SelfCDExtraDataTrackStatus( currNode->ID );
            #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

            // Next velocity
            currNode->Centerline[IdxNext].SetVelocity( currNode->Centerline[IdxCurr].GetVelocity() 
                + m_Parameters.TimeStep/currNode->Centerline[IdxCurr].GetMass() 
                    * ( currNode->Centerline[IdxCurr].GetForce() 
                        + currNode->ExternalForce           // the ExternalForce is used for CD collision force
                        + currNode->ExternalForce_Reserved  // the ExternalForce_Reserved is used in the classes inherited from this class
                        + currNode->ExternalForce_Constrained   // the ExternalForce_Constrained is used by class TAPsAdvSimConstraints.hpp
                        + currNode->ExternalForce_ForAdvSimCtrl )
                                                                
                );
            // Next position
            currNode->Centerline[IdxNext].SetPosition( 
                  currNode->Centerline[IdxCurr].GetPosition() 
                + m_Parameters.TimeStep*currNode->Centerline[IdxNext].GetVelocity() 
            );

            //-----------------------------------------------------------
            // For each orientation (except the last node)
            if ( count < m_Parameters.NumOfNodes-1 ) {

                //-----------------------------------------------------------
                // SEMI-IMPLICIT EULER SCHEME FOR ORIENTATION
                //-----------------------------------------------------------
                // Compute the components of the inverse of the inertia tensor matrix
                T A = m_Parameters.MaterialDensity * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius;
                T B = currNode->OrientationRestLength * A;
                Jinv[0,0] = Jinv[1,1] = 4 / B;
                Jinv[2,2] = 2 / A;
                // Compute the angular velocity from the 4d torque
                //   1/2 * Q_j^T * torque_in_4Dim
                //   (Q_j^T * torque_in_4Dim) equals "the multiplication of conjugation_of_q_j with torque_in_4Dim"
                Quaternion<T> torque_transformed( T(0.5) * currNode->Orientation[IdxCurr].GetConjugate() * currNode->Torque );
                // Angular velocity := 1/2 * Jinv * torque_transformed_into_3Dim
                Quaternion<T> angularVelocity( 
                    0,  // deliberately set to zero
                    0.5*Jinv[0,0]*torque_transformed.GetI(), 
                    0.5*Jinv[1,1]*torque_transformed.GetJ(), 
                    0.5*Jinv[2,2]*torque_transformed.GetK()
                );

                // Compute the velocity of the quaternion
                Quaternion<T> q_velocity( T(0.5) * currNode->Orientation[IdxCurr] * angularVelocity );

                // Set the next quaternion
                currNode->Orientation[IdxNext] = currNode->Orientation[IdxCurr] + q_velocity*m_Parameters.TimeStep;
                //-----------------------------------------------------------

                //-----------------------------------------------------------
                // For each orientation
                // Renormalize the quaternion, by the coordinate projection method
                currNode->Orientation[IdxNext].Normalized();
                //-----------------------------------------------------------
            }

            #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
                m_CD.CheckAndRestoreWithSelfCDExtraData( currNode->ID );
            #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

            // Enforce Suture's Position
            if ( currNode->UseEnforcedPosition ) {
                currNode->Centerline[IdxNext].SetPosition( currNode->EnforcedPosition );

                // Average between the new calculated position and the enforced position
                //currNode->Centerline[IdxNext].SetPosition( ( currNode->EnforcedPosition + currNode->Centerline[IdxNext].GetPosition() ) * T(0.5) );
                //currNode->Centerline[IdxNext].SetPosition( ( currNode->EnforcedPosition*T(7) + currNode->Centerline[IdxNext].GetPosition()*T(3) ) * T(0.1) );

            }
            // Enforce Suture's Orientation
            if ( currNode->UseEnforcedOrientation ) {
                currNode->Orientation[IdxNext] = currNode->EnforcedOrientation;
            }

        }
        ++currNode;
    }

    #if ER_TIMING_ADV_SIM_DETAILS != 0
        End_T_inner_sim_loop = clock();
        if ( !isFirstRun ) {
            clock_t timediff = End_T_inner_sim_loop - Start_T_inner_sim_loop;
            T_inner_sim_loop += timediff;
            if ( T_inner_sim_loop_MAX < timediff )  T_inner_sim_loop_MAX = timediff;
            if ( T_inner_sim_loop_MIN > timediff )  T_inner_sim_loop_MIN = timediff;
            if ( times >= ER_TIMING_ADV_SIM_STOP-1 ) {
                std::cout << "Time of inner sim loop (for " << times << " times): " << 1000.0 * T_inner_sim_loop / CLOCKS_PER_SEC 
                    << " ms with (MIN & MAX: " << T_inner_sim_loop_MIN << " & " << T_inner_sim_loop_MAX << ").\n";
            }
        }
    #endif

    #ifdef  TAPs_SIM_WITH_POSITION_BASED_DYNAMICS
        // Update the tree and check for self intersection (resolving collision by geometric setting)
        UpdateBVHTree_PBD();
        CDRforSelfIntersections_PBD();
        UpdateVelocities_PBD( m_Parameters.TimeStep );
    #endif//TAPs_SIM_WITH_POSITION_BASED_DYNAMICS


    // Update the extra self CD data
    #ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
        //m_CD.PreventMissedCDWithSelfCDExtraData();
        m_CD.UpdateSelfCDExtraData();
    #endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION


    #ifdef  TAPs_ADD_KNOT_RECOGNITION
        #ifdef  TAPs_ADD_ANIMATED_KNOTS
            m_KR.ProcessKnotAnimation();
        #endif//TAPs_ADD_ANIMATED_KNOTS
        m_KR.ConstrainKnots();
    #endif//TAPs_ADD_KNOT_RECOGNITION

    // IMPORTANT!!!
    // Switch data accessing (previous becomes current and current becomes previous)
    SwitchDataAccessing();

    #if ER_TIMING_ADV_SIM != 0
        endTime = clock();  // TIMING
        if ( !isFirstRun ) {
            compTime += endTime - startTime;    // TIMING
            ++times;
        }
    #endif

    #if ER_TIMING_ADV_SIM != 0
        if ( isFirstRun )   isFirstRun = false;
        std::cout << "SIM (CPU) INNER AdvSim TIME (for " << times << " times): " << 1000.0 * compTime / CLOCKS_PER_SEC << " ms.\n";
    #endif
}

// Compute the length of a centerline segment
template <typename T>
T ModelElasticRod<T>::ComputeLengthOfCenterlineSegment (
        PointMass<T> const & c1, 
        PointMass<T> const & c2 )
{
    return (c2.GetPosition()-c1.GetPosition()).Length();
}

// Compute the length of an orientation segment
template <typename T>
T ModelElasticRod<T>::ComputeLengthOfOrientationSegment ( 
        PointMass<T> const & c1, 
        PointMass<T> const & c2, 
        PointMass<T> const & c3 )
{
    // The formula from the CORDE model
    //return 0.5*((c3.GetPosition()-c2.GetPosition()).Length()+(c2.GetPosition()-c1.GetPosition()).Length());
    // But the formula should return the length of vector ( (c3+c2)*0.5 - (c2+c1)*0.5 )
    //                                                     |             |
    //                                                     V             V
    //                                              midPtOf[c3,c2]    midPtOf[c2,c1]
    // Which is simplified to
    return 0.5 * ( c3.GetPosition()-c1.GetPosition() ).Length();
}

// Compute the stretch force of a centerline
template <typename T>
void ModelElasticRod<T>::ComputeStretchForceOfCenterline ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node
    ElasticRodNode<T> & currNode = *iterNode;
    // Next Node
    ElasticRodNode<T> & nextNode = *(++iterNode);
    --iterNode;     // return to the current position

    // Compute the stretch force
    // F_s = K_s * ( curr_len/rest_len - 1 ) * (nextPtMass-currPtMass)/curr_len
    Vector3<T> p2_p1 = nextNode.Centerline[IdxCurr].GetPosition() - currNode.Centerline[IdxCurr].GetPosition();
    T currLength = p2_p1.Length();

    T ratio = currLength/currNode.CenterlineRestLength - 1;
    T stretchMagnitude = ratio * m_Parameters.Ps * 10;

    //-----------------------------------------
    #ifdef  TAPs_USE_NONLINEAR_FORCES
    #else //TAPs_USE_NONLINEAR_FORCES
    #endif//TAPs_USE_NONLINEAR_FORCES
    //-----------------------------------------

    Vector3<T> stretchForce = stretchMagnitude * p2_p1/currLength;

    // Add the stretch force to the nodes' total force
    currNode.Centerline[IdxCurr].SetForce( currNode.Centerline[IdxCurr].GetForce() + stretchForce );
    nextNode.Centerline[IdxCurr].SetForce( nextNode.Centerline[IdxCurr].GetForce() - stretchForce );
}


#ifdef  USE_CORDE_SIM
// Compute the translational dissipation force of a centerline
template <typename T>
void ModelElasticRod<T>::ComputeTranslationalDissipationForceOfCenterline ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node
    ElasticRodNode<T> & currNode = *iterNode;
    Vector3<T> const & pos0 = currNode.Centerline[IdxCurr].GetPosition();
    Vector3<T> const & prevpos0 = currNode.Centerline[IdxPrev].GetPosition();
    Vector3<T> const & vel0 = ( pos0 - prevpos0 ) * m_Parameters.TimeStep;
    //Vector3<T> const & vel0 = currNode.Centerline[IdxCurr].GetVelocity();
    // Next Node
    ElasticRodNode<T> & nextNode = *(++iterNode);
    Vector3<T> const & pos1 = nextNode.Centerline[IdxCurr].GetPosition();
    Vector3<T> const & prevpos1 = nextNode.Centerline[IdxPrev].GetPosition();
    Vector3<T> const & vel1 = ( pos1 - prevpos1 ) * m_Parameters.TimeStep;
    //Vector3<T> const & vel1 = nextNode.Centerline[IdxCurr].GetVelocity();
    --iterNode;     // return to the current position


    // Compute the translational dissipation force
    Vector3<T> posRel( pos1-pos0 );
    Vector3<T> velRel( vel1-vel0 );
    Vector3<T> Vrel = posRel * ( velRel * posRel );
    
    T x01 = pos0[0] - pos1[0];
    T y01 = pos0[1] - pos1[1];
    T z01 = pos0[2] - pos1[2];
    T x10 = -x01;
    T y10 = -y01;
    T z10 = -z01;

    Vector3<T> transDissipationForce_0(
        Vrel[2]*x01*z10 + Vrel[1]*x01*y10 + Vrel[0]*x01*x10,
        Vrel[2]*y01*z10 + Vrel[1]*y01*y10 + Vrel[0]*y01*x10,
        Vrel[2]*z01*z10 + Vrel[1]*z01*y10 + Vrel[0]*z01*x10
    );
    T s = pow( currNode.CenterlineRestLength, 5 ) * 2;
    transDissipationForce_0 *= m_Parameters.Dt / s;

    //Since transDissipationForce_1 = -transDissipationForce_0;
    //Vector3<T> transDissipationForce_1(
    //  Vrel[2]*x10*z10 + Vrel[1]*x10*y10 + Vrel[0]*x10*x10,
    //  Vrel[2]*y10*z10 + Vrel[1]*y10*y10 + Vrel[0]*y10*x10,
    //  Vrel[2]*z10*z10 + Vrel[1]*z10*y10 + Vrel[0]*z10*x10
    //);
    //transDissipationForce_1 *= m_Parameters.Dt / s;

    // Add the force to the nodes' total force
    currNode.Centerline[IdxCurr].SetForce( currNode.Centerline[IdxCurr].GetForce() - transDissipationForce_0 );
    nextNode.Centerline[IdxCurr].SetForce( nextNode.Centerline[IdxCurr].GetForce() + transDissipationForce_0 );
}

// Compute the kinetic force of a centerline
template <typename T>
void ModelElasticRod<T>::ComputeKineticForceOfCenterline ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node
    ElasticRodNode<T> & currNode = *iterNode;
    Vector3<T> const & pos0 = currNode.Centerline[IdxCurr].GetPosition();
    Vector3<T> const & prevpos0 = currNode.Centerline[IdxPrev].GetPosition();
    Vector3<T> const & vel0 = ( pos0 - prevpos0 ) * m_Parameters.TimeStep;
    // Next Node
    ElasticRodNode<T> & nextNode = *(++iterNode);
    Vector3<T> const & pos1 = nextNode.Centerline[IdxCurr].GetPosition();
    Vector3<T> const & prevpos1 = nextNode.Centerline[IdxPrev].GetPosition();
    Vector3<T> const & vel1 = ( pos1 - prevpos1 ) * m_Parameters.TimeStep;
    --iterNode;     // return to the current position

    // Compute the kinetic force
    Vector3<T> V10( vel1 + vel0 );
    T s = currNode.CenterlineRestLength * 0.25 * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius * m_Parameters.MaterialDensity;
    Vector3<T> Fk = s * m_Parameters.Kt * V10;

    // Add the force to the nodes' total force
    currNode.Centerline[IdxCurr].SetForce( currNode.Centerline[IdxCurr].GetForce() - Fk );
    nextNode.Centerline[IdxCurr].SetForce( nextNode.Centerline[IdxCurr].GetForce() - Fk );
}
#endif//USE_CORDE_SIM


// Compute the damping force of a centerline
template <typename T>
void ModelElasticRod<T>::ComputeDampingVelocityOfCenterline ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node
    Vector3<T> const & vel = iterNode->Centerline[IdxCurr].GetVelocity();

    // Compute the damping velocity
    Vector3<T> velocityAfterDamping = m_Parameters.Kvdamping * vel;

    // Add the damping velocity to the nodes' velocity
    iterNode->Centerline[IdxCurr].SetVelocity( velocityAfterDamping );
}

// Compute the gravitational force of a centerline
template <typename T>
void ModelElasticRod<T>::ComputeGravitationalForceOfCenterline ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Skip if reaching the end of falling
#ifdef  TAPs_ADD_RESTING_LEVEL_Y
    if ( iterNode->Centerline[IdxCurr].GetPosition()[1] > m_Parameters.Radius + m_Parameters.RestingLevel ) {
        iterNode->Centerline[IdxCurr].GetForce()[1] += m_Parameters.Kgravity * iterNode->Centerline[IdxCurr].GetMass();
    }
    #ifdef  TAPs_SPECIFIC_FOR_SUTUREAPP_02
    else if ( iterNode->Centerline[IdxCurr].GetPosition()[1] < -m_Parameters.Radius + m_Parameters.RestingLevel ) {
        iterNode->Centerline[IdxCurr].GetForce()[1] -= m_Parameters.Kgravity*10 * iterNode->Centerline[IdxCurr].GetMass();
    }
    #endif//TAPs_SPECIFIC_FOR_SUTUREAPP_02
#endif//TAPs_ADD_RESTING_LEVEL_Y
    // Compute and add the gravitational force
    iterNode->Centerline[IdxCurr].GetForce()[1] += m_Parameters.Kgravity * iterNode->Centerline[IdxCurr].GetMass();
}

// Compute the intrinsic strain rate
template <typename T>
void ModelElasticRod<T>::ComputeIntrinsicStrainRate ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    if ( iterNode == m_Nodes.end() )    return;

    // Current node {j}
    ElasticRodNode<T> & currNode = *iterNode;
    Quaternion<T> & q = currNode.Orientation[IdxCurr];
    // Next Node {j+1}
    ElasticRodNode<T> & nextNode = *(++iterNode);

    if ( iterNode == m_Nodes.end() )    return;

    Quaternion<T> & qn = nextNode.Orientation[IdxCurr];
    --iterNode;     // return to the current position

    // Compute the bend force
    Quaternion<T> qa( qn + q );     // q_{j+1} + q_{j}
    Quaternion<T> qs( qn - q );     // q_{j+1} - q_{j}

    // B_k(q_{j+1}+q{j})
    T invRestLen = T(1.0) / currNode.OrientationRestLength;
    currNode.IntrinsicStrainRate[0] = B_1(qa).InnerProduct( qs ) * invRestLen;
    currNode.IntrinsicStrainRate[1] = B_2(qa).InnerProduct( qs ) * invRestLen;
    currNode.IntrinsicStrainRate[2] = B_3(qa).InnerProduct( qs ) * invRestLen;

    // Differentiation of ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) )
    // by q_{j}
    currNode.IntrinsicStrainQ[0].SetRIJK( -qn.GetI(),  qn.GetR(),  qn.GetK(), -qn.GetJ() );
    currNode.IntrinsicStrainQ[1].SetRIJK( -qn.GetJ(), -qn.GetK(),  qn.GetR(),  qn.GetI() );
    currNode.IntrinsicStrainQ[2].SetRIJK( -qn.GetK(),  qn.GetJ(), -qn.GetI(),  qn.GetR() );
    // Differentiation of ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) )
    // by q_{j+1}
    currNode.IntrinsicStrainQn[0].SetRIJK(  q.GetI(), -q.GetR(), -q.GetK(),  q.GetJ() );
    currNode.IntrinsicStrainQn[1].SetRIJK(  q.GetJ(),  q.GetK(), -q.GetR(), -q.GetI() );
    currNode.IntrinsicStrainQn[2].SetRIJK(  q.GetK(), -q.GetJ(),  q.GetI(), -q.GetR() );
}

// Compute the bend force (torque) of an orientation
template <typename T>
void ModelElasticRod<T>::ComputeBendTorqueOfQuaternion ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node {j}
    ElasticRodNode<T> & currNode = *iterNode;
    Quaternion<T> & q = currNode.Orientation[IdxCurr];
    // Next Node {j+1}
    ElasticRodNode<T> & nextNode = *(++iterNode);
    Quaternion<T> & qn = nextNode.Orientation[IdxCurr];
    --iterNode;     // return to the current position

    // Compute the bend force
    Quaternion<T> qa( qn + q );     // q_{j+1} + q_{j}
    Quaternion<T> qs( qn - q );     // q_{j+1} - q_{j}

    // B_k(q_{j+1}+q{j})
    Quaternion<T> B1qa( B_1(qa) );
    Quaternion<T> B2qa( B_2(qa) );
    Quaternion<T> B3qa( B_3(qa) );

    // K_k ( B_k(q_{j+1}+q{j})/rest_length inner_product_with (q_{j+1}-q{j}) )
    T invRestLen = T(1.0) / currNode.OrientationRestLength;
    T KB1 = m_Parameters.Pb[0] * ( B1qa.InnerProduct( qs )*invRestLen - currNode.IntrinsicStrainRate[0]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate );
    T KB2 = m_Parameters.Pb[1] * ( B2qa.InnerProduct( qs )*invRestLen - currNode.IntrinsicStrainRate[1]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate );
    T KB3 = m_Parameters.Pb[2] * ( B3qa.InnerProduct( qs )*invRestLen - currNode.IntrinsicStrainRate[2]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate );

    // Differentiation of ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) )
    // by q_{j}
    Quaternion<T> dB1_q( -qn.GetI(),  qn.GetR(),  qn.GetK(), -qn.GetJ() );
    Quaternion<T> dB2_q( -qn.GetJ(), -qn.GetK(),  qn.GetR(),  qn.GetI() );
    Quaternion<T> dB3_q( -qn.GetK(),  qn.GetJ(), -qn.GetI(),  qn.GetR() );
    // Differentiation of ( B_k(q_{j+1}+q{j}) inner_product_with (q_{j+1}-q{j}) )
    // by q_{j+1}
    Quaternion<T> dB1_qn(  q.GetI(), -q.GetR(), -q.GetK(),  q.GetJ() );
    Quaternion<T> dB2_qn(  q.GetJ(),  q.GetK(), -q.GetR(), -q.GetI() );
    Quaternion<T> dB3_qn(  q.GetK(), -q.GetJ(),  q.GetI(), -q.GetR() );

    // Add the bend torque to the total torque
    currNode.Torque += KB1*(dB1_q  - currNode.IntrinsicStrainQ[0]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate)
                     + KB2*(dB2_q  - currNode.IntrinsicStrainQ[1]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate)
                     + KB3*(dB3_q  - currNode.IntrinsicStrainQ[2]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate);
    nextNode.Torque += KB1*(dB1_qn - currNode.IntrinsicStrainQn[0]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate)
                     + KB2*(dB2_qn - currNode.IntrinsicStrainQn[1]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate)
                     + KB3*(dB3_qn - currNode.IntrinsicStrainQn[2]*m_Parameters.Kconstraint_ScaleIntrinsicStrainRate);
}


#ifdef  USE_CORDE_SIM
// Compute the rotational dissipation torque of an orientation
template <typename T>
void ModelElasticRod<T>::ComputeRotationalDissipationTorqueOfQuaternion ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    T inv_timestep = T(1) / m_Parameters.TimeStep;
    // Current node {j}
    ElasticRodNode<T> & currNode = *iterNode;
    Quaternion<T> & q = currNode.Orientation[IdxCurr];
    Quaternion<T> & q_prev = currNode.Orientation[IdxPrev];
    Quaternion<T> q_v = ( q_prev - q ) * inv_timestep;
    // Next Node {j+1}
    ElasticRodNode<T> & nextNode = *(++iterNode);
    Quaternion<T> & qn = nextNode.Orientation[IdxCurr];
    Quaternion<T> & qn_prev = nextNode.Orientation[IdxPrev];
    Quaternion<T> qn_v = ( qn_prev - qn ) * inv_timestep;
    --iterNode;     // return to the current position

    //
    // Compute the rotational torque
    //
    T q_1 = B0_1(qn).InnerProduct(qn_v) - B0_1(q).InnerProduct(q_v);
    T q_2 = B0_2(qn).InnerProduct(qn_v) - B0_2(q).InnerProduct(q_v);
    T q_3 = B0_3(qn).InnerProduct(qn_v) - B0_3(q).InnerProduct(q_v);
    T Q = m_Parameters.Dr[0]*q_1 + m_Parameters.Dr[1]*q_2 + m_Parameters.Dr[2]*q_3;
    //
    T Ar =  q.GetK() + q.GetJ() + q.GetI();
    T Ai = -q.GetR() - q.GetK() + q.GetJ();
    T Aj = -q.GetR() + q.GetK() - q.GetI();
    T Ak = -q.GetR() - q.GetJ() + q.GetI();
    //
    T Br = -qn.GetK() - qn.GetJ() - qn.GetI();
    T Bi =  qn.GetR() + qn.GetK() - qn.GetJ();
    T Bj =  qn.GetR() - qn.GetK() + qn.GetI();
    T Bk =  qn.GetR() + qn.GetJ() - qn.GetI();
    //
    T s = 4 / m_Parameters.LinkLength;
    Q *= s;
    Quaternion<T> Tr_0( Q * Quaternion<T>( Ar, Ai, Aj, Ak ) );
    Quaternion<T> Tr_1( Q * Quaternion<T>( Br, Bi, Bj, Bk ) );
    //
    currNode.Torque -= Tr_0;
    nextNode.Torque -= Tr_1;
}

// Compute the kinetic torque of an orientation
template <typename T>
void ModelElasticRod<T>::ComputeKineticTorqueOfQuaternion ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    T inv_timestep = T(1) / m_Parameters.TimeStep;
    // Current node {j}
    ElasticRodNode<T> & currNode = *iterNode;
    Quaternion<T> & q = currNode.Orientation[IdxCurr];
    Quaternion<T> & q_prev = currNode.Orientation[IdxPrev];
    Quaternion<T> q_v = ( q_prev - q ) * inv_timestep;
    // Next Node {j+1}
    ElasticRodNode<T> & nextNode = *(++iterNode);
    Quaternion<T> & qn = nextNode.Orientation[IdxCurr];
    Quaternion<T> & qn_prev = nextNode.Orientation[IdxPrev];
    Quaternion<T> qn_v = ( qn_prev - qn ) * inv_timestep;
    --iterNode;     // return to the current position

    Quaternion<T> qqn = q + qn;
    Quaternion<T> qqn_v = q_v + qn_v;
    T Is = m_Parameters.MaterialDensity * Math<T>::PI * m_Parameters.Radius*m_Parameters.Radius;
    T k1 = Is*0.25 * B_1(qqn).InnerProduct(qqn_v);
    T k2 = Is*0.25 * B_2(qqn).InnerProduct(qqn_v);
    T k3 = Is*0.5  * B_3(qqn).InnerProduct(qqn_v);
    T s = m_Parameters.LinkLength * 0.25 * ( m_Parameters.Kr[0]*k1 + m_Parameters.Kr[1]*k2 + m_Parameters.Kr[2]*k3 );

    Quaternion<T> Q(
        -q.GetI() - q_v.GetI() - qn_v.GetI() - qn.GetI(),
         q.GetR() + q_v.GetR() + qn_v.GetR() + qn.GetR(),
         q.GetK() + q_v.GetK() + qn_v.GetK() + qn.GetK(),
         q.GetJ() - q_v.GetJ() - qn_v.GetJ() - qn.GetJ()
    );
    Q *= s;
    currNode.Torque -= Q;
    nextNode.Torque -= Q;
}
#endif//USE_CORDE_SIM


// Compute the constraint force of both centerlines and a quaternion
template <typename T>
void ModelElasticRod<T>::ComputeConstraintForceOfCenterlineAndOrientation ( 
    typename SetOfElasticRodNodes::iterator & iterNode )
{
    // Current node {j}
    ElasticRodNode<T> & currNode = *iterNode;
    // Next Node {j+1}
    ElasticRodNode<T> & nextNode = *(++iterNode);
    --iterNode;     // return to the current position

    PointMass<T> & R  = currNode.Centerline[IdxCurr];   // current centerline
    PointMass<T> & Rn = nextNode.Centerline[IdxCurr];   // next centerline
    Quaternion<T> & Q = currNode.Orientation[IdxCurr];  // the (current) orientation sitting between the current and next centerlines

    Vector3<T> Rn_R ( Rn.GetPosition() - R.GetPosition() );
    T rn_r_len_square = Rn_R.SquaredLength();
    T rn_r_len = sqrt( rn_r_len_square );

    // rest_len * Kappa * ( (r_{i+1}-r_{i})/||r_{i+1}-r_{i}|| - d3(q_{i}) )
    Vector3<T> Vc = currNode.CenterlineRestLength * m_Parameters.Kconstraint_3rdDirAlignTangent * ( Rn_R/rn_r_len - Get3rdDirector(Q) );

    // Compute the constraint force (torque) for the quaternion
    Vector3<T> Vc2 = Vc * 2;
    Quaternion<T> cTorque_q ( 
        Vc2 * Vector3<T>( Q.GetJ(), -Q.GetI(),  Q.GetR() ),
        Vc2 * Vector3<T>( Q.GetK(), -Q.GetR(), -Q.GetI() ),
        Vc2 * Vector3<T>( Q.GetR(),  Q.GetK(), -Q.GetJ() ),
        Vc2 * Vector3<T>( Q.GetI(),  Q.GetJ(),  Q.GetK() )
    );
    // Add the constraint torque to the total torque
    currNode.Torque += cTorque_q;

    // Compute the constraint force for both centerlines
    // The sum of the constaint forces at r_{i} and r_{i+1} is zero.
    Vector3<T> Vcr = Vc / ( rn_r_len_square * rn_r_len );
    T xd = Rn_R.GetX(), yd = Rn_R.GetY(), zd = Rn_R.GetZ();
    T xd_yd = xd * yd;
    T xd_zd = xd * zd;
    T yd_zd = yd * zd;
    Vector3<T> cForce_r (
        Vcr * Vector3<T>( rn_r_len_square - xd*xd,  -xd_yd,                     -xd_zd ),
        Vcr * Vector3<T>( -xd_yd,                   rn_r_len_square - yd*yd,        -yd_zd ),
        Vcr * Vector3<T>( -xd_zd,                   -yd_zd,                     rn_r_len_square - zd*zd )
    );

    currNode.Centerline[IdxCurr].SetForce( currNode.Centerline[IdxCurr].GetForce() + cForce_r );
    nextNode.Centerline[IdxCurr].SetForce( nextNode.Centerline[IdxCurr].GetForce() - cForce_r );
}

// Return the quaternion resulted by B_1 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B_1 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //       |  0  0  0  1 | |i|
    // B_1 = |  0  0  1  0 | |j|
    //       |  0 -1  0  0 | |k|
    //       | -1  0  0  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //       |  0 -1  0  0 | |r|   |-i|
    //       |  1  0  0  0 | |i|   | r|
    // B_1 = |  0  0  0  1 | |j| = | k|
    //       |  0  0 -1  0 | |k|   |-j|
    return Quaternion<T>( -q.GetI(), q.GetR(), q.GetK(), -q.GetJ() );
}

// Return the quaternion resulted by B_2 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B_2 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //       |  0  0 -1  0 | |i|
    // B_2 = |  0  0  0  1 | |j|
    //       |  1  0  0  0 | |k|
    //       |  0 -1  0  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //       |  0  0 -1  0 | |r|   |-j|
    //       |  0  0  0 -1 | |i|   |-k|
    // B_2 = |  1  0  0  0 | |j| = | r|
    //       |  0  1  0  0 | |k|   | i|
    return Quaternion<T>( -q.GetJ(), -q.GetK(), q.GetR(), q.GetI() );
}

// Return the quaternion resulted by B_3 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B_3 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //       |  0  1  0  0 | |i|
    // B_3 = | -1  0  0  0 | |j|
    //       |  0  0  0  1 | |k|
    //       |  0  0 -1  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //       |  0  0  0 -1 | |r|   |-k|
    //       |  0  0  1  0 | |i|   | j|
    // B_3 = |  0 -1  0  0 | |j| = |-i|
    //       |  1  0  0  0 | |k|   | r|
    return Quaternion<T>( -q.GetK(), q.GetJ(), -q.GetI(), q.GetR() );
}

// Return the quaternion resulted by B0_1 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B0_1 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //        |  0  0  0  1 | |i|
    // B0_1 = |  0  0 -1  0 | |j|
    //        |  0  1  0  0 | |k|
    //        | -1  0  0  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //        |  0 -1  0  0 | |r|   |-i|
    //        |  1  0  0  0 | |i|   | r|
    // B0_1 = |  0  0  0 -1 | |j| = |-k|
    //        |  0  0  1  0 | |k|   | j|
    return Quaternion<T>( -q.GetI(), q.GetR(), -q.GetK(), q.GetJ() );
}

// Return the quaternion resulted by B0_2 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B0_2 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //        |  0  0  1  0 | |i|
    // B0_2 = |  0  0  0  1 | |j|
    //        | -1  0  0  0 | |k|
    //        |  0 -1  0  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //        |  0  0 -1  0 | |r|   |-j|
    //        |  0  0  0  1 | |i|   | k|
    // B0_2 = |  1  0  0  0 | |j| = | r|
    //        |  0 -1  0  0 | |k|   |-i|
    return Quaternion<T>( -q.GetJ(), q.GetK(), q.GetR(), -q.GetI() );
}

// Return the quaternion resulted by B0_3 * a quaternion
template <typename T>
Quaternion<T> ModelElasticRod<T>::B0_3 ( Quaternion<T> const & q ) const
{
    // Quaternions in the CoRdE model is <i,j,k,r>
    //        |  0 -1  0  0 | |i|
    // B0_3 = |  1  0  0  0 | |j|
    //        |  0  0  0  1 | |k|
    //        |  0  0 -1  0 | |r|
    // So it has to be converted to fit our quaternion <r,i,j,k>
    // by first remove the last column and add it back as the first column,
    // then remove the last row and add it back as the first row.
    //        |  0  0  0 -1 | |r|   |-k|
    //        |  0  0 -1  0 | |i|   |-j|
    // B0_3 = |  0  1  0  0 | |j| = | i|
    //        |  1  0  0  0 | |k|   | r|
    return Quaternion<T>( -q.GetK(), -q.GetJ(), q.GetI(), q.GetR() );
}

/*
// Get the k-th director from a rotation matrix
template <typename T>
Vector3<T> ModelElasticRod<T>::GetDirector ( int directorNumber, Matrix3x3<T> const & R ) const
{
    // The k-th director of the j-th material frame is obtained as 
    // the k-th column of the rotation matrix R_j.
    // Here k is zero-based.
    assert( 0 <= directorNumber && directorNumber <= 2 );
    return Vector3<T>( R(directorNumber,0), R(directorNumber,1), R(directorNumber,2) );
}
//*/

// Get the 1st director from a quaternion
template <typename T>
Vector3<T> ModelElasticRod<T>::Get1stDirector ( Quaternion<T> const & q ) const
{
    // The k-th director of the j-th material frame is obtained as 
    // the k-th column of the rotation matrix R_j.
    // Here k is zero-based.

    // The rotation matrix from a quaternion can be obtained with this formula
    //Matrix3x3<T>( 
    //      rr+ii-jj-kk,    2*(ij-rk),    2*(ik+rj),
    //        2*(ij+rk),  rr-ii+jj-kk,    2*(jk-ri),
    //        2*(ik-rj),    2*(jk+ri),  rr-ii-jj+kk
    //  );
    return Vector3<T>( 
        q.GetR()*q.GetR() + q.GetI()*q.GetI() - q.GetJ()*q.GetJ() - q.GetK()*q.GetK(),
        2*(q.GetI()*q.GetJ() + q.GetR()*q.GetK()),
        2*(q.GetI()*q.GetK() - q.GetR()*q.GetJ())
    );
}

// Get the 2nd director from a quaternion
template <typename T>
Vector3<T> ModelElasticRod<T>::Get2ndDirector ( Quaternion<T> const & q ) const
{
    // The k-th director of the j-th material frame is obtained as 
    // the k-th column of the rotation matrix R_j.
    // Here k is zero-based.

    // The rotation matrix from a quaternion can be obtained with this formula
    //Matrix3x3<T>( 
    //      rr+ii-jj-kk,    2*(ij-rk),    2*(ik+rj),
    //        2*(ij+rk),  rr-ii+jj-kk,    2*(jk-ri),
    //        2*(ik-rj),    2*(jk+ri),  rr-ii-jj+kk
    //  );
    return Vector3<T>( 
        2*(q.GetI()*q.GetJ() - q.GetR()*q.GetK()),
        q.GetR()*q.GetR() - q.GetI()*q.GetI() + q.GetJ()*q.GetJ() - q.GetK()*q.GetK(),
        2*(q.GetJ()*q.GetK() + q.GetR()*q.GetI())
    );
}

// Get the 3rd director from a quaternion
template <typename T>
Vector3<T> ModelElasticRod<T>::Get3rdDirector ( Quaternion<T> const & q ) const
{
    // The k-th director of the j-th material frame is obtained as 
    // the k-th column of the rotation matrix R_j.
    // Here k is zero-based.

    // The rotation matrix from a quaternion can be obtained with this formula
    //Matrix3x3<T>( 
    //      rr+ii-jj-kk,    2*(ij-rk),    2*(ik+rj),
    //        2*(ij+rk),  rr-ii+jj-kk,    2*(jk-ri),
    //        2*(ik-rj),    2*(jk+ri),  rr-ii-jj+kk
    //  );
    return Vector3<T>( 
        2*(q.GetI()*q.GetK() + q.GetR()*q.GetJ()),
        2*(q.GetJ()*q.GetK() - q.GetR()*q.GetI()),
        q.GetR()*q.GetR() - q.GetI()*q.GetI() - q.GetJ()*q.GetJ() + q.GetK()*q.GetK()
    );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::u_1_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_ds,     // the spatial derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_1(q) * q_ds * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::u_2_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_ds,     // the spatial derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_2(q) * q_ds * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::u_3_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_ds,     // the spatial derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_3(q) * q_ds * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega_1_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_1(q) * q_dt * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega_2_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_2(q) * q_dt * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega_3_LocFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B_3(q) * q_dt * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega0_1_RefFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B0_1(q) * q_dt * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega0_2_RefFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B0_2(q) * q_dt * scale );
}

template <typename T>
Quaternion<T> ModelElasticRod<T>::omega0_3_RefFrame ( 
    Quaternion<T> const & q,        // the current quaternion
    Quaternion<T> const & q_dt,     // the time derivative of the current quaternion
    T scale                         // the scale should be 2/||quaternion||^2
)
{
    return Quaternion<T>( B0_3(q) * q_dt * scale );
}
//-----------------------------------------------------------------------------


//=============================================================================
//-----------------------------------------------------------------------------
template <typename T>
int ModelElasticRod<T>::FindTheClosestPointToPoint (
    Vector3<T> const & closeToPoint,    
    T distance_limit                    
) const
{
    int iNumPoints = GetNumberOfPoints();

    int iClosestPointNumber = -1;
    T tClosestDistance = distance_limit;
    T tDistance;
    for ( int i = 0; i < iNumPoints; ++i ) {
        tDistance = ( closeToPoint - RefToCurrentVertexPositionOfNodeNumber( i ) ).Length();
        if ( tClosestDistance > tDistance ) {
            tClosestDistance = tDistance;
            iClosestPointNumber = i;
        }
    }

    return iClosestPointNumber;
}
//-----------------------------------------------------------------------------
template <typename T>
bool ModelElasticRod<T>::PickAt (
    Vector3<T> const & closeToPoint,    
    T distance_limit,                   
    int & pickedID                      
)
{
    pickedID = FindTheClosestPointToPoint( closeToPoint, distance_limit );
    if ( pickedID < 0 ) return false;
    m_Nodes[ pickedID ].UseEnforcedPosition = true;
    m_Nodes[ pickedID ].SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
    return true;
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelElasticRod<T>::PickPoint (
    int pickedID            
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfPoints() );

    //if ( 0 <= pickedID && pickedID < GetNumberOfPoints() ) {
        m_Nodes[ pickedID ].UseEnforcedPosition = true;
        m_Nodes[ pickedID ].SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
    //}
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelElasticRod<T>::MovePickedPoint (
    int pickedID,           
    Vector3<T> & position   
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfPoints() );
    //if ( 0 <= pickedID && pickedID < GetNumberOfPoints() ) {
        //if ( m_Nodes[ pickedID ].UseEnforcedPosition ) {
            m_Nodes[ pickedID ].EnforcedPosition  = position;
        //}
    //}
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelElasticRod<T>::UnpickPoint (
    int pickedID            
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfPoints() );
    //if ( 0 <= pickedID && pickedID < GetNumberOfPoints() ) {
        m_Nodes[ pickedID ].UseEnforcedPosition = false;
        m_Nodes[ pickedID ].SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
    //}
}
//=============================================================================



/*
//=============================================================================
#ifdef  TAPs_ADD_KNOT_RECOGNITION
//-----------------------------------------------------------------------------
template <typename T>
void ModelElasticRod<T>::ComputeStickPairForce ( typename SetOfElasticRodNodes::iterator & iterNode )
{
    if ( iterNode->StickPairNodeID < 0 )        return;

    // Current node
    //ElasticRodNode<T> & currNode = *iterNode;
    // Next Node
    //ElasticRodNode<T> & nextNode = *(++iterNode);
    //--iterNode;       // return to the current position

    // Compute the stick pair force
    // F_s = K_s * ( curr_len/rest_len - 1 ) * (nextPtMass-currPtMass)/curr_len
    Vector3<T> pB_pA = m_Nodes[iterNode->StickPairNodeID].Centerline[IdxCurr].GetPosition() - iterNode->Centerline[IdxCurr].GetPosition();
    T currLength = pB_pA.Length() - m_Parameters.Radius*2;
    T scale = m_Parameters.KstickPair * currLength;

    Vector3<T> stickForce = scale * pB_pA;

    // Add the stretch force to the nodes' total force
    iterNode->Centerline[IdxCurr].SetForce( iterNode->Centerline[IdxCurr].GetForce() + stickForce );
    //m_Nodes[iterNode->StickPairNodeID].Centerline[IdxCurr].SetForce( m_Nodes[iterNode->StickPairNodeID].Centerline[IdxCurr].GetForce() - stickForce );
}
//-----------------------------------------------------------------------------
#endif//TAPs_ADD_KNOT_RECOGNITION
//=============================================================================
//*/



#if defined(__gl_h_) || defined(__GL_H__)
//=============================================================================
template <typename T>
void ModelElasticRod<T>::InitGL ()
{
    ClearGL();

    Rendering.Material.SetEmission( 0, 0, 0, 1 );
    Rendering.Material.SetAmbient( 0.2, 0.4, 0.8, 1 );
    Rendering.Material.SetDiffuse( 0.2, 0.4, 1.0, 1 );
    //Rendering.Material.SetAmbient( 0.9, 0.9, 1.0, 1 );
    //Rendering.Material.SetDiffuse( 0.9, 0.9, 1.0, 1 );
    Rendering.Material.SetSpecular( 1, 1, 1, 1 );
    Rendering.Material.SetShininess( 128 );


    m_drawCrossSectionVertices = new Vector3<T>[m_drawNV];
    m_drawCrossSectionNormals = new Vector3<T>[m_drawNV];
    m_drawCrossSectionTexCoords = new T[m_drawNV];

    vertices  = new Vector3<T>*[3];
    normals   = new Vector3<T>*[3];
    texCoords = new Vector3<T>*[3];
    for ( int i = 0; i < 3; ++i ) {
        vertices[i]  = new Vector3<T>[m_drawNV];
        normals[i]   = new Vector3<T>[m_drawNV];
        texCoords[i] = new Vector3<T>[m_drawNV];
    }
    sVertices = new Vector3<T>[m_drawNV];

    ComputeVerticesAndNormalsOfCircleCrossSection();

    #ifdef  TAPs_USE_GLSL
        if ( !InitGLSL() ) {
            std::cout << "FAILED: Initializing GLSL!" << std::endl;
            exit(-1);
        }
    #endif//TAPs_USE_GLSL

    // (m_drawNV * 3) is for the triangle strip
    // where m_drawNV is the number of vertices per cross section
    // and each link drawing is divided into 3 cross sections
    //   -------------
    //   | /| /| /| /|
    //   |/ |/ |/ |/ |
    //   -------------
    //   | /| /| /| /|
    //   |/ |/ |/ |/ |
    //   -------------  <--- this one is redundant for the next link
    m_numOfVertices = (m_Parameters.NumOfNodes-1) * (m_drawNV+1) * 4;
    unsigned int size = m_numOfVertices * 4 * 3;    // 4 for xyzw; 3 for position, normal, and texture coordinates
    m_pVertexData = new GLfloat[size];
    m_NumOfBytesOfVertexData = size * sizeof(GLfloat);
    if ( !m_pVertexData ) {
        std::cout << "FAILED: Allocating memory for vertex data for drawing by OpenGL!" << std::endl;
        exit(-2);
    }

    glGenBuffers( 1, &m_GLVertexArray );
    glBindBuffer( GL_ARRAY_BUFFER, m_GLVertexArray );
    glBufferData( GL_ARRAY_BUFFER, m_NumOfBytesOfVertexData, NULL, GL_DYNAMIC_DRAW );
    glBindBuffer( GL_ARRAY_BUFFER, 0 );

    #ifdef TAPs_USE_CUDA
    {
        float * csVertexData = new float[ m_drawNV * 4 * 3 ];   // 4 for xyzw; 3 for V, N, and TC
        int idx = 0;
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            csVertexData[idx++] = m_drawCrossSectionVertices[i][0];
            csVertexData[idx++] = m_drawCrossSectionVertices[i][1];
            csVertexData[idx++] = m_drawCrossSectionVertices[i][2];
            csVertexData[idx++] = 1.0f;
        }
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            csVertexData[idx++] = m_drawCrossSectionNormals[i][0];
            csVertexData[idx++] = m_drawCrossSectionNormals[i][1];
            csVertexData[idx++] = m_drawCrossSectionNormals[i][2];
            csVertexData[idx++] = 1.0f;
        }
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            csVertexData[idx++] = 0.0f;
            csVertexData[idx++] = m_drawCrossSectionTexCoords[i];
            csVertexData[idx++] = 0.0f;
            csVertexData[idx++] = 1.0f;
        }
        TAPs::CUDA::GL__InitailizeDataForElasticRodModel_Drawing( m_drawNV, csVertexData, m_GLVertexArray );
        delete [] csVertexData;
    }
    #endif//TAPs_USE_CUDA
}


template <typename T>
void ModelElasticRod<T>::ClearGL ()
{
    #ifdef TAPs_USE_CUDA
    if ( m_GLVertexArray > 0 ) {
        TAPs::CUDA::GL__UnregisterBufferObject( m_GLVertexArray );
    }
    #endif//TAPs_USE_CUDA

    if ( m_GLVertexArray > 0 ) {
        glDeleteBuffers( 1, &m_GLVertexArray );
        m_GLVertexArray = 0;
    }
    if ( m_pVertexData ) {
        delete [] m_pVertexData;
        m_pVertexData = NULL;
    }

    //*
    if ( m_drawCrossSectionVertices ) {
        delete [] m_drawCrossSectionVertices;
        m_drawCrossSectionVertices = NULL;
    }
    if ( m_drawCrossSectionNormals ) {
        delete [] m_drawCrossSectionNormals;
        m_drawCrossSectionNormals = NULL;
    }
    if ( m_drawCrossSectionTexCoords ) {
        delete [] m_drawCrossSectionTexCoords;
        m_drawCrossSectionTexCoords = NULL;
    }
    //*/

    //*
    if ( vertices ) {
        for ( int i = 0; i < 3; ++i ) {
            delete [] vertices[i];
        }
        delete [] vertices;
        vertices = NULL;
    }
    if ( normals ) {
        for ( int i = 0; i < 3; ++i ) {
            delete [] normals[i];
        }
        delete [] normals;
        normals = NULL;
    }
    if ( texCoords ) {
        for ( int i = 0; i < 3; ++i ) {
            delete [] texCoords[i];
        }
        delete [] texCoords;
        texCoords = NULL;
    }
    if ( sVertices ) {
        delete [] sVertices;
        sVertices = NULL;
    }
    //*/
}


// Set how many vertices per cross section for drawing
template <typename T>
void ModelElasticRod<T>::SetRenderingForNumberOfVerticesPerCrossSection ( unsigned int i )
{
    if      ( i < 2 )   i = 2;
    else if ( i > 32 )  i = 32;
    m_drawNV = i;
    ClearGL();
    InitGL();
}


#ifdef  ELASTIC_MODEL_SIMPLEST_DRAW
template <typename T>
void ModelElasticRod<T>::Draw ()
{
    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();

    Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition();
    (currNode++)->Centerline[IdxCurr].Draw();   // draw the first point
    while ( currNode != m_Nodes.end() ) {
        pos1 = currNode->Centerline[IdxCurr].GetPosition();

        // Draw a line connecting the previous point to this point
        glBegin( GL_LINES );
            glVertex3fv( pos0.GetDataFloat() );
            glVertex3fv( pos1.GetDataFloat() );
        glEnd();

        pos0 = pos1;
        (currNode++)->Centerline[IdxCurr].Draw();
    }
}
#endif//ELASTIC_MODEL_SIMPLEST_DRAW


#ifdef  ELASTIC_MODEL_GENERALIZED_CYLINDERS_DRAW
template <typename T>
void ModelElasticRod<T>::Draw ()
{
    //static Vector3<T> vertices[3][m_drawNV];  // each segment is drawn with three cross sections
    //static Vector3<T> normals[3][m_drawNV];       // each segment is drawn with three cross sections
    //static Vector3<T> sVertices[m_drawNV];        // scale vertices

    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();

    Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition(), pos2;
    Quaternion<T> ori1, ori0 = currNode->Orientation[IdxCurr];
    T scale = m_Parameters.Radius;  // radius of generalized cylinder

    int count = 1;
    ++currNode;     // start from the 2nd point

    glPushAttrib( GL_ENABLE_BIT );
    glEnable( GL_COLOR_MATERIAL );
    glColor3f( 0, 0.75, 0 );
    while ( ++count < m_Parameters.NumOfNodes ) {
        pos1 = currNode->Centerline[IdxCurr].GetPosition();
        ori1 = currNode->Orientation[IdxCurr];
        pos2 = (++currNode)->Centerline[IdxCurr].GetPosition();

        // Draw a line connecting the previous point to this point
        glBegin( GL_LINES );
            glVertex3fv( pos0.GetDataFloat() );
            glVertex3fv( pos1.GetDataFloat() );
        glEnd();

        // Get rotation matrices
        Matrix4x4<T> rotMat0( ori0.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMat1( ori1.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMatAvg( (rotMat0+rotMat1)/2 );
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            sVertices[i] = scale*m_drawCrossSectionVertices[i];
        }

        // Compute the cross section vertices and normals for middle point from pos0 to pos1
        Vector3<T> middlePt0( (pos1+pos0)/2 );
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            normals[0][i]  = Matrix4x4MultVector3( rotMat0, m_drawCrossSectionNormals[i] );
            vertices[0][i] = Matrix4x4MultVector3( rotMat0, sVertices[i] ) + middlePt0;
        }
        // Compute the cross section vertices and normals for pos1
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            normals[1][i]  = Matrix4x4MultVector3( rotMatAvg, m_drawCrossSectionNormals[i] );
            vertices[1][i] = Matrix4x4MultVector3( rotMatAvg, sVertices[i] ) + pos1;
        }
        // Compute the cross section vertices and normals for middle point from pos1 to pos2
        Vector3<T> middlePt1( (pos2+pos1)/2 );
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            normals[2][i]  = Matrix4x4MultVector3( rotMat1, m_drawCrossSectionNormals[i] );
            vertices[2][i] = Matrix4x4MultVector3( rotMat1, sVertices[i] ) + middlePt1;
        }

        // Draw triangle strip from middlePt0 to pos1
        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
                glNormal3fv( normals[0][i].GetDataFloat() );
                glVertex3fv( vertices[0][i].GetDataFloat() );
            }
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
            glNormal3fv( normals[0][0].GetDataFloat() );
            glVertex3fv( vertices[0][0].GetDataFloat() );
        glEnd();

        // Draw triangle strip from pos1 to middlePt1
        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glNormal3fv( normals[2][i].GetDataFloat() );
                glVertex3fv( vertices[2][i].GetDataFloat() );
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
            }
            glNormal3fv( normals[2][0].GetDataFloat() );
            glVertex3fv( vertices[2][0].GetDataFloat() );
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
        glEnd();

        // Next position
        pos0 = pos1;
        ori0 = ori1;
    }

    // Draw all points in red color
    currNode = m_Nodes.begin();
    glColor3f( 1, 0, 0 );
    while ( currNode != m_Nodes.end() ) {
        (currNode++)->Centerline[IdxCurr].Draw();
    }
    glPopAttrib();
}
#endif//ELASTIC_MODEL_GENERALIZED_CYLINDERS_DRAW


#ifdef  TAPs_USE_GLSL
template <typename T>
void ModelElasticRod<T>::Draw ()
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );

    /*
    // Draw all collided points in red color
    //glDisable( GL_TEXTURE_2D );
    glEnable( GL_COLOR_MATERIAL );
    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();
    currNode = m_Nodes.begin();

    glColor3f( 1, 0, 0 );
    while ( currNode != m_Nodes.end() ) {

        // DEBUG
        if ( currNode->IsCollide )

        currNode->Centerline[IdxCurr].Draw( 2 );
        ++currNode;
    }
    //*/

    //DrawOrientations();

    /*
    #ifdef  TAPs_ADVANCED_SIMULATION
    {
        // Draw all clustered points in green color
        glEnable( GL_COLOR_MATERIAL );
        SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();
        currNode = m_Nodes.begin();
        glColor3f( 0, 1, 0 );
        while ( currNode != m_Nodes.end() ) {
            if ( currNode->SimFlags.CheckSimulationConstraints( Enum::AddOn::CLUSTERED ) )
            currNode->Centerline[IdxCurr].Draw( 1.5 );
            ++currNode;
        }
    }
    #endif//TAPs_ADVANCED_SIMULATION
    //*/

    glPopAttrib();



    // Start rendering the elastic rod
    Rendering.glslProgObj->BeginGLSL();

    //glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT );
    glPushAttrib( GL_ALL_ATTRIB_BITS );

    // DEBUG
    //m_CD.Draw();  // draw BVH tree
    #ifdef  TAPs_ADD_KNOT_RECOGNITION
    //m_KR.Draw();  // draw knot recognition
    #endif//TAPs_ADD_KNOT_RECOGNITION

    // Set texture to texture unit 0
    #ifdef  TAPs_USE_WXWIDGETS
    Rendering.Texture.EnableTextureTarget();
    glActiveTexture( GL_TEXTURE0 );
    Rendering.Texture.BindTexture( 0 );
    Rendering.Texture.UseParameters();
    Rendering.glslProgObj->SetUniform1i( "texture", 0 );
    TAPs::OpenGL::Fn::CHECK_GL_ERROR();
    #endif//TAPs_USE_WXWIDGETS

    // Set color material
    Rendering.Material.ApplyMaterial();

#if ER_TIMING_RENDERING != 0
    // TIMING
    static int times = -1;
    ++times;
    clock_t startTime, endTime;
    static clock_t cpu_compV = 0, cpu_drawV = 0;
    static clock_t gpu_compV = 0, gpu_drawV = 0;
    static bool isFirstRun = true;
#endif

#ifdef  TAPs_USE_CUDA
    // Use GPU to compute the generalized cylinder to the already mapped OpenGL buffer object
    if ( m_Parameters.EnableCUDA_GenDrawData && m_Parameters.EnableCUDA )
    {
    #if ER_TIMING_RENDERING != 0
        startTime = clock();    // TIMING
    #endif
        
        m_CUDA.GenCylinderVertexDataForDrawing( m_GLVertexArray, m_drawNV );

    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            gpu_compV += endTime - startTime;   // TIMING
        }
        startTime = clock();    // TIMING
    #endif

        // Draw by OpenGL 2.1
        static GLsizei stride = 12*sizeof(GLfloat);
        glEnableClientState( GL_VERTEX_ARRAY );
        glEnableClientState( GL_NORMAL_ARRAY );
        glEnableClientState( GL_TEXTURE_COORD_ARRAY );
        glBindBuffer( GL_ARRAY_BUFFER, m_GLVertexArray );

        glVertexPointer( 4, GL_FLOAT, stride, 0 );
        glNormalPointer( GL_FLOAT, stride, (GLubyte*)NULL + 4*sizeof(GLfloat) );
        glTexCoordPointer( 4, GL_FLOAT, stride, (GLubyte*)NULL + 8*sizeof(GLfloat) );

        GLint first = 0;
        GLsizei primCount = (m_drawNV+1)*2;

        for ( int i = 0; i < (m_Parameters.NumOfNodes-1)*2 - 1; ++i ) {
            glDrawArrays( GL_TRIANGLE_STRIP, first, primCount );
            first += primCount;
        }

        glBindBuffer( GL_ARRAY_BUFFER, 0 );
        glDisableClientState( GL_VERTEX_ARRAY );
        glDisableClientState( GL_NORMAL_ARRAY );
        glDisableClientState( GL_TEXTURE_COORD_ARRAY );

    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            gpu_drawV += endTime - startTime;   // TIMING
        }
    #endif

    }
    else
#endif//TAPs_USE_CUDA
    {
        // Use CPU to compute the generalized cylinder and send the draw data to OpenGL
        #ifdef  TAPs_ER_DRAW_WITH_SUBDIVISION
            GenAndDrawCylinderVertexData_SUBDIVISION();
        #else //TAPs_ER_DRAW_WITH_SUBDIVISION
            GenAndDrawCylinderVertexData();
        #endif//TAPs_ER_DRAW_WITH_SUBDIVISION
    }

#if ER_TIMING_RENDERING != 0
     TIMING
    if ( times == ER_TIMING_RENDERING_STOP ) {
        std::cout << "times: " << times << "\n";
        std::cout << "Compute drawing data (CPU) ACC TIME: " << 1000.0 * cpu_compV / CLOCKS_PER_SEC << " ms.\n";
        std::cout << "Compute drawing data (GPU) ACC TIME: " << 1000.0 * gpu_compV / CLOCKS_PER_SEC << " ms.\n";
        std::cout << "Draw by glBegin/glEnd ACC TIME: " << 1000.0 * cpu_drawV / CLOCKS_PER_SEC << " ms.\n";
        std::cout << "Draw by glDrawArrays  ACC TIME: " << 1000.0 * gpu_drawV / CLOCKS_PER_SEC << " ms.\n";
        std::cout << std::endl;
        exit(-1);
    }
#endif

    /*
    // DEBUG
    // Draw the texture
    {
        GLfloat s = 0.5;
        glBegin( GL_QUADS );
        glNormal3f( 0, 0, 1 );
        glTexCoord3f( 0, 0, 0 );
        glVertex3f( -s, -s, 0 );
        glNormal3f( 0, 0, 1 );
        glTexCoord3f( 1, 0, 0 );
        glVertex3f(  s, -s, 0 );
        glNormal3f( 0, 0, 1 );
        glTexCoord3f( 1, 1, 0 );
        glVertex3f(  s,  s, 0 );
        glNormal3f( 0, 0, 1 );
        glTexCoord3f( 0, 1, 0 );
        glVertex3f( -s,  s, 0 );
        glEnd();
    }
    //*/
    
    #ifdef  TAPs_USE_WXWIDGETS
    Rendering.Texture.UnbindTexture();
    #endif//TAPs_USE_WXWIDGETS

    glPopAttrib();

    Rendering.glslProgObj->EndGLSL();
}


// Draw Orientations
template <typename T>
void ModelElasticRod<T>::DrawOrientations () const
{
    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();

    Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition(), pos2;
    Quaternion<T> ori1, ori0 = currNode->Orientation[IdxCurr];
    T scale = 8*m_Parameters.Radius;    // radius of generalized cylinder

    // Draw orientation

    int count = 1;
    ++currNode;     // start from the 2nd point

    static GLfloat red_color[4] = { 1, 0, 0, 1 };
    static GLfloat green_color[4] = { 0, 1, 0, 1 };
    static GLfloat blue_color[4] = { 0, 0, 1, 1 };
    
    glPushAttrib( GL_ENABLE_BIT | GL_CURRENT_BIT );
    glDisable( GL_TEXTURE_2D );

    // Set color material
    //Rendering.Material.ApplyMaterial();

    glEnable( GL_COLOR_MATERIAL );

    while ( ++count < m_Parameters.NumOfNodes ) {
        pos1 = currNode->Centerline[IdxCurr].GetPosition();
        ori1 = currNode->Orientation[IdxCurr];
        pos2 = (++currNode)->Centerline[IdxCurr].GetPosition();

        // Draw a line connecting the previous point to this point
        //glColor3f( 0.5, 0.5, 0.5 );
        //glBegin( GL_LINES );
        //  glVertex3fv( pos0.GetDataFloat() );
        //  glVertex3fv( pos1.GetDataFloat() );
        //glEnd();

        // Get rotation matrices
        Matrix4x4<T> rotMat0( ori0.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMat1( ori1.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMatAvg( (rotMat0+rotMat1)/2 );
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            sVertices[i] = scale*m_drawCrossSectionVertices[i];
        }

        Vector3<T> middlePt0( (pos1+pos0)/2 );
        Vector3<T> middlePt1( (pos2+pos1)/2 );

        // Draw orientation
        /*
        // pos0
        glLineWidth( 5 );
        glPushMatrix();
        glTranslatef( pos0[0], pos0[1], pos0[2] );
        glMultMatrixf( rotMat0.GetTransposeDataFloat() );
        glScalef( 0.1, 0.1, 0.1 );
        glBegin( GL_LINES );
            glColor3f( 1, 0, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 1, 0, 0 );
            glColor3f( 0, 1, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 1, 0 );
            glColor3f( 0, 0, 1 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 0, 1 );
        glEnd();
        glPopMatrix();
        //*/
        // middle
        glLineWidth( 1 );
        glPushMatrix();
        glTranslatef( middlePt0[0], middlePt0[1], middlePt0[2] );
        glMultMatrixf( rotMatAvg.GetTransposeDataFloat() );
        GLfloat s = 0.5;
        glScalef( s, s, s );
        
        glEnable( GL_COLOR_MATERIAL );
        glColor3f( 1, 0, 0 );
        OpenGL::OpenGLUsefulObj<T>::DrawOneHeadArrow( CGMath<T>::VectorX, s );
        glColor3f( 0, 1, 0 );
        OpenGL::OpenGLUsefulObj<T>::DrawOneHeadArrow( CGMath<T>::VectorY, s );
        glColor3f( 0, 0, 1 );
        OpenGL::OpenGLUsefulObj<T>::DrawOneHeadArrow( CGMath<T>::VectorZ, s );

        /*
        glBegin( GL_LINES );
            glColor3f( 1, 0, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 1, 0, 0 );
            glColor3f( 0, 1, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 1, 0 );
            glColor3f( 0, 0, 1 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 0, 1 );
        glEnd();
        //*/

        glPopMatrix();
        /*
        // pos1
        glLineWidth( 5 );
        glPushMatrix();
        glTranslatef( pos1[0], pos1[1], pos1[2] );
        glMultMatrixf( rotMat1.GetTransposeDataFloat() );
        glScalef( 0.1, 0.1, 0.1 );
        glBegin( GL_LINES );
            glColor3f( 1, 0, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 1, 0, 0 );
            glColor3f( 0, 1, 0 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 1, 0 );
            glColor3f( 0, 0, 1 ); 
            glVertex3f( 0, 0, 0 );
            glVertex3f( 0, 0, 1 );
        glEnd();
        glPopMatrix();
        //*/

        // Next position
        pos0 = pos1;
        ori0 = ori1;
    }
    glPopAttrib();
} // END: DrawOrientations () const


// Generate cylinder vextex data and draw the data by OpenGL
template <typename T>
void ModelElasticRod<T>::GenAndDrawCylinderVertexData ()
{
    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();

    Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition(), pos2;
    Quaternion<T> ori1, ori0 = currNode->Orientation[IdxCurr];
    T scale = m_Parameters.Radius;  // radius of generalized cylinder

    int count = 1;
    ++currNode;     // start from the 2nd point

    // Scale the cross vertices by the radius
    //#pragma omp parallel for
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        sVertices[i] = scale*m_drawCrossSectionVertices[i];
    }

    while ( ++count < m_Parameters.NumOfNodes ) {

    #if ER_TIMING_RENDERING != 0
        startTime = clock();    // TIMING
    #endif

        pos1 = currNode->Centerline[IdxCurr].GetPosition();
        ori1 = currNode->Orientation[IdxCurr];
        pos2 = (++currNode)->Centerline[IdxCurr].GetPosition();

        // Get rotation matrices
        Matrix4x4<T> rotMat0( ori0.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMat1( ori1.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMatAvg( (rotMat0+rotMat1)/2 );

        // Compute the cross section vertices, normals, and texture coordinates for middle point from pos0 to pos1
        Vector3<T> middlePt0( (pos1+pos0)/2 );
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[0][i].SetXYZ( 0.0, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[0][i]  = Matrix4x4MultVector3( rotMat0, m_drawCrossSectionNormals[i] );
            vertices[0][i] = Matrix4x4MultVector3( rotMat0, sVertices[i] ) + middlePt0;
        }
        // Compute the cross section vertices, normals, and texture coordinates for pos1
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[1][i].SetXYZ( 0.5, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[1][i]  = Matrix4x4MultVector3( rotMatAvg, m_drawCrossSectionNormals[i] );
            vertices[1][i] = Matrix4x4MultVector3( rotMatAvg, sVertices[i] ) + pos1;
        }
        // Compute the cross section vertices, normals, and texture coordinates for middle point from pos1 to pos2
        Vector3<T> middlePt1( (pos2+pos1)/2 );
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[2][i].SetXYZ( 1.0, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[2][i]  = Matrix4x4MultVector3( rotMat1, m_drawCrossSectionNormals[i] );
            vertices[2][i] = Matrix4x4MultVector3( rotMat1, sVertices[i] ) + middlePt1;
        }
        
    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            cpu_compV += endTime - startTime;   // TIMING
        }
    #endif

        //glBegin( GL_TRIANGLE_STRIP );
        //  for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        //      glTexCoord3fv( texCoords[2][i].GetDataFloat() );
        //      glNormal3fv( normals[2][i].GetDataFloat() );
        //      glVertex3fv( vertices[2][i].GetDataFloat() );
        //      glTexCoord3fv( texCoords[0][i].GetDataFloat() );
        //      glNormal3fv( normals[0][i].GetDataFloat() );
        //      glVertex3fv( vertices[0][i].GetDataFloat() );
        //  }
        //  glTexCoord3fv( texCoords[2][0].GetDataFloat() );
        //  glNormal3fv( normals[2][0].GetDataFloat() );
        //  glVertex3fv( vertices[2][0].GetDataFloat() );
        //  glTexCoord3fv( texCoords[0][0].GetDataFloat() );
        //  glNormal3fv( normals[0][0].GetDataFloat() );
        //  glVertex3fv( vertices[0][0].GetDataFloat() );
        //glEnd();

        // Two triangle strips from middlePt0 to pos1 and from pos1 to middlePt1
        // Draw triangle strip from middlePt0 to pos1

    #if ER_TIMING_RENDERING != 0
        startTime = clock();    // TIMING
    #endif

        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glTexCoord3fv( texCoords[1][i].GetDataFloat() );
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
                glTexCoord3fv( texCoords[0][i].GetDataFloat() );
                glNormal3fv( normals[0][i].GetDataFloat() );
                glVertex3fv( vertices[0][i].GetDataFloat() );
            }
            glTexCoord3fv( texCoords[1][0].GetDataFloat() );
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
            glTexCoord3fv( texCoords[0][0].GetDataFloat() );
            glNormal3fv( normals[0][0].GetDataFloat() );
            glVertex3fv( vertices[0][0].GetDataFloat() );
        glEnd();

        // Draw triangle strip from pos1 to middlePt1
        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glTexCoord3fv( texCoords[2][i].GetDataFloat() );
                glNormal3fv( normals[2][i].GetDataFloat() );
                glVertex3fv( vertices[2][i].GetDataFloat() );
                glTexCoord3fv( texCoords[1][i].GetDataFloat() );
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
            }
            glTexCoord3fv( texCoords[2][0].GetDataFloat() );
            glNormal3fv( normals[2][0].GetDataFloat() );
            glVertex3fv( vertices[2][0].GetDataFloat() );
            glTexCoord3fv( texCoords[1][0].GetDataFloat() );
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
        glEnd();

    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            cpu_drawV += endTime - startTime;   // TIMING
        }
    #endif

        // Next position
        pos0 = pos1;
        ori0 = ori1;
    }
}


//-----------------------------------------------------------------------------
#ifdef  TAPs_ER_DRAW_WITH_SUBDIVISION
//-----------------------------------------------------------------------------

// Generate cylinder vextex data (with Chaikin's algorithm) and draw the data by OpenGL
template <typename T>
void ModelElasticRod<T>::GenAndDrawCylinderVertexData_SUBDIVISION ()
{
    // Every time 
    // P_i ------------------------------ P_j
    // P_i ----- Q_i ----------- R_i ---- P_j
    // Q_i = 0.75*P_i + 0.25P_j
    // R_i = 0.25*P_i + 0.75P_j
    //
    // Level  #Pts  Kept (computed pts)
    //   0     0     0                      Level 0 is the original control points
    //   1     2     1                      1st subdivision (#Pts =  0 +  2)
    //   2     6     2                      2nd subdivision (#Pts =  2 +  4)
    //   3    14     3                      3rd subdivision (#Pts =  6 +  8)
    //   4    30     4                      3rd subdivision (#Pts = 14 + 16)

    static const unsigned int level = 2;
    static const unsigned int sizePts = ( 2 << (level+1) ) - 2;
    static Vector3<T>   posKept[level];
    static Matrix4x4<T> rotMatKept[level];
    static Vector3<T>   posPts[sizePts];
    static Matrix4x4<T> rotMatPts[sizePts];

    SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();

    Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition(), pos2;
    Quaternion<T> ori1, ori0 = currNode->Orientation[IdxCurr];
    T scale = m_Parameters.Radius;  // radius of generalized cylinder

    //scale *= 2;

    int count = 1;
    ++currNode;     // start from the 2nd point

    // Scale the cross section vertices by the radius
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        sVertices[i] = scale*m_drawCrossSectionVertices[i];
    }

    // Initialize the kept data
    for ( int i = 0; i < level; ++i ) {
        posKept[i] = pos0;
        rotMatKept[i] = ori0.GetRotationMatrix4x4();
    }

    while ( ++count < m_Parameters.NumOfNodes ) {

        // Get positions and orientations
        pos1 = currNode->Centerline[IdxCurr].GetPosition();
        ori1 = currNode->Orientation[IdxCurr];
        pos2 = (++currNode)->Centerline[IdxCurr].GetPosition();
        // Get rotation matrices
        Matrix4x4<T> rotMat0( ori0.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMat1( ori1.GetRotationMatrix4x4() );
        // Get positions in the middles (i.e. the orientation positions)
        Vector3<T> midPt0( (pos1+pos0)/2 );
        Vector3<T> midPt1( (pos2+pos1)/2 );

        /*
        // Generate level 1
        GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( midPt0, rotMat0, midPt1, rotMat1, posPts[0], rotMatPts[0], posPts[1], rotMatPts[1] );
        // Draw level 1
        GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posKept[0], rotMatKept[0], posPts[0], rotMatPts[0], 0.0, 0.5 );
        GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posPts[0], rotMatPts[0], posPts[1], rotMatPts[1], 0.5, 1.0 );
        // Next position
        posKept[0] = posPts[1];
        rotMatKept[0] = rotMatPts[1];
        //*/


        //if ( count <= 10 || count >= 50 )
        {
            // Generate level 1
            GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( midPt0, rotMat0, midPt1, rotMat1, posPts[0], rotMatPts[0], posPts[1], rotMatPts[1] );
            // Generate level 2
            GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( posKept[0], rotMatKept[0], posPts[0], rotMatPts[0], posPts[2], rotMatPts[2], posPts[3], rotMatPts[3] );
            GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( posPts[0], rotMatPts[0], posPts[1], rotMatPts[1], posPts[4], rotMatPts[4], posPts[5], rotMatPts[5] );
            // Draw level 2
            GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posKept[1], rotMatKept[1], posPts[2], rotMatPts[2], 0.0, 0.25 );
            GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posPts[2], rotMatPts[2], posPts[3], rotMatPts[3], 0.25, 0.5 );
            GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posPts[3], rotMatPts[3], posPts[4], rotMatPts[4], 0.5, 0.75 );
            GenAndDrawCylinderVertexData_SUBDIVISION_Draw( posPts[4], rotMatPts[4], posPts[5], rotMatPts[5], 0.75, 1.0 );

            /*
            // DEBUG -- TEST DRAWING
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                sVertices[i] *= 1.05;
            }
            //*/

        }

        // Next position
        posKept[0] = posPts[1];
        rotMatKept[0] = rotMatPts[1];
        posKept[1] = posPts[5];
        rotMatKept[1] = rotMatPts[5];

        pos0 = pos1;
        ori0 = ori1;
    }









    //SetOfElasticRodNodes::const_iterator currNode = m_Nodes.begin();
    //Vector3<T> pos1, pos0 = currNode->Centerline[IdxCurr].GetPosition(), pos2;
    //Quaternion<T> ori1, ori0 = currNode->Orientation[IdxCurr];
    //T scale = m_Parameters.Radius;    // radius of generalized cylinder
    //int count = 1;
    //++currNode;       // start from the 2nd point

    /*
    currNode = m_Nodes.begin();
    pos0 = currNode->Centerline[IdxCurr].GetPosition();
    ori0 = currNode->Orientation[IdxCurr];
    scale = m_Parameters.Radius;    // radius of generalized cylinder
    count = 1;
    ++currNode;     // start from the 2nd point
    
    // Scale the cross section vertices by the radius
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        sVertices[i] = scale*m_drawCrossSectionVertices[i];
    }

    while ( ++count < m_Parameters.NumOfNodes ) {

    #if ER_TIMING_RENDERING != 0
        startTime = clock();    // TIMING
    #endif

        pos1 = currNode->Centerline[IdxCurr].GetPosition();
        ori1 = currNode->Orientation[IdxCurr];
        pos2 = (++currNode)->Centerline[IdxCurr].GetPosition();

        // Get rotation matrices
        Matrix4x4<T> rotMat0( ori0.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMat1( ori1.GetRotationMatrix4x4() );
        Matrix4x4<T> rotMatAvg( (rotMat0+rotMat1)/2 );
        //#pragma omp parallel for

        // Compute the cross section vertices, normals, and texture coordinates for middle point from pos0 to pos1
        Vector3<T> middlePt0( (pos1+pos0)/2 );
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[0][i].SetXYZ( 0.0, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[0][i]  = Matrix4x4MultVector3( rotMat0, m_drawCrossSectionNormals[i] );
            vertices[0][i] = Matrix4x4MultVector3( rotMat0, sVertices[i] ) + middlePt0;
        }
        // Compute the cross section vertices, normals, and texture coordinates for pos1
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[1][i].SetXYZ( 0.5, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[1][i]  = Matrix4x4MultVector3( rotMatAvg, m_drawCrossSectionNormals[i] );
            vertices[1][i] = Matrix4x4MultVector3( rotMatAvg, sVertices[i] ) + pos1;
        }
        // Compute the cross section vertices, normals, and texture coordinates for middle point from pos1 to pos2
        Vector3<T> middlePt1( (pos2+pos1)/2 );
        //#pragma omp parallel for
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            texCoords[2][i].SetXYZ( 1.0, m_drawCrossSectionTexCoords[i], 0.0 );
            normals[2][i]  = Matrix4x4MultVector3( rotMat1, m_drawCrossSectionNormals[i] );
            vertices[2][i] = Matrix4x4MultVector3( rotMat1, sVertices[i] ) + middlePt1;
        }
        
    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            cpu_compV += endTime - startTime;   // TIMING
        }
    #endif

        //glBegin( GL_TRIANGLE_STRIP );
        //  for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        //      glTexCoord3fv( texCoords[2][i].GetDataFloat() );
        //      glNormal3fv( normals[2][i].GetDataFloat() );
        //      glVertex3fv( vertices[2][i].GetDataFloat() );
        //      glTexCoord3fv( texCoords[0][i].GetDataFloat() );
        //      glNormal3fv( normals[0][i].GetDataFloat() );
        //      glVertex3fv( vertices[0][i].GetDataFloat() );
        //  }
        //  glTexCoord3fv( texCoords[2][0].GetDataFloat() );
        //  glNormal3fv( normals[2][0].GetDataFloat() );
        //  glVertex3fv( vertices[2][0].GetDataFloat() );
        //  glTexCoord3fv( texCoords[0][0].GetDataFloat() );
        //  glNormal3fv( normals[0][0].GetDataFloat() );
        //  glVertex3fv( vertices[0][0].GetDataFloat() );
        //glEnd();

        // Two triangle strips from middlePt0 to pos1 and from pos1 to middlePt1
        // Draw triangle strip from middlePt0 to pos1

    #if ER_TIMING_RENDERING != 0
        startTime = clock();    // TIMING
    #endif

        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glTexCoord3fv( texCoords[1][i].GetDataFloat() );
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
                glTexCoord3fv( texCoords[0][i].GetDataFloat() );
                glNormal3fv( normals[0][i].GetDataFloat() );
                glVertex3fv( vertices[0][i].GetDataFloat() );
            }
            glTexCoord3fv( texCoords[1][0].GetDataFloat() );
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
            glTexCoord3fv( texCoords[0][0].GetDataFloat() );
            glNormal3fv( normals[0][0].GetDataFloat() );
            glVertex3fv( vertices[0][0].GetDataFloat() );
        glEnd();

        // Draw triangle strip from pos1 to middlePt1
        glBegin( GL_TRIANGLE_STRIP );
            for ( unsigned int i = 0; i < m_drawNV; ++i ) {
                glTexCoord3fv( texCoords[2][i].GetDataFloat() );
                glNormal3fv( normals[2][i].GetDataFloat() );
                glVertex3fv( vertices[2][i].GetDataFloat() );
                glTexCoord3fv( texCoords[1][i].GetDataFloat() );
                glNormal3fv( normals[1][i].GetDataFloat() );
                glVertex3fv( vertices[1][i].GetDataFloat() );
            }
            glTexCoord3fv( texCoords[2][0].GetDataFloat() );
            glNormal3fv( normals[2][0].GetDataFloat() );
            glVertex3fv( vertices[2][0].GetDataFloat() );
            glTexCoord3fv( texCoords[1][0].GetDataFloat() );
            glNormal3fv( normals[1][0].GetDataFloat() );
            glVertex3fv( vertices[1][0].GetDataFloat() );
        glEnd();

    #if ER_TIMING_RENDERING != 0
        endTime = clock();  // TIMING
        if ( isFirstRun ) {
            isFirstRun = false;
        }
        else {
            cpu_drawV += endTime - startTime;   // TIMING
        }
    #endif

        // Next position
        pos0 = pos1;
        ori0 = ori1;
    }
    //*/
}


// Generate cylinder vextex data
template <typename T>
void ModelElasticRod<T>::GenAndDrawCylinderVertexData_SUBDIVISION_Gen (
    unsigned int level,             
    Vector3<T> posKept[],           
    Matrix4x4<T> rotMatKept[],      
    Vector3<T> posPts[],            
    Matrix4x4<T> rotMatPts[]        
)
{
    if ( level < 2 ) return;    // this fn only for level >= 2

    int idxk = level - 1;   // index to kept positions
    int size = 2 << (level-1);
    int idx0 = size - 2;    // I/P index
    size = size << 1;
    int idx1 = size - 2;    // O/P index
    GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( posKept[idxk], rotMatKept[idxk], posPts[idx0], rotMatPts[idx0], posPts[idx1], rotMatPts[idx1], posPts[idx1+1], rotMatPts[idx1+1] );
    for ( int i = 1; i < size; ++i ) {
        ++idx0;
        idx1 += 2;
        GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts( posPts[idx0], rotMatPts[idx0], posPts[idx0+1], rotMatPts[idx0+1], posPts[idx1], rotMatPts[idx1], posPts[idx1+1], rotMatPts[idx1+1] );
    }
}


// Draw cylinder vextex data
template <typename T>
void ModelElasticRod<T>::GenAndDrawCylinderVertexData_SUBDIVISION_Draw (
    Vector3<T> const & P,           
    Matrix4x4<T> const & RotMatP,   
    Vector3<T> const & Q,           
    Matrix4x4<T> const & RotMatQ,   
    T   texXstart,                  
    T   texXend                     
)
{
    // Compute the cross section vertices, normals, and texture coordinates for point P
    //#pragma omp parallel for
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        texCoords[0][i].SetXYZ( texXstart, m_drawCrossSectionTexCoords[i], 0.0 );
        normals[0][i]  = Matrix4x4MultVector3( RotMatP, m_drawCrossSectionNormals[i] );
        vertices[0][i] = Matrix4x4MultVector3( RotMatP, sVertices[i] ) + P;
    }
    // Compute the cross section vertices, normals, and texture coordinates for point Q
    //#pragma omp parallel for
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        texCoords[2][i].SetXYZ( texXend, m_drawCrossSectionTexCoords[i], 0.0 );
        normals[2][i]  = Matrix4x4MultVector3( RotMatQ, m_drawCrossSectionNormals[i] );
        vertices[2][i] = Matrix4x4MultVector3( RotMatQ, sVertices[i] ) + Q;
    }

    // Draw triangle strip from P to Q
    glBegin( GL_TRIANGLE_STRIP );
        for ( unsigned int i = 0; i < m_drawNV; ++i ) {
            glTexCoord3fv( texCoords[2][i].GetDataFloat() );
            glNormal3fv( normals[2][i].GetDataFloat() );
            glVertex3fv( vertices[2][i].GetDataFloat() );
            glTexCoord3fv( texCoords[0][i].GetDataFloat() );
            glNormal3fv( normals[0][i].GetDataFloat() );
            glVertex3fv( vertices[0][i].GetDataFloat() );
        }
        glTexCoord3fv( texCoords[2][0].GetDataFloat() );
        glNormal3fv( normals[2][0].GetDataFloat() );
        glVertex3fv( vertices[2][0].GetDataFloat() );
        glTexCoord3fv( texCoords[0][0].GetDataFloat() );
        glNormal3fv( normals[0][0].GetDataFloat() );
        glVertex3fv( vertices[0][0].GetDataFloat() );
    glEnd();
}


// Chaikin's subdivision
template <typename T>
void ModelElasticRod<T>::GenAndDrawCylinderVertexData_SUBDIVISION_Chaikin_Pts (
    Vector3<T> const & Pi,          
    Matrix4x4<T> const & RotMatPi,  
    Vector3<T> const & Pj,          
    Matrix4x4<T> const & RotMatPj,  
    Vector3<T> & Q,         
    Matrix4x4<T> & RotMatQ, 
    Vector3<T> & R,         
    Matrix4x4<T> & RotMatR  
)
{
    Q = 0.75*Pi + 0.25*Pj;
    R = 0.25*Pi + 0.75*Pj;
    RotMatQ = 0.75*RotMatPi + 0.25*RotMatPj;
    RotMatR = 0.25*RotMatPi + 0.75*RotMatPj;
}

//-----------------------------------------------------------------------------
#endif//TAPs_ER_DRAW_WITH_SUBDIVISION
//-----------------------------------------------------------------------------


// Initialize GLSL
template <typename T>
bool ModelElasticRod<T>::InitGLSL ()
{
    //if ( g_IsGLSLInit )   return true;

    if ( Rendering.GetVersionMajorNumber() < 2 ) {
        return false;
    }

    #ifdef  TAPs_USE_WXWIDGETS
    //std::string texFile( Rendering.GetTAPsResourcePath() + "Textures/Common/four.jpg" );
    std::string texFile( Rendering.GetTAPsResourcePath() + "Textures/Common/Default.jpg" );
    //std::string texFile( Rendering.GetTAPsResourcePath() + "Textures/ForModels/rope_01.jpg" );
    //std::string texFile( Rendering.GetTAPsResourcePath() + "Textures/ForModels/big_brick_01.jpg" );
    if ( Rendering.InitTexture2D(  texFile ) ) {
        std::cout << "Rendering Info: \n" << Rendering << "\n";
        //return Rendering.InitShaders( "GLSL_ModelElasticRod_wTextures", true, false, true );
        //g_IsGLSLInit = true;
        return Rendering.InitShaders( "GLSL_ModelElasticRod_wTextures", true, false, true );
    }
    else
    #endif//TAPs_USE_WXWIDGETS
    {
        std::cout << "Rendering Info: \n" << Rendering << "\n";
        //g_IsGLSLInit = true;
        return Rendering.InitShaders( "GLSL_ModelElasticRod", true, false, true );
    }
}
#endif//TAPs_USE_GLSL


// Multiplication of Matrix4x4 with Vector3
template <typename T>
Vector3<T> ModelElasticRod<T>::Matrix4x4MultVector3 ( Matrix4x4<T> const &M, Vector3<T> const &V ) const
{
    return Vector3<T>(
        V[0]*M(0,0) + V[1]*M(0,1) + V[2]*M(0,2) + M(0,3),
        V[0]*M(1,0) + V[1]*M(1,1) + V[2]*M(1,2) + M(1,3),
        V[0]*M(2,0) + V[1]*M(2,1) + V[2]*M(2,2) + M(2,3)
    );
}


// Compute the vertices and normals of circle corsss section
template <typename T>
void ModelElasticRod<T>::ComputeVerticesAndNormalsOfCircleCrossSection ()
{
    T stepAngle = 2*Math<T>::PI / m_drawNV;
    T angle = 0;
    for ( unsigned int i = 0; i < m_drawNV; ++i ) {
        T c = cos(angle);
        T s = sin(angle);
        m_drawCrossSectionVertices[i].SetXYZ( c, s, 0 );
        m_drawCrossSectionNormals[i] = m_drawCrossSectionVertices[i].GetUnit();
        
        // The first texture coordinate is set to zero and the last one is set to close to or equal to one.
        if ( m_drawCrossSectionVertices[i][1] >= 0 ) {
            if ( m_drawCrossSectionVertices[i][0] > 0 ) {   // the first quadrand
                m_drawCrossSectionTexCoords[i] = m_drawCrossSectionVertices[i][1]/4;
            }
            else {                                          // the second quadrand
                m_drawCrossSectionTexCoords[i] = 0.5 - m_drawCrossSectionVertices[i][1]/4;
            }
        }
        else {
            if ( m_drawCrossSectionVertices[i][0] < 0 ) {   // the third quadrand
                m_drawCrossSectionTexCoords[i] = 0.5 - m_drawCrossSectionVertices[i][1]/4;
            }
            else {                                          // the fourth quadrand
                m_drawCrossSectionTexCoords[i] = 1.0 + m_drawCrossSectionVertices[i][1]/4;
            }
        }
        angle += stepAngle;
        //std::cout << "m_drawCrossSectionVertices["<<i<<"]: " << m_drawCrossSectionVertices[i] << "\n";
        //std::cout << "m_drawCrossSectionNormals["<<i<<"]: " << m_drawCrossSectionNormals[i] << "\n";
        //std::cout << "m_drawCrossSectionTexCoords["<<i<<"]: " << m_drawCrossSectionTexCoords[i] << "\n";
    }
}
//=============================================================================
#endif // #if defined(__gl_h_) || defined(__GL_H__)


#ifdef  TAPs_USE_WXWIDGETS
//=============================================================================
    template <typename T>
    void ModelElasticRod<T>::ShowPropertyDialog ( wxWindow * parent )
    {
        if ( Dialogs.IsInitialized() ) {
            // If the dialog is shown, then close it.
            if ( Dialogs.IsShown() ) {
                Dialogs.Close();
            }
            // If the dialog is not shown, then show it.
            else {
                Dialogs.PropertyDialog<T>( 
                    parent, &m_Parameters, &m_Nodes
                    #ifdef  TAPs_USE_CUDA
                    , m_CUDA.IsUtilizingPageLockedHostMemory()
                    #endif//TAPs_USE_CUDA
                );
            }
        }
        else {
            wxMessageDialog( NULL, "Elastic Rod's Property Dialog is not available!", "Info", wxOK );
        }
    }
//=============================================================================
#endif//TAPs_USE_WXWIDGETS

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