TAPsModelElasticRod.hpp

Go to the documentation of this file.

/******************************************************************************
TAPsModelEasticRod.hpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (07/07/2009)
UPDATE          (01/27/2010)
******************************************************************************/
#ifndef TAPs_MODEL_ELASTIC_ROD_HPP
#define TAPs_MODEL_ELASTIC_ROD_HPP


//#define USE_CORDE_SIM


// FOR TIMING
#define ER_TIMING_ADV_SIM   0
#define ER_TIMING_ADV_SIM_STOP      10000.0
#define ER_TIMING_RENDERING 0
#define ER_TIMING_RENDERING_STOP    10000.0

#if ER_TIMING_ADV_SIM != 0
#define ER_TIMING_ADV_SIM_DETAILS 1     // ER_TIMING_ADV_SIM has to be 1 in order for this to be effective
#endif

// For knot recognition
//#define   TAPs_ADD_KNOT_RECOGNITION
#ifdef  TAPs_ADD_KNOT_RECOGNITION




    #ifndef TAPs_ADVANCED_SIMULATION
        #define TAPs_ADVANCED_SIMULATION
    #endif
    #ifdef  TAPs_ADVANCED_SIMULATION
        //#include "../Support/TAPsAdvSimSupport_DS.hpp"
        #include "ElasticRodSupport/TAPsElasticRodKR.hpp"
    #endif//TAPs_ADVANCED_SIMULATION
#endif//TAPs_ADD_KNOT_RECOGNITION

//#define   TAPs_ADD_INTERACTIONS
#ifdef  TAPs_ADD_INTERACTIONS
    #ifndef TAPs_ADVANCED_SIMULATION
        #define TAPs_ADVANCED_SIMULATION
    #endif//TAPs_ADVANCED_SIMULATION
    //#include "../Physics/TAPsForces.hpp"
    //#include "../Support/TAPsAdvSimCtrl.hpp"
#endif//TAPs_ADD_INTERACTIONS

//#define   TAPs_ADVANCED_SELF_COLLISION_DETECTION
#ifdef  TAPs_ADVANCED_SELF_COLLISION_DETECTION
#endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION


#include "ElasticRodSupport/TAPsElasticRodParameters.hpp"
#include "ElasticRodSupport/TAPsElasticRodNode.hpp"

// For collision detection
#include "ElasticRodSupport/TAPsElasticRodCD.hpp"

// For drawing by OpenGL
#if defined(__gl_h_) || defined(__GL_H__)
    #include "../OpenGL/TAPsOpenGLObj.hpp"
#endif

#define TAPs_ER_DRAW_WITH_SUBDIVISION

#ifdef  TAPs_ER_DRAW_WITH_SUBDIVISION
#endif//TAPs_ER_DRAW_WITH_SUBDIVISION

#ifdef  TAPs_USE_CUDA
    #include "ElasticRodSupport/TAPsElasticRod_CompByCUDA.hpp"
#endif//TAPs_USE_CUDA

// For GUIs by wxWidgets
#ifdef  TAPs_USE_WXWIDGETS
    #include "../wxWidgets/Dialogs/TAPsWXElasticRodParameters.hpp"
#endif//TAPs_USE_WXWIDGETS


BEGIN_NAMESPACE_TAPs
//=============================================================================
template <typename T>
class ModelElasticRod {

    // Member Functions -------------------------------------------------------
    friend std::ostream & operator<< ( std::ostream &output, ModelElasticRod<T> const &obj )
    {
        output << obj.StrInfo();
        return output;
    }

public:
    //enum Shape {
    //  STRAIGHT_SHAPE,
    //  HELIX_SHAPE,
    //};    //!< List of initial shapes

    // For initializing the start shape
    class ShapeInitializationParameters {
    public:
        enum Shape {
            STRAIGHT_SHAPE,
            HELIX_SHAPE,
        };  

        ShapeInitializationParameters () : Helix_Radius( 0.5 ), Helix_Pitch( 0.2 ), StartShape( STRAIGHT_SHAPE ) {}
        T Helix_Radius;
        T Helix_Pitch;
        Shape StartShape;
    };
    ShapeInitializationParameters   InitShapeParameters;    

    typedef std::vector< ElasticRodNode<T> >    SetOfElasticRodNodes;
    //-------------------------------------------------------------------------
    ModelElasticRod (
        int iNoLinks,       
        T tRadius,          
        T tTotalLength,     
        T tTotalMass,       
        Vector3<T> & posOfVertex0 = Vector3<T>(),   
        ShapeInitializationParameters ShapeParameters = ShapeInitializationParameters() 
    #ifdef  TAPs_USE_CUDA
        ,
        bool    bUseCUDAsPLHM = true 
    #endif//TAPs_USE_CUDA
    );

    //ModelElasticRod ( ModelElasticRod<T> const &obj );

    ModelElasticRod<T> * CopySegment ( int startLink, int endLink );

    virtual ~ModelElasticRod ();

    //-------------------------------------------------------------------------
    virtual std::string StrInfo () const;

