TAPsAdvSimConstraints.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsAdvSimConstraints.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (03/16/2011)
UPDATE          (03/31/2011)
******************************************************************************/
#include "TAPsAdvSimConstraints.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
//=============================================================================
// class AdvSimConstraint_ConstrainedPoint
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_ConstrainedPoint<T>::AdvSimConstraint_ConstrainedPoint ()
    : PointID( -1 )
{}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_ConstrainedPoint<T>::AdvSimConstraint_ConstrainedPoint ( AdvSimConstraint_ConstrainedPoint<T> const &orig )
    : PointID( orig.PointID )
    , HomePosition( orig.HomePosition )
    , TargetPosition( orig.TargetPosition )
{}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_ConstrainedPoint<T> & AdvSimConstraint_ConstrainedPoint<T>::operator= ( AdvSimConstraint_ConstrainedPoint<T> const &orig )
{   
    PointID = orig.PointID;
    HomePosition = orig.HomePosition;
    TargetPosition = orig.TargetPosition;
    return *this;
}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_ConstrainedPoint<T>::~AdvSimConstraint_ConstrainedPoint ()
{}
//-----------------------------------------------------------------------------
template <typename T>
std::string AdvSimConstraint_ConstrainedPoint<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "AdvSimConstraint_ConstrainedPoint<" << typeid(T).name() << ">";
    ss << "\nHome Position ("<<HomePosition;
    ss << "); TargetPosition ("<<TargetPosition;
    ss << "); PointNumber ("<<PointNumber;
    ss << ")\n";
    return ss.str();
}
//-----------------------------------------------------------------------------
// class AdvSimConstraint_ConstrainedPoint
//=============================================================================


//=============================================================================
// class AdvSimConstraint_GroupOfConstrainedPoints
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_GroupOfConstrainedPoints<T>::AdvSimConstraint_GroupOfConstrainedPoints ()
    : Ratio( 0.5f )
    //, ModelID( -1 )
{}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_GroupOfConstrainedPoints<T>::AdvSimConstraint_GroupOfConstrainedPoints ( AdvSimConstraint_GroupOfConstrainedPoints<T> const &orig )
    : Ratio( orig.Ratio )
    //, ModelID( orig.ModelID )
    , ListOfConstrainedPoints( orig.ListOfConstrainedPoints )
{}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_GroupOfConstrainedPoints<T> & AdvSimConstraint_GroupOfConstrainedPoints<T>::operator= ( AdvSimConstraint_GroupOfConstrainedPoints<T> const &orig )
{   
    Ratio = orig.Ratio;
    //ModelID = orig.ModelID;
    ListOfConstrainedPoints = orig.ListOfConstrainedPoints;
    return *this;
}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraint_GroupOfConstrainedPoints<T>::~AdvSimConstraint_GroupOfConstrainedPoints ()
{
    ListOfConstrainedPoints.clear();
}
//-----------------------------------------------------------------------------
template <typename T>
std::string AdvSimConstraint_GroupOfConstrainedPoints<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "AdvSimConstraint_GroupOfConstrainedPoints<" << typeid(T).name() << ">";
    ss << " contains " << ListOfConstrainedPoints.size() << " of constrained points\n";
    for ( unsigned int i = 0; i < ListOfConstrainedPoints.size(); ++i ) {
        ss << "Point#" << i << ": " << ListOfConstrainedPoints[i];
    }
    ss << "\n";
    return ss.str();
}
//-----------------------------------------------------------------------------
#if defined(__gl_h_) || defined(__GL_H__)
template <typename T>
void AdvSimConstraint_GroupOfConstrainedPoints<T>::Draw ( Vector3<T> & color, T PtSize ) const
{
    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glPointSize( static_cast<float>(PtSize) );
    glEnable( GL_COLOR_MATERIAL );
    glColor3fv( color.GetDataFloat() );
    glBegin( GL_POINTS );
    for ( unsigned int i = 0; i < ListOfConstrainedPoints.size(); ++i ) {
        glVertex3fv( ListOfConstrainedPoints[i].TargetPosition.GetDataFloat() );
    }
    glEnd();
    glBegin( GL_LINE_STRIP );
    glLineWidth( 1 );
    for ( unsigned int i = 0; i < ListOfConstrainedPoints.size(); ++i ) {
        glVertex3fv( ListOfConstrainedPoints[i].TargetPosition.GetDataFloat() );
    }
    glEnd();
    glPopAttrib();
}
#endif
//-----------------------------------------------------------------------------
// class AdvSimConstraint_GroupOfConstrainedPoints
//=============================================================================


