TAPsAdvSimSupport_DS.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsAdvSimSupport_DS.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (09/29/2008)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsAdvSimSupport_DS.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, typename DATA>
AdvSimSupport_DS<T,DATA>::AdvSimSupport_DS ()
{}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
AdvSimSupport_DS<T,DATA>::~AdvSimSupport_DS ()
{
    ClearAllConstraints();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
std::string AdvSimSupport_DS<T,DATA>::StrInfo () const
{
    std::stringstream ss;
    ss << "AdvSimSupport_DS<T,DATA>\n";
    return ss.str();
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
//template <typename T, typename DATA>
//int AdvSimSupport_DS<T,DATA>::AddModel ( OpenGL::ModelSuture<T> * pModel )
//{
//  m_ListOfSutureModelsBasedOnMSS.push_back( pModel );
//}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
int AdvSimSupport_DS<T,DATA>::AddModel ( ModelElasticRod<T> * pModel )
{
    m_ListOfSutureModelsBasedOnER.push_back( pModel );
    return AddERBasedSuture();
}
//-----------------------------------------------------------------------------
//template <typename T, typename DATA>
//int AdvSimSupport_DS<T,DATA>::AddModel ( ModelForSurgery<T> * pModel )
//{
//  m_ListOfModelsForSurgeryBasedOnMSS.push_back( pModel );
//
//  // Add each part of the surgery model to the list of submodels
//  for ( int i = 0; i < pModel->GetNumberOfParts(); ++i ) {
//      m_ListOfHETriMeshOneModelMultiParts.push_back( pModel->GetPtrToPartNo(i) );
//  }
//}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
int AdvSimSupport_DS<T,DATA>::AddModel ( ModelDefBasedOnFEM<T,DATA> * pModel )
{
    m_ModelType.push_back( MODEL_BASED_ON_FEM );
    m_ModelLocation.push_back( m_ListOfModelsForSurgeryBasedOnFEM.size() );
    m_ListOfModelsForSurgeryBasedOnFEM.push_back( pModel );

    return AddModel();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
int AdvSimSupport_DS<T,DATA>::AddModel ()
{
    // Add data storage for the added model
    m_ListOfInteractionModelVsModel.push_back( std::vector< std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * > >() );
    std::vector< std::vector< std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * > > >::iterator it = m_ListOfInteractionModelVsModel.begin();
    while ( it != m_ListOfInteractionModelVsModel.end() ) {
        // Add data storage for the existed models with the added model
        it->push_back( std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * >() );
        ++it;
    }

    // DEBUG
    //std::cout << "size of m_ListOfInteractionModelVsModel: " << m_ListOfInteractionModelVsModel.size() << "\n";
    //for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel.size(); ++i ) {
    //  std::cout << "size of m_ListOfInteractionModelVsModel["<<i<<"]: " << m_ListOfInteractionModelVsModel[i].size() << "\n";
    //}

    // Add data storage for interaction with ER-based sutures
    std::vector< std::vector< std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * > > >::iterator itS = m_ListOfInteractionERSutureVsModel.begin();
    while ( itS != m_ListOfInteractionERSutureVsModel.end() ) {
        itS->push_back( std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >() );
        ++itS;
    }

    // returns the assigned ID for the added model
    return m_ListOfInteractionModelVsModel.size()-1;
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
int AdvSimSupport_DS<T,DATA>::AddERBasedSuture ()
{
    // Add data storage for the added suture model
    m_ListOfInteractionERSutureVsModel.push_back( std::vector< std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * > >() );
    std::vector< std::vector< std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * > > >::iterator it = m_ListOfInteractionModelVsModel.begin();
    while ( it != m_ListOfInteractionModelVsModel.end() ) {
        // Add data storage for interaction of the existed (non-suture) models with the added suture model
        m_ListOfInteractionERSutureVsModel.back().push_back( std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >() );
        ++it;
    }

    // returns the assigned ID for the added ER-based suture
    return m_ListOfInteractionERSutureVsModel.size()-1;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AddVertexConnectionModelAndModel (
        unsigned int modelA,                        
        HEVertex<T> * pVertexA,                     
        enum Enum::AddOn::SimConstraints flagsA,    
        unsigned int modelB,                        
        HEVertex<T> * pVertexB,                     
        enum Enum::AddOn::SimConstraints flagsB,    
        T forceRatio,           
        T forceScaleForModelA,  
        T forceScaleForModelB,  
        T forceThresholdForModelA,  
        T forceThresholdForModelB   
)
{
    pVertexA->SimFlags.SetSimulationConstraints( flagsA );
    pVertexB->SimFlags.SetSimulationConstraints( flagsB );
    m_ListOfInteractionModelVsModel[modelA][modelB].push_back( 
        new AdvSimConstraint_HEVertexVsHEVertex<T>( pVertexA, pVertexB, forceRatio, forceScaleForModelA, forceScaleForModelB, forceThresholdForModelA, forceThresholdForModelB ) );
    m_ListOfInteractionModelVsModel[modelA][modelB].back()->RefToSavedPositionA() = pVertexA->GetPosition();
    m_ListOfInteractionModelVsModel[modelA][modelB].back()->RefToSavedPositionB() = pVertexB->GetPosition();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AddVertexConnectionERSutureAndModel (
        unsigned int sutureID,                          
        unsigned int vertexIDSuture,                    
        //DS::SimulationFlags * simFlagsSuture,         //!< simulation flags of suture
        //Vector3<T> * vertexSuture,                        //!< vertex of suture
        enum Enum::AddOn::SimConstraints flagsSuture,   
        unsigned int modelID,                           
        HEVertex<T> * pVertexModel,                     
        enum Enum::AddOn::SimConstraints flagsModel,    
        T forceRatio,           
        T forceScaleForSuture,  
        T forceScaleForModel,   
        T forceThresholdForSuture,  
        T forceThresholdForModel    
)
{
    std::cout << "suture vertex id: " << vertexIDSuture << "\n";
    std::cout << "SIM FLAG" << m_ListOfSutureModelsBasedOnER[sutureID]->GetListOfNodes()[vertexIDSuture].SimFlags << "\n";
    
    m_ListOfSutureModelsBasedOnER[sutureID]->GetListOfNodes()[vertexIDSuture].SimFlags.SetSimulationConstraints( flagsSuture );
    m_ListOfSutureModelsBasedOnER[sutureID]->GetListOfNodes()[vertexIDSuture].SimFlags.SetSimulationConstraints( TAPs::Enum::AddOn::ATTACHED );

    std::cout << "SIM FLAG" << m_ListOfSutureModelsBasedOnER[sutureID]->GetListOfNodes()[vertexIDSuture].SimFlags << "\n";
    
    pVertexModel->SimFlags.SetSimulationConstraints( flagsModel );
    m_ListOfInteractionERSutureVsModel[sutureID][modelID].push_back( 
        new AdvSimConstraint_ERSutureVsHEVertex<T>( vertexIDSuture, pVertexModel, forceRatio, forceScaleForSuture, forceScaleForModel, forceThresholdForSuture, forceThresholdForModel ) );
    //m_ListOfInteractionERSutureVsModel[sutureID][modelID].back()->RefToSavedPositionA() = m_ListOfSutureModelsBasedOnER[sutureID]->GetListOfNodes()[vertexIDSuture].GetPosition();
    m_ListOfInteractionERSutureVsModel[sutureID][modelID].back()->RefToSavedPositionB() = pVertexModel->GetPosition();
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConstraints ()
{
    // Clear all connection (aka interaction) loads from FEM-based models
    for ( unsigned int i = 0; i < m_ListOfModelsForSurgeryBasedOnFEM.size(); ++i ) {
        m_ListOfModelsForSurgeryBasedOnFEM[i]->ClearAllConnectionLoads();
    }

    // Clear all point forces
    ListOfPointForces::iterator it = m_ListOfPointForces.begin();
    while ( it != m_ListOfPointForces.end() ) {
        delete *it;
        ++it;
    }
    m_ListOfPointForces.clear();

    // model-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel.size(); ++i ) {
        for ( unsigned int j = i; j < m_ListOfInteractionModelVsModel.size(); ++j ) {
            EnforceAllConstraintsOfTwoModels( i, j );
        }
    }

    // ERSuture-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionERSutureVsModel.size(); ++i ) {
        for ( unsigned int j = 0; j < m_ListOfInteractionERSutureVsModel[i].size(); ++j ) {
            EnforceAllConstraintsOfERSutureAndModel( i, j );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConstraintsOfTwoModels ( int modelA, int modelB )
{
    Matrix4x4<T> TrxModelA;
    Matrix4x4<T> InvTrxModelA;
    if ( m_ModelType[modelA] == MODEL_BASED_ON_FEM ) {
        unsigned int loc = m_ModelLocation[modelA];
        TrxModelA = m_ListOfModelsForSurgeryBasedOnFEM[modelA]->RefToTransformationSupport().ReturnMatrixTransform();
        InvTrxModelA = TrxModelA.GetInverse();
    }
    Matrix4x4<T> TrxModelB;
    Matrix4x4<T> InvTrxModelB;
    if ( m_ModelType[modelB] == MODEL_BASED_ON_FEM ) {
        unsigned int loc = m_ModelLocation[modelB];
        TrxModelB = m_ListOfModelsForSurgeryBasedOnFEM[modelB]->RefToTransformationSupport().ReturnMatrixTransform();
        InvTrxModelB = TrxModelB.GetInverse();
    }

    modelB -= modelA;   // since the data in the std::vector starts from 0, not from the model number
    for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel[modelA][modelB].size(); ++i ) {
        EnforceTheConstraintNumberOfTwoModels( modelA, TrxModelA, InvTrxModelA, modelB, TrxModelB, InvTrxModelB, i );
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConstraintsOfERSutureAndModel ( int suture, int model )
{
    Matrix4x4<T> TrxModel;
    Matrix4x4<T> InvTrxModel;
    TrxModel = m_ListOfModelsForSurgeryBasedOnFEM[model]->RefToTransformationSupport().ReturnMatrixTransform();
    InvTrxModel = TrxModel.GetInverse();
    for ( unsigned int i = 0; i < m_ListOfInteractionERSutureVsModel[suture][model].size(); ++i ) {
        EnforceTheConstraintNumberOfERSutureAndModel( suture, model, TrxModel, InvTrxModel, i );
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceTheConstraintNumberOfTwoModels (
    int modelA,                         
    Matrix4x4<T> const & TrxModelA,     
    Matrix4x4<T> const & InvTrxModelA,  
    int modelB,                         
    Matrix4x4<T> const & TrxModelB,     
    Matrix4x4<T> const & InvTrxModelB,  
    unsigned int constraintNumber       
)
{
    AdvSimConstraint_HEVertexVsHEVertex<T> * pConstraint = m_ListOfInteractionModelVsModel[modelA][modelB][constraintNumber];

    HEVertex<T> * heA = pConstraint->GetPtrToVertexFromModel_1();
    HEVertex<T> * heB = pConstraint->GetPtrToVertexFromModel_2();
    Vector3<T> V1 = heA->GetProtectedPosition() = pConstraint->RefToSavedPositionA();
    Vector3<T> V2 = heB->GetProtectedPosition() = pConstraint->RefToSavedPositionB();
    V1 = TrxModelA * V1;
    V2 = TrxModelB * V2;
    T ratio = pConstraint->GetForceRatio();
    pConstraint->RefToTargetPositionA() = pConstraint->RefToTargetPositionB() = ratio*V1 + (1-ratio)*V2;
    V1 = InvTrxModelA * pConstraint->RefToTargetPositionA();
    V2 = InvTrxModelB * pConstraint->RefToTargetPositionB();
    pConstraint->RefToTargetPositionA() = V1;
    pConstraint->RefToTargetPositionB() = V2;

    // If modelA is FEM-based, then add the connection as a point force.
    if ( m_ModelType[modelA] == MODEL_BASED_ON_FEM ) {
        Vector3<T> Fa = V1 - heA->GetProtectedPosition();
        PointForce<T> * pF = new PointForce<T>();
        m_ListOfPointForces.push_back( pF );

        // Set connection (i.e. point) force
        pF->SetID( heA->GetID() );
        pF->RefToPosition() = heA->GetBarycentricPtr()->RetGenBaryCoords();
        pF->RefToForce() = Fa * pConstraint->GetForceScaleA();

        unsigned int loc = m_ModelLocation[modelA];
        m_ListOfModelsForSurgeryBasedOnFEM[loc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() );
    }
    // If modelB is FEM-based, then add the connection as a point force.
    if ( m_ModelType[modelB] == MODEL_BASED_ON_FEM ) {
        Vector3<T> Fb = V2 - heB->GetProtectedPosition();
        PointForce<T> * pF = new PointForce<T>();
        m_ListOfPointForces.push_back( pF );

        // Set connection (i.e. point) force
        pF->SetID( heB->GetID() );
        pF->RefToPosition() = heB->GetBarycentricPtr()->RetGenBaryCoords();
        pF->RefToForce() = Fb * pConstraint->GetForceScaleB();

        unsigned int loc = m_ModelLocation[modelB];
        m_ListOfModelsForSurgeryBasedOnFEM[loc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() );
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceTheConstraintNumberOfERSutureAndModel (
    int suture,                         
    int model,                          
    Matrix4x4<T> const & TrxModel,      
    Matrix4x4<T> const & InvTrxModel,   
    unsigned int constraintNumber       
)
{
    AdvSimConstraint_ERSutureVsHEVertex<T> * pConstraint = m_ListOfInteractionERSutureVsModel[suture][model][constraintNumber];
    int SuturePt = pConstraint->GetVertexIDFromSuture();

    if ( m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].UseEnforcedPosition ) {
        // Fix the suture point and make the model's point follow the suture's point
        Vector3<T> & S = m_ListOfSutureModelsBasedOnER[suture]->RefToCurrentVertexPositionOfNodeNumber( SuturePt );
        HEVertex<T> * he = pConstraint->GetPtrToVertexFromModel();
        Vector3<T> V = he->GetProtectedPosition() = pConstraint->RefToSavedPositionB();
        V = TrxModel * V;
        pConstraint->RefToTargetPositionA() = pConstraint->RefToTargetPositionB() = S;
        V = InvTrxModel * pConstraint->RefToTargetPositionB();
        pConstraint->RefToTargetPositionB() = V;

        // If model is FEM-based, then add the connection as a point force.
        if ( m_ModelType[model] == MODEL_BASED_ON_FEM ) {
            Vector3<T> F = V - he->GetProtectedPosition();
            PointForce<T> * pF = new PointForce<T>();
            m_ListOfPointForces.push_back( pF );

            // Set connection (i.e. point) force
            pF->SetID( he->GetID() );
            pF->RefToPosition() = he->GetBarycentricPtr()->RetGenBaryCoords();
            pF->RefToForce() = F * pConstraint->GetForceScaleB();
            unsigned int loc = m_ModelLocation[model];
            m_ListOfModelsForSurgeryBasedOnFEM[loc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() );
        }
    }
    else {
        Vector3<T> & S = m_ListOfSutureModelsBasedOnER[suture]->RefToCurrentVertexPositionOfNodeNumber( SuturePt );
        HEVertex<T> * he = pConstraint->GetPtrToVertexFromModel();
        Vector3<T> V = he->GetProtectedPosition() = pConstraint->RefToSavedPositionB();
        V = TrxModel * V;
        T ratio = pConstraint->GetForceRatio();
        pConstraint->RefToTargetPositionA() = 
        pConstraint->RefToTargetPositionB() = ratio*S + (1-ratio)*V;
        //S = pConstraint->RefToTargetPositionA();  // suture current position
        V = InvTrxModel * pConstraint->RefToTargetPositionB();
        pConstraint->RefToTargetPositionB() = V;

        // If model is FEM-based, then add the connection as a point force.
        if ( m_ModelType[model] == MODEL_BASED_ON_FEM ) {
            Vector3<T> F = V - he->GetProtectedPosition();
            PointForce<T> * pF = new PointForce<T>();
            m_ListOfPointForces.push_back( pF );

            // Set connection (i.e. point) force
            pF->SetID( he->GetID() );
            pF->RefToPosition() = he->GetBarycentricPtr()->RetGenBaryCoords();
            pF->RefToForce() = F * pConstraint->GetForceScaleB();
            unsigned int loc = m_ModelLocation[model];
            m_ListOfModelsForSurgeryBasedOnFEM[loc]->AddConnectionForceInLocalCoordinates( m_ListOfPointForces.back() );

            std::cout << "F mag: " << pF->RefToForce().Magnitude() << "\n";
            std::cout << "Force Ratio: " << pConstraint->GetForceRatio() << "\n";
            std::cout << "Force Scale A: " << pConstraint->GetForceScaleA() << "\n";
            std::cout << "Force Threshold A: " << pConstraint->GetForceThresholdA() << "\n";
            std::cout << "Force Scale B: " << pConstraint->GetForceScaleB() << "\n";
            std::cout << "Force Threshold B: " << pConstraint->GetForceThresholdB() << "\n";
            std::cout << "SuturePt: " << SuturePt << "\n";
            std::cout << m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags << "\n";
            std::cout << "WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ???\n";

            //if ( SuturePt < m_ListOfSutureModelsBasedOnER[suture]->GetNumberOfLinks()-1 ) {
            //  //if ( m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )
            //  // WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ???
            //  {
            //      if ( pF->RefToForce().Magnitude() > pConstraint->GetForceThresholdA() )
            //      {
            //          //if (
            //          // Backward ???

            //          // Forward
            //          ++SuturePt;

            //          std::cout << "SuturePt after: " << SuturePt << "\n";

            //          pConstraint->SetVertexIDFromSuture( SuturePt );
            //          m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
            //      }
            //  }
            //}
        }

        // Calculate the external force apply to Suture
        Vector3<T> Fs = pConstraint->RefToTargetPositionA() - S;

        bool bLinearForce = true;
        T scaleNonlinearForce = 0.5;

        if ( bLinearForce )
        {
            // LINEAR FORCE
            Fs *= pConstraint->GetForceScaleA();
        }
        else
        {
            // NONLINEAR FORCE
            T suture_link_length = m_ListOfSutureModelsBasedOnER[suture]->GetLinkLength();
            T magnitude = Fs.Magnitude();
            T scale = pow( pConstraint->GetForceScaleA(), magnitude/suture_link_length );
            if ( scale*magnitude <= m_ListOfSutureModelsBasedOnER[suture]->GetStretchModulus()*scaleNonlinearForce ) {
                Fs *= scale;
            }
            else {
                Fs *= m_ListOfSutureModelsBasedOnER[suture]->GetStretchModulus();
            }
        }

        T SforceMag = Fs.Magnitude();
        //Fs /= m_ListOfSutureModelsBasedOnER[suture]->GetSimTimeStep();

        //std::cout << "GetForceScaleA: " << pConstraint->GetForceScaleA() << "\n";
        std::cout << "Fs: " << Fs << "\n";
        m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].ExternalForce_ForAdvSimCtrl = Fs;

        // For slidable suture points
        //if ( !m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].UseEnforcedPosition ... && ...
        if ( SuturePt < m_ListOfSutureModelsBasedOnER[suture]->GetNumberOfLinks()-5 ) {
            if ( m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )
            // WHY THE SIMFLAGS IS NOT SET PROPERLY (GOT RESET SOMEWHERE) ???
            {
                // If the next suture point is not attached, then check if the suture point will slide due to the high force.
                //if ( m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt+1].SimFlags.CheckSimulationConstraints( Enum::AddOn::ATTACHED ) ) 
                {
                    if ( SforceMag > pConstraint->GetForceThresholdA() )
                    {
                        //if (
                        // Backward ???

                        //-----------------------------------------------
                        // Forward
                        //-----------------------------------------------
                        // Clear simulation states of the current node, e.g. clear ATTACHED status
                        m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].ExternalForce_ForAdvSimCtrl.Clear();
                        m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.ClearSimulationConstraints( Enum::AddOn::ATTACHED );

                        // move to next suture point
                        ++SuturePt;

                        std::cout << "SuturePt after: " << SuturePt << "\n";

                        pConstraint->SetVertexIDFromSuture( SuturePt );

                        // Set the status of the new suture point

                        //*
                        // DEBUG
                        // Enforce Suture's Position
                        if ( SuturePt == 25 )
                        {
                            m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].UseEnforcedPosition = true;
                        }
                        //*/

                        // Calculate the external force apply to Suture
                        Vector3<T> Fs = pConstraint->RefToTargetPositionA() - m_ListOfSutureModelsBasedOnER[suture]->RefToCurrentVertexPositionOfNodeNumber( SuturePt );
                        if ( bLinearForce )
                        {
                            // LINEAR FORCE
                            Fs *= pConstraint->GetForceScaleA();
                        }
                        else
                        {
                            // NONLINEAR FORCE
                            T suture_link_length = m_ListOfSutureModelsBasedOnER[suture]->GetLinkLength();
                            T magnitude = Fs.Magnitude();
                            T scale = pow( pConstraint->GetForceScaleA(), magnitude/suture_link_length );
                            if ( scale*magnitude <= m_ListOfSutureModelsBasedOnER[suture]->GetStretchModulus()*scaleNonlinearForce ) {
                                Fs *= scale;
                            }
                            else {
                                Fs *= m_ListOfSutureModelsBasedOnER[suture]->GetStretchModulus();
                            }
                        }
                        m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].ExternalForce_ForAdvSimCtrl = Fs;
                        if ( SuturePt < m_ListOfSutureModelsBasedOnER[suture]->GetNumberOfLinks()-5 ) {
                            m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
                            m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED );
                        }
                        else {
                            m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[SuturePt].SimFlags.SetSimulationConstraints( Enum::AddOn::ATTACHED );
                        }
                    }
                }
            }
        }
    }
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositions ()
{
    // model-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel.size(); ++i ) {
        for ( unsigned int j = i; j < m_ListOfInteractionModelVsModel.size(); ++j ) {
            EnforceAllConsistentPositionsOfTwoModels( i, j );
        }
    }

    // ERSuture-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionERSutureVsModel.size(); ++i ) {
        for ( unsigned int j = 0; j < m_ListOfInteractionERSutureVsModel[i].size(); ++j ) {
            EnforceAllConsistentPositionsOfERSutureAndModel( i, j );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositionsOfTwoModels ( int modelA, int modelB )
{
    modelB -= modelA;   // since the data in the std::vector starts from 0, not from the model number
    std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * >::iterator it = m_ListOfInteractionModelVsModel[modelA][modelB].begin();
    while ( it != m_ListOfInteractionModelVsModel[modelA][modelB].end() ) {
        (*it)->RefToSavedPositionA() = (*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition();
        (*it)->RefToSavedPositionB() = (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition();
        (*it)->GetPtrToVertexFromModel_1()->GetProtectedPosition() = (*it)->RefToTargetPositionA();
        (*it)->GetPtrToVertexFromModel_2()->GetProtectedPosition() = (*it)->RefToTargetPositionB();
        ++it;
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::EnforceAllConsistentPositionsOfERSutureAndModel ( int suture, int model )
{
    Matrix4x4<T> TrxModel;
    Matrix4x4<T> InvTrxModel;
    TrxModel = m_ListOfModelsForSurgeryBasedOnFEM[model]->RefToTransformationSupport().ReturnMatrixTransform();
    InvTrxModel = TrxModel.GetInverse();

    std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >::iterator it = m_ListOfInteractionERSutureVsModel[suture][model].begin();
    while ( it != m_ListOfInteractionERSutureVsModel[suture][model].end() ) {

        // Move the suture's constrained point to the target point
        //Vector3<T> & Sp = m_ListOfSutureModelsBasedOnER[suture]->RefToPreviousVertexPositionOfNodeNumber( (*it)->GetVertexIDFromSuture() );
        //Vector3<T> & Sc = m_ListOfSutureModelsBasedOnER[suture]->RefToCurrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromSuture() );
        //Sp = Sc = (*it)->RefToTargetPositionA();

        unsigned int nodeID = (*it)->GetVertexIDFromSuture();

        // Enforce Suture's Position
        m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[nodeID].EnforcedPosition = (*it)->RefToTargetPositionA();

        // Move the model's constrained point to the target point
        (*it)->RefToSavedPositionB() = (*it)->GetPtrToVertexFromModel()->GetProtectedPosition();
        //(*it)->GetPtrToVertexFromModel()->GetProtectedPosition() = (*it)->RefToTargetPositionB(); // TO THE TARGET POSITION B
        (*it)->GetPtrToVertexFromModel()->GetProtectedPosition() = InvTrxModel * 
            m_ListOfSutureModelsBasedOnER[suture]->RefToCurrentVertexPositionOfNodeNumber( 
                (*it)->GetVertexIDFromSuture() );   // TO THE SUTURE POINT

        ++it;
    }
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::ClearAllConstraints ()
{
    // Clear model-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel.size(); ++i ) {
        for ( unsigned int j = i; j < m_ListOfInteractionModelVsModel.size(); ++j ) {
            ClearAllConstraintsOfTwoModels( i, j );
        }
    }

    // Clear ERSuture-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionERSutureVsModel.size(); ++i ) {
        for ( unsigned int j = 0; j < m_ListOfInteractionERSutureVsModel[i].size(); ++j ) {
            ClearAllConstraintsOfERSutureAndModel( i, j );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfTwoModels ( int modelA, int modelB )
{
    modelB -= modelA;   // since the data in the std::vector starts from 0, not from the model number
    std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * >::iterator it = m_ListOfInteractionModelVsModel[modelA][modelB].begin();
    while ( it != m_ListOfInteractionModelVsModel[modelA][modelB].end() ) {
        delete *it;
        ++it;
    }
    m_ListOfInteractionModelVsModel[modelA][modelB].clear();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::ClearAllConstraintsOfERSutureAndModel ( int suture, int model )
{
    std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >::iterator it = m_ListOfInteractionERSutureVsModel[suture][model].begin();
    while ( it != m_ListOfInteractionERSutureVsModel[suture][model].end() ) {

        // Commented out because causing a run-time error during the exit of the program
        // ALL OF THESE STATEMENTS ARE ADDED INTO Reset() function of ModelElasticRod.
        //m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].EnforcedPosition.Clear();
        //m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].UseEnforcedPosition = false;
        //m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].ExternalForce_ForAdvSimCtrl.Clear();

        delete *it;
        ++it;
    }
    m_ListOfInteractionERSutureVsModel[suture][model].clear();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::ClearTheConstraintNumberOfTwoModels ( int modelA, int modelB, unsigned int constraintNumber )
{
    // REMARK: The erased element's destructor is called before it is removed from the std::vector.
    // Therefore, calling the desctructor after the erase will generate a runtime error.
    modelB -= modelA;   // since the data in the std::vector starts from 0, not from the model number
    m_ListOfInteractionModelVsModel[modelA][modelB].erase( m_ListOfInteractionModelVsModel[modelA][modelB].begin() + constraintNumber );
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::ClearTheConstraintNumberOfERSutureAndModel ( int suture, int model, unsigned int constraintNumber )
{
    // REMARK: The erased element's destructor is called before it is removed from the std::vector.
    // Therefore, calling the desctructor after the erase will generate a runtime error.
    std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >::iterator it = m_ListOfInteractionERSutureVsModel[suture][model].begin() + constraintNumber;
    m_ListOfInteractionERSutureVsModel[suture][model].erase( it );

    // Enforce Suture's Position
    //m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].EnforcedPosition.Clear();
    //m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].UseEnforcedPosition = false;

    m_ListOfSutureModelsBasedOnER[suture]->GetListOfNodes()[(*it)->GetVertexIDFromSuture()].ExternalForce_ForAdvSimCtrl.Clear();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AdvSimAllModels (
    T timestep,                         
    bool bEnforceAllConstraints,        
    bool bEnforceAllConsistentPositions 
)
{
    // Enforce all constraints
    if ( bEnforceAllConstraints ) {
        EnforceAllConstraints();
    }

    //for ( unsigned int i = 0; i < m_ListOfSutureModelsBasedOnMSS.size(); ++i ) {
    //  AdvSimSutureModelBasedOnMSS( i, timestep );
    //}
    for ( unsigned int i = 0; i < m_ListOfSutureModelsBasedOnER.size(); ++i ) {
        AdvSimSutureModelBasedOnER( i, timestep );
    }
    //for ( unsigned int i = 0; i < m_ListOfModelsForSurgeryBasedOnMSS.size(); ++i ) {
    //  AdvSimModelForSurgeryBasedOnMSS( i, timestep );
    //}
    for ( unsigned int i = 0; i < m_ListOfModelsForSurgeryBasedOnFEM.size(); ++i ) {
        AdvSimModelForSurgeryBasedOnFEM( i, timestep );
    }

    if ( bEnforceAllConsistentPositions ) {
        EnforceAllConsistentPositions();
    }

    for ( unsigned int i = 0; i < m_ListOfModelsForSurgeryBasedOnFEM.size(); ++i ) {
        m_ListOfModelsForSurgeryBasedOnFEM[i]->UpdateSurfaceMeshNormals();
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AdvSimSutureModelBasedOnMSS ( unsigned int sutureID, T timestep )
{
    m_ListOfSutureModelsBasedOnMSS[sutureID]->AdvanceSimulation( 0, timestep );
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AdvSimSutureModelBasedOnER ( unsigned int sutureID, T timestep )
{
    m_ListOfSutureModelsBasedOnER[sutureID]->AdvanceSimulation();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AdvSimModelForSurgeryBasedOnMSS ( unsigned int sutureID, T timestep )
{
    m_ListOfModelsForSurgeryBasedOnMSS[sutureID]->AdvanceSimulation( 0, timestep );
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::AdvSimModelForSurgeryBasedOnFEM ( unsigned int sutureID, T timestep )
{
    m_ListOfModelsForSurgeryBasedOnFEM[sutureID]->AdvanceSimulationDynamic();
}
//-----------------------------------------------------------------------------




//=============================================================================
#if defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::DrawAllModels ()
{
    std::vector< OpenGL::ModelSuture<T> * >::iterator A = m_ListOfSutureModelsBasedOnMSS.begin();
    while ( A != m_ListOfSutureModelsBasedOnMSS.end() ) {
        (*A)->Draw();
        ++A;
    }
    std::vector< ModelForSurgery<T> * >::iterator B = m_ListOfModelsForSurgeryBasedOnMSS.begin();
    while ( B != m_ListOfModelsForSurgeryBasedOnMSS.end() ) {
        (*B)->Draw();
        ++B;
    }
    std::vector< ModelElasticRod<T> * >::iterator C = m_ListOfSutureModelsBasedOnER.begin();
    while ( C != m_ListOfSutureModelsBasedOnER.end() ) {
        (*C)->Draw();
        ++C;
    }
    std::vector< ModelDefBasedOnFEM<T,DATA> * >::iterator D = m_ListOfModelsForSurgeryBasedOnFEM.begin();
    while ( D != m_ListOfModelsForSurgeryBasedOnFEM.end() ) {
        (*D)->Draw();
        ++D;
    }
}
//-----------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::DrawAddedInteractions ()
{
    // Draw model-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionModelVsModel.size(); ++i ) {
        for ( unsigned int j = i; j < m_ListOfInteractionModelVsModel.size(); ++j ) {
            DrawAddedInteractionsBetweenTwoModels( i, j );
            //std::cout << "model#" << i << " vs model#" << j+i << "\n";
        }
    }

    // Draw ERSuture-model interactions
    for ( unsigned int i = 0; i < m_ListOfInteractionERSutureVsModel.size(); ++i ) {
        for ( unsigned int j = 0; j < m_ListOfInteractionERSutureVsModel[i].size(); ++j ) {
            DrawAddedInteractionsBetweenERSutureAndModel( i, j );
            //std::cout << "ERsuture#" << i << " vs model#" << j << "\n";
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenTwoModels ( int modelA, int modelB )
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glDisable( GL_LIGHTING );
    glLineWidth( 2 );
    glPointSize( 5 );
    GLfloat V[2][3];

    Matrix4x4<T> Trx1;
    if ( m_ModelType[modelA] == MODEL_BASED_ON_FEM ) {
        unsigned int loc = m_ModelLocation[modelA];
        Trx1 = m_ListOfModelsForSurgeryBasedOnFEM[modelA]->RefToTransformationSupport().ReturnMatrixTransform();
    }
    Matrix4x4<T> Trx2;
    if ( m_ModelType[modelB] == MODEL_BASED_ON_FEM ) {
        unsigned int loc = m_ModelLocation[modelB];
        Trx2 = m_ListOfModelsForSurgeryBasedOnFEM[modelB]->RefToTransformationSupport().ReturnMatrixTransform();
    }

    modelB -= modelA;   // since the data in the std::vector starts from 0, not from the model number
    std::vector< AdvSimConstraint_HEVertexVsHEVertex<T> * >::iterator it = m_ListOfInteractionModelVsModel[modelA][modelB].begin();
    while ( it != m_ListOfInteractionModelVsModel[modelA][modelB].end() ) {
        Vector3<T> v1 = (Trx1 * Vector4<T>( (*it)->GetPtrToVertexFromModel_1()->GetPosition() ) ).GetVector3();
        Vector3<T> v2 = (Trx2 * Vector4<T>( (*it)->GetPtrToVertexFromModel_2()->GetPosition() ) ).GetVector3();
        V[0][0] = v1[0];    V[0][1] = v1[1];    V[0][2] = v1[2];
        V[1][0] = v2[0];    V[1][1] = v2[1];    V[1][2] = v2[2];
        glBegin( GL_POINTS );
            glColor3f( 0.3f, 0.7f, 0.2f );
            glVertex3fv( V[0] );
            glVertex3fv( V[1] );
            glColor3f( 1.0f, 1.0f, 0.0f );
            glVertex3fv( (Trx1*Vector4<T>((*it)->RefToTargetPositionA())).GetVector3().GetDataFloat() );
            glColor3f( 1.0f, 0.0f, 1.0f );
            glVertex3fv( (Trx2*Vector4<T>((*it)->RefToTargetPositionB())).GetVector3().GetDataFloat() );
        glEnd();
        glBegin( GL_LINES );
            glColor3f( 0.3f, 0.7f, 0.5f );
            glVertex3fv( V[0] );
            glVertex3fv( V[1] );
        glEnd();
        ++it;
    }
    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T, typename DATA>
void AdvSimSupport_DS<T,DATA>::DrawAddedInteractionsBetweenERSutureAndModel ( int suture, int model )
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glDisable( GL_LIGHTING );
    glLineWidth( 2 );
    glPointSize( 5 );
    GLfloat V[2][3];
    
    Matrix4x4<T> Trx;
    if ( m_ModelType[model] == MODEL_BASED_ON_FEM ) {
        unsigned int loc = m_ModelLocation[model];
        Trx = m_ListOfModelsForSurgeryBasedOnFEM[model]->RefToTransformationSupport().ReturnMatrixTransform();
    }

    std::vector< AdvSimConstraint_ERSutureVsHEVertex<T> * >::iterator it = m_ListOfInteractionERSutureVsModel[suture][model].begin();
    while ( it != m_ListOfInteractionERSutureVsModel[suture][model].end() ) {
        Vector3<T> v1 = m_ListOfSutureModelsBasedOnER[suture]->RefToCurrrentVertexPositionOfNodeNumber( (*it)->GetVertexIDFromSuture() );
        Vector3<T> v2 = (Trx * Vector4<T>( (*it)->GetPtrToVertexFromModel()->GetPosition() ) ).GetVector3();
        V[0][0] = v1[0];    V[0][1] = v1[1];    V[0][2] = v1[2];
        V[1][0] = v2[0];    V[1][1] = v2[1];    V[1][2] = v2[2];
        glBegin( GL_POINTS );
            glColor3f( 0.3f, 0.7f, 0.2f );
            glVertex3fv( V[0] );
            glVertex3fv( V[1] );
            glColor3f( 1.0f, 1.0f, 0.0f );
            glVertex3fv( (*it)->RefToTargetPositionA().GetDataFloat() );
            glColor3f( 1.0f, 0.0f, 1.0f );
            glVertex3fv( (Trx*Vector4<T>((*it)->RefToTargetPositionB())).GetVector3().GetDataFloat() );
        glEnd();
        glBegin( GL_LINES );
            glColor3f( 0.3f, 0.7f, 0.5f );
            glVertex3fv( V[0] );
            glVertex3fv( V[1] );
        glEnd();
        ++it;
    }
    glPopAttrib();
}
//-----------------------------------------------------------------------------
#endif  // #if defined(__gl_h_) || defined(__GL_H__)
//=============================================================================

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