    //-------------------------------------------------------------------------
    //inline ModelElasticRod<T> & operator= ( ModelElasticRod<T> const &v );

    //-------------------------------------------------------------------------
    // Get/Set Fn(s)
    // Self Collision Detection -----------------
    inline void EnableSelfCollision ( bool b = true )   { m_Parameters.EnableSelfCDR = b; }
    inline void DisableSelfCollision ( bool b = true )  { m_Parameters.EnableSelfCDR = !b; }
    inline T    GetConstantSelfCollisionResponse () const   { return m_Parameters.KselfCDR; }
    inline void SetConstantSelfCollisionResponse ( T k )    { m_Parameters.KselfCDR = k; }
    inline T    GetDistanceScaleForConstantSelfCollisionResponse () const   { return m_Parameters.KselfCDR_ScaleDistance; }
    inline void SetDistanceScaleForConstantSelfCollisionResponse ( T s )    { m_Parameters.KselfCDR_ScaleDistance = s; }
    inline ElasticRodCD<T> &        CDUnit ()       { return m_CD; }
    inline ElasticRodCD<T> const &  CDUnit () const { return m_CD; }

    inline T    GetConstantCollisionResponseWithCylinderBV () const { return m_Parameters.K_CDRwBV_Cylinder; }
    inline void SetConstantCollisionResponseWithCylinderBV ( T k )  { m_Parameters.K_CDRwBV_Cylinder = k; }
    inline T    GetConstantCollisionResponseWithSphereBV () const   { return m_Parameters.K_CDRwBV_Sphere; }
    inline void SetConstantCollisionResponseWithSphereBV ( T k )    { m_Parameters.K_CDRwBV_Sphere = k; }
    inline T    GetConstantCollisionResponseWithTriangle () const   { return m_Parameters.K_CDRwTriangle; }
    inline void SetConstantCollisionResponseWithTriangle ( T k )    { m_Parameters.K_CDRwTriangle = k; }

#ifdef  TAPs_ADD_RESTING_LEVEL_Y

    inline virtual void SetRestingLevel ( T y ) { m_Parameters.RestingLevel = y; }
#endif//TAPs_ADD_RESTING_LEVEL_Y

#ifdef  TAPs_ADD_KNOT_RECOGNITION
    // Knot Recognition -------------------------
    inline void EnableKnotRecognition ( bool b = true )     { m_Parameters.EnableKR = b; }
    inline void DisableKnotRecognition ( bool b = true )    { m_Parameters.EnableKR = !b; }
    inline void EnableLockKnots ( bool b = true )   { m_Parameters.EnableKR_LockKnots = b; }
    inline void DisableLockKnots ( bool b = true )  { m_Parameters.EnableKR_LockKnots = !b; }

    inline T    GetConstantStickPair () const   { return m_Parameters.KstickPair; }
    inline void SetConstantStickPair ( T k )    { m_Parameters.KstickPair = k; }

    unsigned int    GetNumOfKnots () const      { return m_KR.GetKnotCount(); }
    unsigned int    GetNumberOfKnots () const   { return m_KR.GetKnotCount(); }

#endif//TAPs_ADD_KNOT_RECOGNITION


    static inline int   GetNumberOfElasticRods () { return g_NumOfElasticRods; }
    static inline int   GetNumOfElasticRods () { return g_NumOfElasticRods; }

    inline int  GetID () const  { return m_ID; }

    inline ElasticRodParameters<T> const & RefToParameters () const { return m_Parameters; }

    inline virtual int  GetNumOfPoints () const     { return m_Parameters.NumOfNodes; }
    inline virtual int  GetNumberOfPoints () const  { return m_Parameters.NumOfNodes; }
    inline virtual int  GetNumOfLinks () const      { return m_Parameters.NumOfNodes-1; }
    inline virtual int  GetNumberOfLinks () const   { return m_Parameters.NumOfNodes-1; }
    inline T    GetTotalLength () const { return m_Parameters.TotalLength; }
    inline T    GetLinkLength () const  { return m_Parameters.LinkLength; }
    inline T    GetTotalMass () const   { return m_Parameters.TotalMass; }
    inline T    GetRadius () const  { return m_Parameters.Radius; }

    inline T    GetSimTimeStep () const { return m_Parameters.TimeStep; }
    inline void SetSimTimeStep ( T h )  { m_Parameters.TimeStep = h; }

    inline unsigned int GetSimIterationTimes () const   { return m_Parameters.SimIterationTimes; }
    inline void SetSimIterationTimes ( unsigned int times ) { m_Parameters.SimIterationTimes = times; }

    inline T    GetConstantPotentialStretch () const    { return m_Parameters.Ps; }
    inline void SetConstantPotentialStretch ( T k )     { m_Parameters.Ps = k; }