//=============================================================================
// class AdvSimConstraints
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraints<T>::AdvSimConstraints ()
{}
//-----------------------------------------------------------------------------
//template <typename T>
//AdvSimConstraints<T>::AdvSimConstraints ( AdvSimConstraints<T> const &orig )
//{}
//-----------------------------------------------------------------------------
//template <typename T>
//AdvSimConstraints<T> & AdvSimConstraints<T>::operator= ( AdvSimConstraints<T> const &orig )
//{ 
//  return *this;
//}
//-----------------------------------------------------------------------------
template <typename T>
AdvSimConstraints<T>::~AdvSimConstraints ()
{
    m_ListOfConstrainedPtGrps.clear();
}
//-----------------------------------------------------------------------------
template <typename T>
std::string AdvSimConstraints<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "AdvSimConstraints<" << typeid(T).name() << ">";
    ss << " contains " << m_ListOfConstrainedPtGrps.size << " of constrained point groups\n";
    for ( unsigned int i = m_ListOfConstrainedPtGrps; i < m_ListOfConstrainedPtGrps.size(); ++i ) {
        ss << "Group#" << i << ": " << m_ListOfConstrainedPtGrps[i];
    }
    ss << "\n";
    return ss.str();
}
//-----------------------------------------------------------------------------
template <typename T>
unsigned int AdvSimConstraints<T>::AddConstrainedModelER (
    ModelElasticRod<T> & ModelER    
)
{
    m_ListOfConstrainedERs.push_back( AdvSimConstraint_ConstrainedER<T>() );
    m_ListOfConstrainedERs.back().pModelER = &ModelER;
    return m_ListOfConstrainedERs.size() - 1;
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::AssociateCPtsGrpWithModelER (
    unsigned int CPtsGrp_ID,    
    unsigned int ModelER_ID     
)
{
    assert( CPtsGrp_ID < m_ListOfConstrainedPtGrps.size() );
    assert( ModelER_ID < m_ListOfConstrainedERs.size() );
    m_ListOfConstrainedERs[ModelER_ID].ListOfCPtsGrpIDs.push_back( CPtsGrp_ID );
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstraints_A__NotForMultiGrp (
    ModelElasticRod<T> * pER,   
    int groupID     
)
{
    VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
    int size = CPts.size();
    T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

    Real val_s = 0;
    Real val_e = 0;
    Real extendThd;

    int firstPt = 0;
    int lastPt = size - 1;// - firstPt;
    int pt_s = CPts[firstPt].PointID;
    int pt_e = pt_s + lastPt;

    TAPs::Vector3<Real> & S_s2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s );
    TAPs::Vector3<Real> & S_e2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e );

    //
    // Case the number of constrained points is more than 2
    //
    // Compare the link lengths formed by the two home positions of the 1st & 2nd constraints 
    // with the suture link length formed by the two constrained points associated with the two constraints.
    // If the suture link length is longer, then prepare to check for a forward movement of the suture
    // by calculating the relative length of the suture link compare to the constrained link, noted as Ls.
    // Else mark the length as zero.
    //
    // Compare the link lengths formed by the two home positions of the last and previous one before the last constraints
    // with the suture link length formed by the two constrained points associated with the two constriants.
    // If the suture link length is longer, then prepare to check for a backward movement of the suture
    // by calculating the relative length of the suture link compare to the constrained link, noted as Le.
    // Else mark the length as zero.
    //
    // Pick one with the longer length of Ls and Le, and check it against the threshold length.
    // If the length is greater than the threshold, then do the move accordingly 
    // -- If Ls is picked, then move forward, else move backward.

    // If the first link length is longer than the last link length, then check for forward movement.
    // Else check for backward movement.
    if ( size > 2 ) {

        extendThd = 1 + ratio*2;

        // Find the first constrained link formed by the 1st & 2nd constraints
        Real LC_s = (CPts[0].HomePosition - CPts[1].HomePosition).Length();

        //Find the last constrained formed by the last & the previous one before the last constraints
        Real LC_e = (CPts[lastPt].HomePosition - CPts[lastPt-1].HomePosition).Length();

        //TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
        TAPs::Vector3<Real> S_e1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e-1 );
        TAPs::Vector3<Real> S_s3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s+1 );
        //TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );

        //Real L_s1 = (S_s1 - S_s2).Length();
        Real L_e1 = (S_e1 - S_e2).Length();
        Real L_s2 = (S_s3 - S_s2).Length();
        //Real L_e2 = (S_e3 - S_e2).Length();

        val_s = L_s2 / LC_s;
        val_e = L_e1 / LC_e;
    }
    //
    // Case the number of constrained points is less than 3
    //
    // In this case there are only one link if the number of constraints is two 
    // and no link if the number of constraints is one.
    //
    // The check is done by comparing the length between the two links that sandwiches the constraint(s).
    else {

        extendThd = 1 + ratio*2;

        TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
        TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );
        Real Ls = (S_s1 - S_s2).Length();
        Real Le = (S_e3 - S_e2).Length();

        val_s = Ls / Le;
        val_e = Le / Ls;
    }

    //
    // Determine if the conditions are allowed for forward or backward movement
    //
    bool bCanMoveForward = true;
    bool bCanMoveBackward = true;
    ElasticRodNode<T> & NodeBegin = pER->GetListOfNodes()[pt_s-1];
    ElasticRodNode<T> & NodeEnd = pER->GetListOfNodes()[pt_e+1];

    /*
    if ( !NodeEnd.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )    {
        bCanMoveForward = false;
    }
    if ( !NodeBegin.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )  {
        bCanMoveBackward = false;
    }
    //*/

    //
    // If the longer length is longer than the shorter link by 
    //
    // Move forward
    if ( val_s > val_e ) {

        if ( pt_e < pSuture->GetNumOfPoints()-4 )   // boundary protection
        if ( bCanMoveForward )
        if ( val_s > extendThd )
        {

            //*
            // Clear the start node
            ElasticRodNode<T> & NodeS = pER->GetListOfNodes()[pt_s];
            NodeS.ExternalForce_ForAdvSimCtrl.Clear();
            NodeS.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

            // Set the new end node
            //NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
            NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
            //*/

            for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                ++CPts[i].PointID;
            }
        }
    }
    // Move backward
    else {
        if ( pt_s > 4 ) // boundary protection
        if ( bCanMoveBackward )
        if ( val_e > extendThd )
        {

            //*
            // Clear the end node
            ElasticRodNode<T> & NodeE = pER->GetListOfNodes()[pt_e+1];
            NodeE.ExternalForce_ForAdvSimCtrl.Clear();
            NodeE.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

            // Set the new start node
            //NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
            NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
            //*/

            for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                --CPts[i].PointID;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstraints_B__NotForMultiGrp (
    ModelElasticRod<T> * pER,   
    int groupID     
)
{
    VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
    T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

    // Constraint the positions
    for ( unsigned int i = 0; i < CPts.size(); ++i ) {
        int pt = CPts[i].PointID;
        TAPs::Vector3<Real> Target = ratio*pER->RefToCurrentVertexPositionOfNodeNumber( pt ) + (1-ratio)*CPts[i].HomePosition;
    
        pER->GetListOfNodes()[pt].Centerline[pSuture->GetCurrentIndex()].SetPosition( Target );

        CPts[i].TargetPosition = Target;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstraints_A (
    ModelElasticRod<T> * pER,   
    int groupID     
)
{
    VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
    int size = CPts.size();
    T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

    Real val_s = 0;
    Real val_e = 0;
    Real extendThd;

    int firstPt = 0;
    int lastPt = size - 1;// - firstPt;
    int pt_s = CPts[firstPt].PointID;
    int pt_e = pt_s + lastPt;

    //std::cout << "Grp#" << groupID << "; pt_s: " << pt_s << ", pt_e: " << pt_e << "\n";
    //std::cout << "ratio: " << ratio << "\n";

    //std::cout << "Aaddresses: " << &CPts << " and " << &m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints << "\n";
    //std::cout << "Baddresses: " << &CPts[0] << " and " << &m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints[0] << "\n";

    TAPs::Vector3<Real> & S_s2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s );
    TAPs::Vector3<Real> & S_e2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e );

    //
    // Case the number of constrained points is more than 2
    //
    // Compare the link lengths formed by the two home positions of the 1st & 2nd constraints 
    // with the suture link length formed by the two constrained points associated with the two constraints.
    // If the suture link length is longer, then prepare to check for a forward movement of the suture
    // by calculating the relative length of the suture link compare to the constrained link, noted as Ls.
    // Else mark the length as zero.
    //
    // Compare the link lengths formed by the two home positions of the last and previous one before the last constraints
    // with the suture link length formed by the two constrained points associated with the two constriants.
    // If the suture link length is longer, then prepare to check for a backward movement of the suture
    // by calculating the relative length of the suture link compare to the constrained link, noted as Le.
    // Else mark the length as zero.
    //
    // Pick one with the longer length of Ls and Le, and check it against the threshold length.
    // If the length is greater than the threshold, then do the move accordingly 
    // -- If Ls is picked, then move forward, else move backward.

    // If the first link length is longer than the last link length, then check for forward movement.
    // Else check for backward movement.
    if ( size > 2 ) {
    //if ( false ) {

        extendThd = 1 + ratio*2;

        // Find the first constrained link formed by the 1st & 2nd constraints
        Real LC_s = (CPts[0].HomePosition - CPts[1].HomePosition).Length();

        //Find the last constrained formed by the last & the previous one before the last constraints
        Real LC_e = (CPts[lastPt].HomePosition - CPts[lastPt-1].HomePosition).Length();

        //TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
        TAPs::Vector3<Real> S_e1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e-1 );
        TAPs::Vector3<Real> S_s3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s+1 );
        //TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );

        //Real L_s1 = (S_s1 - S_s2).Length();
        Real L_e1 = (S_e1 - S_e2).Length();
        Real L_s2 = (S_s3 - S_s2).Length();
        //Real L_e2 = (S_e3 - S_e2).Length();

        val_s = L_s2 / LC_s;
        val_e = L_e1 / LC_e;

        //std::cout << "pt_s: " << pt_s << "; " << "pt_e: " << pt_e << "\n";
        //std::cout << "val_s and val_e: " << val_s << ", " << val_e << "\n";

    }
    //
    // Case the number of constrained points is less than 3
    //
    // In this case there are only one link if the number of constraints is two 
    // and no link if the number of constraints is one.
    //
    // The check is done by comparing the length between the two links that sandwiches the constraint(s).
    else {

        extendThd = 1 + ratio*2;

        TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
        TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );
        Real Ls = (S_s1 - S_s2).Length();
        Real Le = (S_e3 - S_e2).Length();

        val_s = Ls / Le;
        val_e = Le / Ls;
    }


    //std::cout << "TEST: val_s and val_e: " << val_s << ", " << val_e << "\n";
    //std::cout << "extendThd: " << extendThd << "\n";


    //
    // Determine if the conditions are allowed for forward or backward movement
    //
    bool bCanMoveForward = true;
    bool bCanMoveBackward = true;
    ElasticRodNode<T> & NodeBegin = pER->GetListOfNodes()[pt_s-1];
    ElasticRodNode<T> & NodeEnd = pER->GetListOfNodes()[pt_e+1];

    if ( !NodeEnd.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )    {
        bCanMoveForward = false;
    }
    if ( !NodeBegin.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )  {
        bCanMoveBackward = false;
    }

    //std::cout << "bCanMoveForward: " << bCanMoveForward << "\n";
    //std::cout << "bCanMoveBackward: " << bCanMoveBackward << "\n";

    //
    // If the longer length is longer than the shorter link by 
    //
    // Move forward
    if ( val_s > val_e ) {

        if ( pt_e < pSuture->GetNumOfPoints()-4 )   // boundary protection
        if ( bCanMoveForward )
        if ( val_s > extendThd )
        {

            //*
            // Clear the start node
            ElasticRodNode<T> & NodeS = pER->GetListOfNodes()[pt_s];
            NodeS.ExternalForce_ForAdvSimCtrl.Clear();
            NodeS.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

            // Set the new end node
            //NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
            NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
            //*/

            for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                ++CPts[i].PointID;
            }
        }
    }
    // Move backward
    else {
        if ( pt_s > 4 ) // boundary protection
        if ( bCanMoveBackward )
        if ( val_e > extendThd )
        {

            //*
            // Clear the end node
            ElasticRodNode<T> & NodeE = pER->GetListOfNodes()[pt_e+1];
            NodeE.ExternalForce_ForAdvSimCtrl.Clear();
            NodeE.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

            // Set the new start node
            //NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
            NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
            //*/

            for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                --CPts[i].PointID;
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstraints_B (
    ModelElasticRod<T> * pER,   
    int groupID     
)
{
    VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
    T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

    // Constraint the positions
    for ( unsigned int i = 0; i < CPts.size(); ++i ) {
        int pt = CPts[i].PointID;
        TAPs::Vector3<Real> Target = ratio*pER->RefToCurrentVertexPositionOfNodeNumber( pt ) + (1-ratio)*CPts[i].HomePosition;
    
        pER->GetListOfNodes()[pt].Centerline[pER->GetCurrentIndex()].SetPosition( Target );

        CPts[i].TargetPosition = Target;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstraints_B__ByForce (
    ModelElasticRod<T> * pER,   
    int groupID,                
    Vector3<T> & Force          
)
{
    VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
    T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

    // Constraint the positions
    for ( unsigned int i = 0; i < CPts.size(); ++i ) {
        int pt = CPts[i].PointID;
        TAPs::Vector3<Real> Target = ratio*pER->RefToCurrentVertexPositionOfNodeNumber( pt ) + (1-ratio)*CPts[i].HomePosition;
    
        pER->GetListOfNodes()[pt].ExternalForce_Constrained[0] = (Target-pER->RefToCurrentVertexPositionOfNodeNumber( pt ))[0] * Force[0];
        pER->GetListOfNodes()[pt].ExternalForce_Constrained[1] = (Target-pER->RefToCurrentVertexPositionOfNodeNumber( pt ))[1] * Force[1];
        pER->GetListOfNodes()[pt].ExternalForce_Constrained[2] = (Target-pER->RefToCurrentVertexPositionOfNodeNumber( pt ))[2] * Force[2];

        CPts[i].TargetPosition = Target;
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceConstrainedModelER_A (
    AdvSimConstraint_ConstrainedER<T> & ConstrainedER   
)
{
    ModelElasticRod<T> * pER = ConstrainedER.pModelER;

    for ( unsigned int i = 0; i < ConstrainedER.ListOfCPtsGrpIDs.size(); ++i )
    {
        unsigned int groupID = ConstrainedER.ListOfCPtsGrpIDs[i];
        // The constrained point group that contains the start part
        VectorOfCPts & CPts = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;
        int size = CPts.size();
        T ratio = m_ListOfConstrainedPtGrps[groupID].Ratio;

        Real val_s = 0;
        Real val_e = 0;
        Real extendThd;

        int firstPt = 0;
        int lastPt = size - 1;// - firstPt;
        int pt_s = CPts[firstPt].PointID;
        int pt_e = pt_s + lastPt;

        //
        // Check the next constrained point groups
        //
        unsigned int i_old = i;
        unsigned int groupIDNext = groupID;
        unsigned int j = i+1;
        bool bKeepLooping = true;
        while ( bKeepLooping && j < ConstrainedER.ListOfCPtsGrpIDs.size() ) {
            groupIDNext = ConstrainedER.ListOfCPtsGrpIDs[j];
            VectorOfCPts & CPtsNext = m_ListOfConstrainedPtGrps[groupIDNext].ListOfConstrainedPoints;
            int pt_s_next = CPtsNext[0].PointID;
            if ( pt_s_next == pt_e+1 ) {
                lastPt = CPtsNext.size() - 1;
                pt_e = pt_s_next + lastPt;
                groupID = groupIDNext;  // add this group into the calculation
                i = j;  // will skip the next group in the next iteration of the for loop
                size += lastPt + 1;
            }
            else {
                bKeepLooping = false;
            }
            ++j;
        }

        // The constrained point group that contains the end part
        VectorOfCPts & CPtsNext = m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints;


        //std::cout << "Grp#" << groupID << "; pt_s: " << pt_s << ", pt_e: " << pt_e << "\n";
        //std::cout << "ratio: " << ratio << "\n";

        //std::cout << "Aaddresses: " << &CPts << " and " << &m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints << "\n";
        //std::cout << "Baddresses: " << &CPts[0] << " and " << &m_ListOfConstrainedPtGrps[groupID].ListOfConstrainedPoints[0] << "\n";

        TAPs::Vector3<Real> & S_s2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s );
        TAPs::Vector3<Real> & S_e2 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e );

        //
        // Case the number of constrained points is more than 2
        //
        // Compare the link lengths formed by the two home positions of the 1st & 2nd constraints 
        // with the suture link length formed by the two constrained points associated with the two constraints.
        // If the suture link length is longer, then prepare to check for a forward movement of the suture
        // by calculating the relative length of the suture link compare to the constrained link, noted as Ls.
        // Else mark the length as zero.
        //
        // Compare the link lengths formed by the two home positions of the last and previous one before the last constraints
        // with the suture link length formed by the two constrained points associated with the two constriants.
        // If the suture link length is longer, then prepare to check for a backward movement of the suture
        // by calculating the relative length of the suture link compare to the constrained link, noted as Le.
        // Else mark the length as zero.
        //
        // Pick one with the longer length of Ls and Le, and check it against the threshold length.
        // If the length is greater than the threshold, then do the move accordingly 
        // -- If Ls is picked, then move forward, else move backward.

        // If the first link length is longer than the last link length, then check for forward movement.
        // Else check for backward movement.
        if ( size > 2 ) {
        //if ( false ) {

            extendThd = 1 + ratio*1.25;

            // Find the first constrained link formed by the 1st & 2nd constraints
            Real LC_s = (CPts[0].HomePosition - CPts[1].HomePosition).Length();

            //Find the last constrained formed by the last & the previous one before the last constraints
            Real LC_e = (CPtsNext[lastPt].HomePosition - CPtsNext[lastPt-1].HomePosition).Length();

            //TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
            TAPs::Vector3<Real> S_e1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e-1 );
            TAPs::Vector3<Real> S_s3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s+1 );
            //TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );

            //Real L_s1 = (S_s1 - S_s2).Length();
            Real L_e1 = (S_e1 - S_e2).Length();
            Real L_s2 = (S_s3 - S_s2).Length();
            //Real L_e2 = (S_e3 - S_e2).Length();

            val_s = L_s2 / LC_s;
            val_e = L_e1 / LC_e;

            //std::cout << "pt_s: " << pt_s << "; " << "pt_e: " << pt_e << "\n";
            //std::cout << "val_s and val_e: " << val_s << ", " << val_e << "\n";

        }
        //
        // Case the number of constrained points is less than 3
        //
        // In this case there are only one link if the number of constraints is two 
        // and no link if the number of constraints is one.
        //
        // The check is done by comparing the length between the two links that sandwiches the constraint(s).
        else {

            extendThd = 1 + ratio*2;

            TAPs::Vector3<Real> S_s1 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_s-1 );
            TAPs::Vector3<Real> S_e3 = pER->RefToCurrentVertexPositionOfNodeNumber( pt_e+1 );
            Real Ls = (S_s1 - S_s2).Length();
            Real Le = (S_e3 - S_e2).Length();

            val_s = Ls / Le;
            val_e = Le / Ls;
        }


        //std::cout << "TEST: val_s and val_e: " << val_s << ", " << val_e << "\n";
        //std::cout << "extendThd: " << extendThd << "\n";


        //
        // Determine if the conditions are allowed for forward or backward movement
        //
        bool bCanMoveForward = true;
        bool bCanMoveBackward = true;
        ElasticRodNode<T> & NodeBegin = pER->GetListOfNodes()[pt_s-1];
        ElasticRodNode<T> & NodeEnd = pER->GetListOfNodes()[pt_e+1];

        //*
        if ( !NodeEnd.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )    {
            bCanMoveForward = false;
        }
        if ( !NodeBegin.SimFlags.CheckSimulationConstraints( Enum::AddOn::SLIDABLE ) )  {
            bCanMoveBackward = false;
        }
        //*/

        //std::cout << "bCanMoveForward: " << bCanMoveForward << "\n";
        //std::cout << "bCanMoveBackward: " << bCanMoveBackward << "\n";

        //
        // If the longer length is longer than the shorter link by 
        //
        // Move forward
        if ( val_s > val_e ) {

            if ( pt_e < pER->GetNumOfPoints()-4 )   // boundary protection
            if ( bCanMoveForward )
            if ( val_s > extendThd )
            {

                //*
                // Clear the start node
                ElasticRodNode<T> & NodeS = pER->GetListOfNodes()[pt_s];
                NodeS.ExternalForce_ForAdvSimCtrl.Clear();
                NodeS.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

                // Set the new end node
                //NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
                NodeEnd.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
                //*/

                for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                    ++CPts[i].PointID;
                }
            }
            else {
                i = i_old;  // if no moving then restore the i position
            }
        }
        // Move backward
        else {
            if ( pt_s > 4 ) // boundary protection
            if ( bCanMoveBackward )
            if ( val_e > extendThd )
            {

                //*
                // Clear the end node
                ElasticRodNode<T> & NodeE = pER->GetListOfNodes()[pt_e+1];
                NodeE.ExternalForce_ForAdvSimCtrl.Clear();
                NodeE.SimFlags.ClearSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );

                // Set the new start node
                //NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::SLIDABLE );
                NodeBegin.SimFlags.SetSimulationConstraints( Enum::AddOn::IGNORE_CD_FORCE );
                //*/

                for ( unsigned int i = 0; i < CPts.size(); ++i ) {
                    --CPts[i].PointID;
                }
            }
            else {
                i = i_old;  // if no moving then restore the i position
            }
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModels_A ()
{
    EnforceAllConstrainedModelERs_A();
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModels_B ()
{
    EnforceAllConstrainedModelERs_B();
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModels_B__ByForce ( Vector3<T> & Force )
{
    EnforceAllConstrainedModelERs_B__ByForce( Force );
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModelERs_A ()
{
    for ( VectorOfConstrainedERs::iterator it = m_ListOfConstrainedERs.begin(); it != m_ListOfConstrainedERs.end(); ++it )
    {
        //for ( unsigned int i = 0; i < it->ListOfCPtsGrpIDs.size(); ++i ) {
        //  EnforceConstraints_A( it->pModelER, it->ListOfCPtsGrpIDs[i] );
        //  //EnforceConstraints_A__NotForMultiGrp( it->pModelER, it->ListOfCPtsGrpIDs[i] );
        //}

        EnforceConstrainedModelER_A( *it );
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModelERs_B ()
{
    for ( VectorOfConstrainedERs::iterator it = m_ListOfConstrainedERs.begin(); it != m_ListOfConstrainedERs.end(); ++it )
    {
        for ( unsigned int i = 0; i < it->ListOfCPtsGrpIDs.size(); ++i ) {
            EnforceConstraints_B( it->pModelER, it->ListOfCPtsGrpIDs[i] );
            //EnforceConstraints_B__NotForMultiGrp( it->pModelER, it->ListOfCPtsGrpIDs[i] );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::EnforceAllConstrainedModelERs_B__ByForce ( Vector3<T> & Force )
{
    for ( VectorOfConstrainedERs::iterator it = m_ListOfConstrainedERs.begin(); it != m_ListOfConstrainedERs.end(); ++it )
    {
        for ( unsigned int i = 0; i < it->ListOfCPtsGrpIDs.size(); ++i ) {
            EnforceConstraints_B__ByForce( it->pModelER, it->ListOfCPtsGrpIDs[i], Force );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::ClearAllConstraints ()
{
    //m_ListOfConstrainedERs.clear();   // this statement is replaced by the code below
    for ( int i = m_ListOfConstrainedERs.size()-1; i >= 0; --i ) {
        ClearAllConstriantsOnERatID( i );
    }

    m_ListOfConstrainedPtGrps.clear();
}
//-----------------------------------------------------------------------------
template <typename T>
void AdvSimConstraints<T>::ClearAllConstriantsOnERatID ( unsigned int ERid )
{
    ModelElasticRod<T> * pModelER = m_ListOfConstrainedERs[ERid].pModelER;
    for ( std::vector< unsigned int >::iterator it = m_ListOfConstrainedERs[ERid].ListOfCPtsGrpIDs.begin(); it < m_ListOfConstrainedERs[ERid].ListOfCPtsGrpIDs.end(); ++it ) {
        unsigned int grpID = m_ListOfConstrainedERs[ERid].ListOfCPtsGrpIDs[*it];
        for ( VectorOfCPts::iterator pt = m_ListOfConstrainedPtGrps[grpID].ListOfConstrainedPoints.begin(); pt < m_ListOfConstrainedPtGrps[grpID].ListOfConstrainedPoints.end(); ++pt ) {
            pModelER->GetListOfNodes()[pt->PointID].ExternalForce_Constrained.Clear();
        }
    }
    m_ListOfConstrainedERs.erase( m_ListOfConstrainedERs.begin()+ERid );
}
//-----------------------------------------------------------------------------
#if defined(__gl_h_) || defined(__GL_H__)
template <typename T>
void AdvSimConstraints<T>::Draw ( Vector3<T> & color, T PtSize ) const
{
    for ( unsigned int i = 0; i < m_ListOfConstrainedPtGrps.size(); ++i ) {
        m_ListOfConstrainedPtGrps[i].Draw( color, PtSize );
    }
}
#endif
//-----------------------------------------------------------------------------
// class AdvSimConstraints
//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----