    inline void GetConstantPotentialBend ( T k[3] ) const   { k[0]=m_Parameters.Pb[0]; k[1]=m_Parameters.Pb[1]; k[2]=m_Parameters.Pb[2]; }
    inline void GetConstantPotentialBend ( T &k0, T &k1, T &k2 ) const  { k0=m_Parameters.Pb[0]; k1=m_Parameters.Pb[1]; k2=m_Parameters.Pb[2]; }
    inline void SetConstantPotentialBend ( T k[3] ) { m_Parameters.Pb[0]=k[0]; m_Parameters.Pb[1]=k[1]; m_Parameters.Pb[2]=k[2]; }
    inline void SetConstantPotentialBend ( T k0, T k1, T k2 )   { m_Parameters.Pb[0]=k0; m_Parameters.Pb[1]=k1; m_Parameters.Pb[2]=k2; }

    inline T    GetConstantKineticTranslational () const    { return m_Parameters.Kt; }
    inline void SetConstantKineticTranslational ( T k )     { m_Parameters.Kt = k; }

    inline void GetConstantKineticRotational ( T k[3] ) const   { k[0]=m_Parameters.Kr[0]; k[1]=m_Parameters.Kr[1]; k[2]=m_Parameters.Kr[2]; }
    inline void GetConstantKineticRotational ( T &k0, T &k1, T &k2 ) const  { k0=m_Parameters.Kr[0]; k1=m_Parameters.Kr[1]; k2=m_Parameters.Kr[2]; }
    inline void SetConstantKineticRotational ( T k[3] ) { m_Parameters.Kr[0]=k[0]; m_Parameters.Kr[1]=k[1]; m_Parameters.Kr[2]=k[2]; }
    inline void SetConstantKineticRotational ( T k0, T k1, T k2 )   { m_Parameters.Kr[0]=k0; m_Parameters.Kr[1]=k1; m_Parameters.Kr[2]=k2; }

    inline T    GetConstantDissipationTranslational () const    { return m_Parameters.Dt; }
    inline void SetConstantDissipationTranslational ( T k )     { m_Parameters.Dt = k; }

    inline void GetConstantDissipationRotational ( T k[3] ) const   { k[0]=m_Parameters.Dr[0]; k[1]=m_Parameters.Dr[1]; k[2]=m_Parameters.Dr[2]; }
    inline void GetConstantDissipationRotational ( T &k0, T &k1, T &k2 ) const  { k0=m_Parameters.Dr[0]; k1=m_Parameters.Dr[1]; k2=m_Parameters.Dr[2]; }
    inline void SetConstantDissipationRotational ( T k[3] ) { m_Parameters.Dr[0]=k[0]; m_Parameters.Dr[1]=k[1]; m_Parameters.Dr[2]=k[2]; }
    inline void SetConstantDissipationRotational ( T k0, T k1, T k2 )   { m_Parameters.Dr[0]=k0; m_Parameters.Dr[1]=k1; m_Parameters.Dr[2]=k2; }

    inline T    GetConstantConstraint_3rdDirAlignTangent () const   { return m_Parameters.Kconstraint_3rdDirAlignTangent; }
    inline void SetConstantConstraint_3rdDirAlignTangent ( T k )    { m_Parameters.Kconstraint_3rdDirAlignTangent = k; }

    inline T    GetConstantConstraint_ScaleIntrinsicStrainRate () const { return m_Parameters.Kconstraint_ScaleIntrinsicStrainRate; }
    inline void SetConstantConstraint_ScaleIntrinsicStrainRate ( T k )  { m_Parameters.Kconstraint_ScaleIntrinsicStrainRate = k; }

    inline T    GetConstantDampingVelocity () const { return 1-m_Parameters.Kvdamping; }
    inline void SetConstantDampingVelocity ( T k )  { m_Parameters.Kvdamping = 1-k; }

    inline T    GetGravitationalConstant () const   { return m_Parameters.Kgravity; }
    inline void SetGravitationalConstant ( T k )    { m_Parameters.Kgravity = -k; }

    inline T    GetStretchModulus () const;
    inline void SetStretchModulus ( T k );

    inline T    GetBendModulus () const;
    inline void SetBendModulus ( T k );

    inline T    GetShearModulus () const;
    inline void SetShearModulus ( T k );

    inline T    GetMaterialDensity () const;
    inline void SetMaterialDensity ( T material_density );

    inline void SetMaterialProperties ( T stretch_modulus, T bend_modulus, T shear_modulus, T material_density );

    inline SetOfElasticRodNodes &       GetListOfNodes ()       { return m_Nodes; }
    inline SetOfElasticRodNodes const & GetListOfNodes () const { return m_Nodes; }

    inline Vector3<T> const & RefToCurrentVertexPositionOfNodeNumber ( unsigned int id ) const  { return m_Nodes[id].Centerline[IdxCurr].GetPosition(); }
    inline Vector3<T> const & RefToPreviousVertexPositionOfNodeNumber ( unsigned int id ) const { return m_Nodes[id].Centerline[IdxPrev].GetPosition(); }
    inline Vector3<T> & RefToCurrentVertexPositionOfNodeNumber ( unsigned int id )  { return m_Nodes[id].Centerline[IdxCurr].GetPosition(); }
    inline Vector3<T> & RefToPreviousVertexPositionOfNodeNumber ( unsigned int id ) { return m_Nodes[id].Centerline[IdxPrev].GetPosition(); }

    inline Quaternion<T> const & RefToCurrentOrientationOfLinkNumber ( unsigned int id ) const  { return m_Nodes[id].Orientation[IdxCurr]; }
    inline Quaternion<T> const & RefToPreviousOrientationOfLinkNumber ( unsigned int id ) const { return m_Nodes[id].Orientation[IdxPrev]; }
    inline Quaternion<T> & RefToCurrentOrientationOfLinkNumber ( unsigned int id )  { return m_Nodes[id].Orientation[IdxCurr]; }
    inline Quaternion<T> & RefToPreviousOrientationOfLinkNumber ( unsigned int id ) { return m_Nodes[id].Orientation[IdxPrev]; }

    inline Vector3<T> const & RefToCurrentVertexForceOfNodeNumber ( unsigned int id ) const     { return m_Nodes[id].Centerline[IdxCurr].GetForce(); }
    inline Vector3<T> const & RefToPreviousVertexForceOfNodeNumber ( unsigned int id ) const    { return m_Nodes[id].Centerline[IdxPrev].GetForce(); }
    inline Vector3<T> & RefToCurrentVertexForceOfNodeNumber ( unsigned int id )     { return m_Nodes[id].Centerline[IdxCurr].GetForce(); }
    inline Vector3<T> & RefToPreviousVertexForceOfNodeNumber ( unsigned int id )    { return m_Nodes[id].Centerline[IdxPrev].GetForce(); }

    inline int  GetCurrentIndex ()  { return IdxCurr; }
    inline int  GetPreviousIndex () { return IdxPrev; }

    //-------------------------------------------------------------------------
    // Operations

    virtual void    Reset ();

    inline Vector3<T>   GetPosOfStartPt ();
    inline Vector3<T>   GetPosOfEndPt ();
    inline void SetPosOfStartPt ( Vector3<T> const & pos, Quaternion<T> const & ori = Quaternion<T>() );
    inline void SetPosOfEndPt ( Vector3<T> const & pos, Quaternion<T> const & ori = Quaternion<T>() );
    inline void MovePosOfStartPt ( Vector3<T> const & pos, Quaternion<T> const & ori = Quaternion<T>() );
    inline void MovePosOfEndPt ( Vector3<T> const & pos, Quaternion<T> const & ori = Quaternion<T>() );

    inline virtual void ClearExternalForces ();

    //
    inline virtual void ClearExternalForces_Reserved ();

    inline virtual bool CDRwith (
        MultiBoundingVolume<T> * const pMBVObj,     
        TransformationSupport<T> * const pTransform 
    );

    inline virtual bool CDRwith (
        TAPs::OpenGL::HETriMeshOneModelMultiParts<T> * pObj,    
        TransformationSupport<T> * const pTransform             
    );

    virtual void    AdvanceSimulation ();

    virtual int FindTheClosestPointToPoint ( 
        Vector3<T> const & closeToPoint,    
        T distance_limit                    
    ) const;

    inline virtual bool PickAt (
        Vector3<T> const & closeToPoint,    
        T distance_limit,                   
        int & pickedID                      
    );

    inline virtual void PickPoint (
        int pickedID            
    );

    inline virtual void MovePickedPoint (
        int pickedID,           
        Vector3<T> & position   
    );

    inline virtual void UnpickPoint (
        int pickedID            
    );

    // Data Members -----------------------------------------------------------
//=============================================================================
protected:
    // Member Functions -------------------------------------------------------

    inline virtual void ClearInternalForces ();

public:
    inline virtual void StoreInternalForces ();

    inline virtual void RestoreInternalForces ();
protected:

    inline void AdvSimLoop ();

    void    Initialize ( enum ShapeInitializationParameters::Shape shape ) throw(...);

    void    InitializeElasticRodNodes ( enum ShapeInitializationParameters::Shape shape );

    inline void SwitchDataAccessing ();

    void    ComputeAndStoreCenterlineRestLengths ();

    void    ComputeAndStoreOrientationRestLengths ();

    // Computations

    inline T    ComputeLengthOfCenterlineSegment ( 
        PointMass<T> const & c1, 
        PointMass<T> const & c2 
    );

    inline T    ComputeLengthOfOrientationSegment ( 
        PointMass<T> const & c1, 
        PointMass<T> const & c2, 
        PointMass<T> const & c3 
    );

    // Data Members -----------------------------------------------------------
    SetOfElasticRodNodes    m_Nodes;    

        
    ElasticRodParameters<T> m_Parameters;

    // Elastic rod global properties
    //int   m_iNoNodes;     //!< number of nodes
    //T m_tRadius;          //!< radius
    //T m_tTotalMass;       //!< total mass
    //T m_tTotalLength;     //!< total length
    //T m_tLinkLength;      //!< link length
    Vector3<T> m_vStartPos; 

    // Simulation Constants
    //T Kstretch_modulus;   //!< stretch Young's modulus (Es)
    //T Kbend_modulus;      //!< bend Young's modulus (Eb)
    //T Kshear_modulus;     //!< shear modulus (G)
    //T MaterialDensity;    //!< material density

    //T Ps;         //!< potential stretch constant (Ps = Es*Area, where area is \pi*r^2)
    //T Pb[3];      //!< potential bend constant (Pb[0] = Pb[1] = Eb*pi*r^2*0.25; Pb[2] = G*pi*r^2*0.5)
    //T Kt;         //!< kinetic translational constant
    //T Kr[3];      //!< kinetic rotational constant
    //T Dt;         //!< translational dissipation constant
    //T Dr[3];      //!< rotational dissipation constant
    //T Kconstraint_3rdDirAlignTangent; //!< a constant for computing constraint force.  This value should be in the same order as Ps.
    //T Kvdamping;  //!< damping constant for velocity (in the range of [0,1])
    Matrix3x3<T> Jinv;      

    // Related to rotation by quaternion
    Vector3<T>  U_strain_rates; // strain rates in the local frame


    // REMARK:
    // Ps is the stretch stiffness constant ($K_s$) that is computed from the 
    // stretch Young's modulus ($E_s$) and the radius of the elastic rod.
    // $K_s = E_s \pi r^2$

    // Kr[3] is the diagonal of the stiffness tensor (K), since an elastic rod 
    // is assumed to have a uniform cross-section, the off-diagonal terms in 
    // K can be neglected.
    // Where $K_{11} = K{22} = E_b \frac{\pi r^2}{4}$, and $K_{33} = G \frac{\pi r^2}{2}$,
    // $E_b$ is the bend Young's modulus governing the bend resistance,
    // $G$ is the shear modulus governing the torsional resistance,
    // and r is the radius of the elastic rod's cross-section.



    // r'_i = \frac{r_{i+1}-r_{i}}{\|r_{i+1}-r_{i}\|} or
    // r'_i \approx \frac{r_{i+1}-r_{i}}{l^0_i}
    // where l_i = \|r^0_{i+1}-r^0_{i}\| is the resting length of the center-
    // line element i.  Where r^0 are the initial positions of the point masses.
    // This is based on assuming a high stretch stiffness.

    // q'_j \approx \frac{q_{j+1}-q_{j}}{l_j}
    // where l_j = \frac{1}{2}(\|r^0_{j+2}-r^0_{j+1}\| + \|r^0_{j+1}-r^0_{j}\|) 
    // is the resting length of the orientation element j.

    // Therefore, the total energy of the rod does not have to be computed.
    // It is enough just to compute the energies of each element.
    // The energies for centerline and orientation element is computed by 
    // integrating the energies over the length of the respective element.
    // These energies are the potential, kinetic and dissipation energies.

        // the nodes.  Therefore, the displacements are interpolated within 
        // the elements.  Here, a constant shape function \bar{r} and \bar{q} 
        // is used where \bar{r}_i = \frac{1}{2}(r_{i}+r_{i+1}) and 
        //               \bar{q}_j = \frac{1}{2}(q_{j}+q_{j+1}).

        // the stretch energy
        // V_s[i] = \frac{1}{2} l_i K_s ( \frac{1}{l_i}\sqrt{(r_{i+1}-r_{i})\cdot(r_{i+1}-r_{i})} - 1 )^2
        // the bend energy
        // V_b[j] = \frac{l_j}{2} \sum_{k=1}^{3} K_{kk} ( B_k (q_{j}+q_{j+1}) \cdot \frac{1}{l_j}(q_{j+1}-q_{j} ) - \hat{u}_k )^2

        // the translational kinetic energy
        // T_t[i] = \frac{1}{8} \rho\pi r^2 l_i (\dot{r}_{i}+\dot{r}_{i+1})\cdot(\dot{r}_{i}+\dot{r}_{i+1})
        // the rotational kinetic energy
        // T_r[j] = \frac{l_j}{8} \sum_{k=1}^3 I_{kk} ( B_k (q_{j}+q_{j+1}) \cdot (\dot{q}_{j}+\dot{q}_{j+1}) )^2

        // the translational dissipation energy
        // D_t[i] = \frac{l_i}{2} \gamma_t v_i^(rel) \cdot v_i^(rel)
        // where v_i^(rel) = \frac{1}{\|r'_i\|^2} r'_i ( \dot{r}'_i \cdot r'_i )
        // assume that \|r'_i\| \approx 1, which is valid for a large stretch 
        // stiffness, then 
        // v_i^(rel) = \frac{1}{{l_i}^3} (r_{i+1}-r{i}) ( (\dot{r}_{i+1}-\dot{r}_{i}) \cdot (r_{i+1}-r_{i}) )
        // the rotational dissipation energy
        // D_r[j] = \frac{2}{l_j} \gamma_r \sum_{k=1}^3 ( B^0_k q_{j+1} \dot{q}_{j+1} - B^0_k q_{j} \dot{q}_{j} )^2
        // where \|q_j\| \approx 1 is assumed for all j.

        // A penalty constraint to force d_3 to align with r_i can be treated 
        // as an addtional potential energy term in the Lagrangian equation 
        // of motion of the rod.
        // E_p[i] = \frac{l_i}{2} \kappa ( \frac{r_{i+1}-r_{i}}{\|r_{i+1}-r_{i}\|} - d_3(q_i) ) \cdot ( \frac{r_{i+1}-r_{i}}{\|r_{i+1}-r_{i}\|} - d_3(q_i) )
        // To enforce the quaternion unit length constraint, 
        // a coordinate projection is used
        // q_i \leftarrow \frac{\hat{q}_i}{\|\hat{q}_i\|}

        // The evolution of the point masses
        // v^{t+h}_i = v^t_i + \frac{h}{m_i} F^t
        // r^{t+h}_i = r^t_i + (h v^{t+h}_i)
        // The evolution of the orientations
        // \omega^{t+h}_j = I^{-1} ( \tilde{\tau}^t_j - \omega^t_j \times (I\omega^t_j) )h + \omega^t_j
        // where \tilde{\tau}_j is the vector part of \frac{1}{2} Q^T_j \tau_j
        // \hat{q}^{t+h}_j = \frac{1}{2} Q^t_j ( 0, \omega^{t+h}_j )^T h + q^t_j
        // q^{t+h}_j = \frac{\hat{q}^{t+h}_j}{\|\hat{q}^{t+h}_j\|}

    // Member Functions -------------------------------------------------------

    //---------------------------------------------------------------
    // START: Related to Centerline

    inline void ComputeStretchForceOfCenterline ( typename SetOfElasticRodNodes::iterator & iterNode );

#ifdef  USE_CORDE_SIM

    inline void ComputeTranslationalDissipationForceOfCenterline ( typename SetOfElasticRodNodes::iterator & iterNode );
    inline void ComputeKineticForceOfCenterline ( typename SetOfElasticRodNodes::iterator & iterNode );
#endif//USE_CORDE_SIM

    inline void ComputeDampingVelocityOfCenterline ( typename SetOfElasticRodNodes::iterator & iterNode );

    inline void ComputeGravitationalForceOfCenterline ( typename SetOfElasticRodNodes::iterator & iterNode );
    //inline void   ComputeGravitationalVelocityOfCenterline ( T timestep, typename SetOfElasticRodNodes::iterator & iterNode );

    // END: Related to Centerline
    //---------------------------------------------------------------
    
    //---------------------------------------------------------------
    // START: Related to Quaternion

    inline void ComputeBendTorqueOfQuaternion ( typename SetOfElasticRodNodes::iterator & iterNode );

#ifdef  USE_CORDE_SIM

    inline void ComputeRotationalDissipationTorqueOfQuaternion ( typename SetOfElasticRodNodes::iterator & iterNode );
    inline void ComputeKineticTorqueOfQuaternion ( typename SetOfElasticRodNodes::iterator & iterNode );
#endif//USE_CORDE_SIM

    void ComputeIntrinsicStrainRate ( typename SetOfElasticRodNodes::iterator & iterNode );

    //inline Quaternion<T>  DiffOrientation ( typename SetOfElasticRodNodes::iterator & iterNode );
    
    // END: Related to Quaternion
    //---------------------------------------------------------------

    inline void ComputeConstraintForceOfCenterlineAndOrientation ( typename SetOfElasticRodNodes::iterator & iterNode );

    
//=============================================================================
private:
    // Member Functions -------------------------------------------------------

    // Compute the factorization of the rotation matrix using constant 
    // skew-symmetric matrices (see appendix in the CoRdE papers)

    inline Quaternion<T>    B_1 ( Quaternion<T> const & q ) const;
    inline Quaternion<T>    B_2 ( Quaternion<T> const & q ) const;
    inline Quaternion<T>    B_3 ( Quaternion<T> const & q ) const;
    inline Quaternion<T>    B0_1 ( Quaternion<T> const & q ) const;
    inline Quaternion<T>    B0_2 ( Quaternion<T> const & q ) const;
    inline Quaternion<T>    B0_3 ( Quaternion<T> const & q ) const;


    inline Vector3<T>   Get1stDirector ( Quaternion<T> const & q ) const;
    inline Vector3<T>   Get2ndDirector ( Quaternion<T> const & q ) const;
    inline Vector3<T>   Get3rdDirector ( Quaternion<T> const & q ) const;

    inline Quaternion<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
    );
    inline Quaternion<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
    );
    inline Quaternion<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
    );

    inline Quaternion<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
    );
    inline Quaternion<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
    );
    inline Quaternion<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
    );

    inline Quaternion<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
    );
    inline Quaternion<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
    );
    inline Quaternion<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
    );

    // Data Members -----------------------------------------------------------

    int IdxCurr;//      = 0;        //!< an index to current data
    int IdxPrev;//      = 1;        //!< an index to previous data
    int &IdxNext;//     = IdxPrev;  //!< an alias for the index to previous data

    //static bool   g_IsGLSLInit;       //!< true if the GLSL is already initialized
    static int  g_NumOfElasticRods; 
    int         m_ID;               
//=============================================================================


// START: FOR COLLISION DETECTION
//=============================================================================
public:
    // MEMBER FUNCTIONS -------------------------------------------------------
    BVHTree<T> *        GetBVHTree ()       { return m_CD.GetBVHTree(); }
    BVHTree<T> const *  GetBVHTree () const { return m_CD.GetBVHTree(); }

    virtual std::vector< BVHNode<T> * > & GetListOfCollidedNodes()      { return m_svCollidedNode; }
    virtual std::vector< BVHNode<T> * > & GetListOfCollidedNodesThat()  { return m_svCollidedNodeThat; }

protected:
    // MEMBER FUNCTIONS -------------------------------------------------------

    void UpdateBVHTree ()   { m_CD.UpdateBVHTree(); }

    void UpdateBVHTree_PBD ()   { m_CD.UpdateBVHTree_PBD(); }

    void UpdateVelocities_PBD ( T timestep )
    {
        SetOfElasticRodNodes::iterator  currNode  = m_Nodes.begin();
        while ( currNode != m_Nodes.end() ) {
            currNode->Centerline[IdxNext].SetVelocity( (currNode->Centerline[IdxNext].GetPosition() - currNode->Centerline[IdxCurr].GetPosition() ) / timestep );
            ++currNode;
        }
    }

    void CDRforSelfIntersections ()
    {
        if ( m_Parameters.EnableSelfCDR ) {

            //#ifdef    TAPs_ADVANCED_SELF_COLLISION_DETECTION
            //  #ifdef  TAPs_ADD_KNOT_RECOGNITION
            //      m_KR.GetCurrentTrackingKnot_StartAndEndLinkIDs ( m_CD.StartLinkIDofCurrentTrackedKnot, m_CD.EndLinkIDofCurrentTrackedKnot );
            //  #endif//TAPs_ADD_KNOT_RECOGNITION
            //#endif//TAPs_ADVANCED_SELF_COLLISION_DETECTION

            m_CD.CDRforSelfIntersections();
        }
    }

    void CDRforSelfIntersections_PBD () { if ( m_Parameters.EnableSelfCDR ) m_CD.CDRforSelfIntersections_PBD(); }

    // DATA MEMBERS -----------------------------------------------------------
    std::vector< BVHNode<T> * > m_svCollidedNode;       
    std::vector< BVHNode<T> * > m_svCollidedNodeThat;   
    ElasticRodCD<T> m_CD;           
//=============================================================================
// END: FOR COLLISION DETECTION


// START: FOR KNOT RECOGNITION
//=============================================================================
#ifdef  TAPs_ADD_KNOT_RECOGNITION

public:
    // MEMBER FUNCTIONS -------------------------------------------------------
    std::string CheckSurgeonsKnot ()
    {
        if ( m_Parameters.EnableKR ) {
        #ifdef  TAPs_ADD_ANIMATED_KNOTS
            if ( m_KR.GetStatusKnotAnimation() ) {
                return "Knot is being animated!";
            }
            else {
                return m_KR.CheckSurgeonsKnot();
            }
        #else //TAPs_ADD_ANIMATED_KNOTS
            return m_KR.CheckSurgeonsKnot();
        #endif//TAPs_ADD_ANIMATED_KNOTS
        }
        else {
            return "";
        }

    }

    inline ElasticRodKR<T> &        KRUnit ()       { return m_KR; }
    inline ElasticRodKR<T> const &  KRUnit () const { return m_KR; }

protected:
    // MEMBER FUNCTIONS -------------------------------------------------------

    // DATA MEMBERS -----------------------------------------------------------
    ElasticRodKR<T> m_KR;   

#endif//TAPs_ADD_KNOT_RECOGNITION
//=============================================================================
// END: FOR KNOT RECOGNITION

    
// START: FOR COMPUTATION BY CUDA
//=============================================================================
#ifdef  TAPs_USE_CUDA

public:
    // MEMBER FUNCTIONS -------------------------------------------------------

    // Is the simulation computed by CUDA?
    bool IsCompByCUDA () const          {   return m_Parameters.EnableCUDA; }
    // Enable/disable the simulation computed by CUDA
    void EnableCompByCUDA ( bool b )    {   m_Parameters.EnableCUDA = b; }

    #if defined(__gl_h_) || defined(__GL_H__)
    // Is the rendering computed by CUDA with OpenGL?
    bool IsRenderByCUDA_GL () const         {   return m_Parameters.EnableCUDA_GenDrawData; }
    // Enable/disable the rendering computed by CUDA with OpenGL
    void EnableRenderByCUDA_GL ( bool b )   {   m_Parameters.EnableCUDA_GenDrawData = b; }
    #endif//#if defined(__gl_h_) || defined(__GL_H__)

protected:
    // MEMBER FUNCTIONS -------------------------------------------------------

    // DATA MEMBERS -----------------------------------------------------------
    ElasticRod_CompByCUDA<T>    m_CUDA;     

#endif//TAPs_USE_CUDA
//=============================================================================
// END: FOR COMPUTATION BY CUDA

    
//OpenGL
//-----------------------------------------------------------------------------
//#define __GL_H__
//#define   TAPs_USE_GLSL
//-----------------------------------------------------------------------------
#if defined(__gl_h_) || defined(__GL_H__)
public:


    #ifndef TAPs_USE_GLSL
        //#define ELASTIC_MODEL_SIMPLEST_DRAW
        #define ELASTIC_MODEL_GENERALIZED_CYLINDERS_DRAW
    #endif//TAPs_USE_GLSL

    unsigned int GetRenderingForNumberOfVerticesPerCrossSection () const    { return m_drawNV; }
    void SetRenderingForNumberOfVerticesPerCrossSection ( unsigned int i );
//  static void SFn_SetRenderingForNumberOfVerticesPerCrossSection ( void * pObj, unsigned int i )
//  {
//      ModelElasticRod<T> * pER = dynamic_cast<ModelElasticRod *>( pObj );
//      if ( pER ) {
//          pER->SetRenderingForNumberOfVerticesPerCrossSection( i );
//      }
//  }

    virtual void Draw ();
    virtual void DrawForDebugging ()
    {
        #ifdef  TAPs_ADD_KNOT_RECOGNITION
        //m_KR.DrawForDebug();
        m_KR.Draw();
        #endif//TAPs_ADD_KNOT_RECOGNITION

        m_CD.DrawExtraSelfCDData();

        Draw();

        glPushAttrib( GL_ALL_ATTRIB_BITS );
        glDisable( GL_LIGHTING );
        glDisable( GL_DEPTH_TEST );
        glPointSize( 5 );
        glColor3f( 0, 1, 0 );
        glBegin( GL_POINTS );
        for ( int i = 0; i < GetNumOfPoints(); ++i ) {
            glVertex3fv( RefToCurrentVertexPositionOfNodeNumber(i).GetDataFloat() );
        }
        glEnd();
        glColor3f( 1, 1, 0 );
        glBegin( GL_LINE_STRIP );
        for ( int i = 0; i < GetNumOfPoints(); ++i ) {
            glVertex3fv( RefToCurrentVertexPositionOfNodeNumber(i).GetDataFloat() );
        }
        glEnd();
        glPopAttrib();
    }

    // FOR DEBUGGING
    // Draw orientations
    void DrawOrientations () const;

    OpenGL::OpenGLObj   Rendering;  

private:
    Vector3<T> Matrix4x4MultVector3 ( Matrix4x4<T> const &M, Vector3<T> const &V ) const;

    void ComputeVerticesAndNormalsOfCircleCrossSection ();

    void GenAndDrawCylinderVertexData ();

    #ifdef  TAPs_ER_DRAW_WITH_SUBDIVISION
    void GenAndDrawCylinderVertexData_SUBDIVISION ();
    void GenAndDrawCylinderVertexData_SUBDIVISION_Gen (
        unsigned int level,             
        Vector3<T> posKept[],           
        Matrix4x4<T> rotMatKept[],      
        Vector3<T> posPts[],            
        Matrix4x4<T> rotMatPts[]        
    );
    void GenAndDrawCylinderVertexData_SUBDIVISION_Draw (
        Vector3<T> const & P,           
        Matrix4x4<T> const & RotMatP,   
        Vector3<T> const & Q,           
        Matrix4x4<T> const & RotMatQ,   
        T   texXstart = 0.0,            
        T   texXend   = 1.0             
    );
    inline void 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  
    );
    #endif//TAPs_ER_DRAW_WITH_SUBDIVISION

    void InitGL ();

    void ClearGL ();

    #ifdef  TAPs_USE_GLSL

        bool InitGLSL ();
    #endif//TAPs_USE_GLSL

    //void ComputeVertexData ();
        
    //static const unsigned int m_drawNV = 8;   //!< number of vertices per cross section
    //Vector3<T>    m_drawCrossSectionVertices[m_drawNV];   //!< cross section's vertices
    //Vector3<T>    m_drawCrossSectionNormals[m_drawNV];    //!< cross section's normals
    //T         m_drawCrossSectionTexCoords[m_drawNV];  //!< cross section's texture coordinates
    unsigned int    m_drawNV;                       
    Vector3<T> *    m_drawCrossSectionVertices;     
    Vector3<T> *    m_drawCrossSectionNormals;      
    T *             m_drawCrossSectionTexCoords;    
    Vector3<T> **   vertices;       
    Vector3<T> **   normals;        
    Vector3<T> **   texCoords;      
    Vector3<T> *    sVertices;      

    GLuint      m_GLVertexArray;    
    GLfloat *   m_pVertexData;      
    GLuint      m_numOfVertices;    
    GLuint      m_NumOfBytesOfVertexData;   

#endif//defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------


//wxWidgets
//-----------------------------------------------------------------------------
#ifdef  TAPs_USE_WXWIDGETS
public:
    void ShowPropertyDialog ( wxWindow * parent = NULL );
private:
    WX::WXElasticRodParameters  Dialogs;    
#endif//TAPs_USE_WXWIDGETS
//-----------------------------------------------------------------------------
//=============================================================================
}; // CLASS END: ModelElasticRod
//=============================================================================
END_NAMESPACE_TAPs
//-----------------------------------------------------------------------------
#include "TAPsModelElasticRod.cpp"
//-----------------------------------------------------------------------------
#endif
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----