TAPsModelSurgicalSutureWithHeadNeedle.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsModelSurgicalSutureWithHeadNeedle.cpp
******************************************************************************/
/******************************************************************************
SUKITTI PUNAK   (07/22/2010)
UPDATE          (12/30/2010)
******************************************************************************/
#include "TAPsModelSurgicalSutureWithHeadNeedle.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
//=============================================================================
// Constructors
//-----------------------------------------------------------------------------
template <typename T>
ModelSurgicalSutureWithHeadNeedle<T>::ModelSurgicalSutureWithHeadNeedle (
    int iNoLinks,               
    T tRadius,                  
    T tTotalLength,             
    T tTotalMass,               
    Vector3<T> & posOfVertex0,  
    ShapeInitializationParameters ShapeParameters,  
    std::string * pHeadNeedleFile,  
    int iNoInteractionPts,          
    T   tScaleForExtendedSharpPoint 
)   
    : ModelSurgicalSutureThread<T>( iNoLinks, tRadius, tTotalLength, tTotalMass, posOfVertex0, ShapeParameters )
    , m_pHeadNeedle( NULL)
    , m_pHeadNeedle_asHalfEdgeModel( NULL )
    , m_pNeedleSharpPt( NULL )
    , m_pNeedleSutureConnectingPt( NULL )
    , m_iHeadNeedleIPtSimFlagSetCount( 0 )

    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    , m_bDisableHeadNeedle( false )
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS
{
    m_pHeadNeedle = new RigidBodyDynamics<T>();
    if ( !m_pHeadNeedle ) {
        CleanUp();
        std::cout << "The needle of surgicalSutureWithHeadNeedle was not created successfully!" << std::endl;
        return;
    }

    // Create the head needle from file and create the BVH tree (for CD) for the head needle
    if ( m_pHeadNeedle->SetModelFromFile( *pHeadNeedleFile, Enum::MODEL_TYPE_HALFEDGE ) ) {

        int level = 0;
        int totalPoints = 1;
        while ( totalPoints < iNoInteractionPts ) {
            ++level;
            totalPoints += totalPoints; // since the BVH tree is assumed to be based on a binary tree
        }

        // a. Create a set of interaction points based on the BVH tree of the head needle
        m_pHeadNeedle->CreateInteractionPointGroupBasedOnBVHTree( level );

        // NOW THE RUNTIME ERROR IS GONE. WHY?
        // So doing b. then a. is more efficient.
    }
    else {
        CleanUp();
        std::cout << "pHeadNeedleFile: " << *pHeadNeedleFile << "\n";
        std::cout << "The needle, created from filename \"" << *pHeadNeedleFile << "\", of surgicalSutureWithHeadNeedle was not created successfully!" << std::endl;
        return;
    }

    // Find the needle's sharp point and its connecting point to suture
    FindAndAddNeedleSharpPtAndSutureConnectionPt();

    // Set the radii of head needle's interaction points
    SetHeadNeedleInteractionPointsRadii();

    // Set extended sharp point of the head needle for CD on needle puncturing an object
    SetExtendedHeadNeedleSharpPt( tScaleForExtendedSharpPoint );

    // Set the head needle's postion and orientation to connect with the first point of suture thread
    CoupleHeadNeedleOrienAndPosWithSutureThreadOrienAndPos();

    // Total points
    m_iTotalPoints = iNoLinks + 1 + m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().size();
    //std::cout << "m_iTotalPoints: " << m_iTotalPoints << "\n";

    // Create CDUnit for head needle
    m_CDUnit_HeadNeedle.CreateCD( m_pHeadNeedle, m_pNeedleSharpPt, m_pNeedleSutureConnectingPt );

    // Set the list of constraint statuses of interaction points
    m_vbIsConstrained.resize( m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().size(), false );

    InitHeadNeedleCirclePath();
}
//-----------------------------------------------------------------------------
template <typename T>
ModelSurgicalSutureWithHeadNeedle<T>::~ModelSurgicalSutureWithHeadNeedle ()
{
    CleanUp();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::CleanUp ()
{
    if ( m_pHeadNeedle )    delete m_pHeadNeedle;
}
//-----------------------------------------------------------------------------
template <typename T>
std::string ModelSurgicalSutureWithHeadNeedle<T>::StrInfo () const
{
    std::ostringstream ss;
    ss << "ModelSurgicalSutureWithHeadNeedle<" << typeid(T).name() << ">";
    ss << "with suture: " << ModelSurgicalSutureThread<T>::StrInfo();
    return ss.str();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::Reset ()
{
    ModelSurgicalSutureThread<T>::Reset();

    m_iHeadNeedleIPtSimFlagSetCount = 0;
    GetHeadNeedle().UnlockPosition();

    m_HeadNeedleCirclePath.Clear();

    UnpickAllPoints();

    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    EnableHeadNeedle();
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::FindAndAddNeedleSharpPtAndSutureConnectionPt ()
{
    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() )   return;
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    // The head model is an instance of RigidBodyDynamics class.

    // Add the head needle's connecting point to suture to the head needle model 
    m_pHeadNeedle->RefToInteractionPointGroup().AddInteractionPoint( Vector3<T>(0,0,0) );

    // Add the head needle's sharp point to the head needle model
    // Assume that the furtest mesh vertex of the head model is the sharp point
    m_pHeadNeedle_asHalfEdgeModel = dynamic_cast< OpenGL::HalfEdgeModel<T> * >( m_pHeadNeedle->GetPtrToModel() );
    if ( !m_pHeadNeedle_asHalfEdgeModel ) {
        std::cout << "Cannot convert the pointer of head needle model to HalfEdge model pointer!" << std::endl;
        return;
    }
    // Find the furtest mesh vertex from the origin
    HEVertex<T> * pHEVertex = m_pHeadNeedle_asHalfEdgeModel->GetVertexList()->Head();
    Vector3<T> pos(0,0,0); // the origin
    T length, distance = 0;
    while ( pHEVertex != NULL ) {
        length = pHEVertex->GetPosition().Length();
        if ( length > distance ) {
            distance = length;
            pos = pHEVertex->GetPosition();
        }
        pHEVertex = pHEVertex->Next();
    }
    // Add the furtest mesh vertex from the origin to the interaction point group
    m_pHeadNeedle->RefToInteractionPointGroup().AddInteractionPoint( pos );

    // Sort all interaction points based on the distance from the origin position
    m_pHeadNeedle->RefToInteractionPointGroup().SortInteractionPointsBasedOnDistanceFromTheOriginPos_GreaterThan();

    // After the sorting the first interaction point is the connection point between the suture and the head needle
    // and the last interaction point is the sharp point of the head needle
    m_pNeedleSutureConnectingPt = &m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().back();
    m_pNeedleSharpPt = &m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().front();

    //std::cout << "size of IPs: " << m_pHeadNeedle->RefInteractionPointGroup().GetTheSetOfInteractionPoints().size() << "\n";
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::SetHeadNeedleInteractionPointsRadii ()
{
    for ( unsigned int i = 0; i < m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints().size(); ++i ) {
        m_pHeadNeedle->RefToInteractionPointGroup().GetInteractionPointAtIndex(i).SetSize( GetRadius()*T(2.5) );
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::SetExtendedHeadNeedleSharpPt ( T scaleForExtendedSharpPoint )
{
    // From the second interaction point
    // Where the first interaction point is the head needle's sharp point
    m_ExtendedHeadNeedleSharpPtPos = GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex(1).GetPosition();

    // Reflect in x, y, and z directions, originated at the sharp point
    // and reduce the y value by a scale value, i.e. change the angle
    m_ExtendedHeadNeedleSharpPtPos[2] = m_pNeedleSharpPt->GetPosition()[2] + m_pNeedleSharpPt->GetPosition()[2] - m_ExtendedHeadNeedleSharpPtPos[2];
    m_ExtendedHeadNeedleSharpPtPos[0] = m_pNeedleSharpPt->GetPosition()[0] + m_pNeedleSharpPt->GetPosition()[0] - m_ExtendedHeadNeedleSharpPtPos[0];    
    m_ExtendedHeadNeedleSharpPtPos[1] = m_pNeedleSharpPt->GetPosition()[1] + scaleForExtendedSharpPoint*(m_pNeedleSharpPt->GetPosition()[1] - m_ExtendedHeadNeedleSharpPtPos[1]);

    // Make it extend more by a scale
    T scale = 0.5;
    m_ExtendedHeadNeedleSharpPtPos = scale*( m_ExtendedHeadNeedleSharpPtPos - m_pNeedleSharpPt->GetPosition() ) + m_pNeedleSharpPt->GetPosition();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::AdvanceSimulation ()
{
    // Collision Detection of suture thread with head needle
    this->ClearExternalForces_Reserved();
    CDRofThreadwithNeedle();

    // Suture thread simulation
    ModelSurgicalSutureThread<T>::AdvanceSimulation();

    // Head needle simulation
    GetHeadNeedle().AdvanceSimulation( GetSimTimeStep() );


    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() ) {
        CoupleHeadNeedleOrienAndPosWithSutureThreadOrienAndPos();
        return;
    }
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS


    if ( m_iHeadNeedleIPtSimFlagSetCount || IsHeadNeedlePositionLocked() ) {
        // Set the suture thread's 1st point to follow the needle
        SetSutureThreadOrienAndPosToFollowHeadNeedle();

        // The suture thread is set to follow the head needle by enforcing a constraint on it
        // The constraint is set by PickPoint and PickAt functions and is clear by UnpickPoint function.
    }
    else {
        // Couple the head needle movement with the suture thread
        CoupleHeadNeedleOrienAndPosWithSutureThreadOrienAndPos();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::CoupleHeadNeedleOrienAndPosWithSutureThreadOrienAndPos ()
{
    //static Matrix4x4<T> rotX(
    //   1,  0,  0,  0,
    //   0,  0,  1,  0,
    //   0,  1,  0,  0,
    //   0,  0,  0,  1
    //);
    //static Matrix4x4<T> rotY(
    //   0,  0, -1,  0,
    //   0,  1,  0,  0,
    //  -1,  0,  0,  0,
    //   0,  0,  0,  1
    //);
    //static Matrix4x4<T> Rot = rotX * rotY;

    // Set rotation center of the head needle
    GetHeadNeedle().SetOrientationCenter( CGMath<T>::ZeroVector );

    // Set orientation at the origin
    // An ad-hoc method to find an average of the orientations
    Quaternion<T> ori( ( GetHeadNeedle().GetOrientation() + this->RefToCurrentOrientationOfLinkNumber( 0 ) ) * T(0.5) );
    ori.Normalized();
    GetHeadNeedle().SetOrientation( ori );
    this->RefToCurrentOrientationOfLinkNumber( 0 ) = ori;

    // Set translation
    // The translation is the average of the positions -- suture thread's 1st point position and head needle's end point position
    Vector3<T> pos = ( GetHeadNeedle().GetOriginPoint() + this->RefToCurrentVertexPositionOfNodeNumber( 0 ) ) * T(0.5);
    GetHeadNeedle().SetOriginPoint( pos );
    this->RefToCurrentVertexPositionOfNodeNumber( 0 ) = pos;
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::SetSutureThreadOrienAndPosToFollowHeadNeedle ()
{
    this->RefToCurrentOrientationOfLinkNumber( 0 ) = GetHeadNeedle().GetOrientation();
    this->RefToCurrentVertexPositionOfNodeNumber( 0 ) = GetHeadNeedle().GetOriginPoint();
}
//-----------------------------------------------------------------------------
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle<T>::CDRofThreadwithNeedle ()
{
    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() )   return false;
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    // The algorithm is based on the BVH trees of the suture thread and of the head needles

    //if ( GetBVHTree_HeadNeedle() == NULL )    return false;

    bool bResult = false;

    // Collision Detection
    Matrix4x4<T> & TrxMat = GetBVHTree_HeadNeedle()->GetTransform().RefToMatrixTransform() = GetHeadNeedle().CalMatrixTransform();
    bResult = CDUnit().GetBVHTree()->TestOverlapWithTillLeafNodes( GetBVHTree_HeadNeedle() );
    if ( !bResult ) return bResult;

    // Collision Response
    bResult = false;
    T scale = RefToParameters().KselfCDR / ( RefToParameters().TimeStep * RefToParameters().TimeStep );

    // The list of collided BV leaf nodes
    std::vector< BVHNode<T> * > & Thread_BVNodes  = CDUnit().GetBVHTree()->GetListOfCollidedNodes();
    std::vector< BVHNode<T> * > & HeadNeedle_BVNodes = CDUnit().GetBVHTree()->GetListOfCollidedNodesThat();

    for ( int i = 0; i < static_cast<int>( Thread_BVNodes.size() ); ++i ) {
        BVHNodeLeafElasticRodNode<T> * NodeThread = dynamic_cast< BVHNodeLeafElasticRodNode<T> * >( Thread_BVNodes[i] );
        BVHNodeLeafALinkFormedByTwoPointMasses<T> * NodeHeadNeedle = dynamic_cast< BVHNodeLeafALinkFormedByTwoPointMasses<T> * >( HeadNeedle_BVNodes[i] );

        // Skip the first link
        if ( NodeThread->GetPtrToPrimitive_1()->ID != 0 
            //|| NodeThread->GetPtrToPrimitive_2()->ID 
        ) {
            Vector3<T> cyl_1A( NodeThread->GetPtrToPrimitive_1()->Centerline[GetCurrentIndex()].GetPosition() );
            Vector3<T> cyl_1B( NodeThread->GetPtrToPrimitive_2()->Centerline[GetCurrentIndex()].GetPosition() );
            Vector3<T> cyl_2A( TrxMat * NodeHeadNeedle->GetPtrToPrimitive_1()->GetPosition() );
            Vector3<T> cyl_2B( TrxMat * NodeHeadNeedle->GetPtrToPrimitive_2()->GetPosition() );
            Vector3<T> direction;
            T distance;
            T headNeedle_radius = ( NodeHeadNeedle->GetPtrToPrimitive_1()->GetSize() + NodeHeadNeedle->GetPtrToPrimitive_2()->GetSize() ) * T(0.5);

            TAPs::CGMath<T>::FindClosestDistanceBetweenTwoLineSegments( 
                cyl_1A, cyl_1B,
                cyl_2A, cyl_2B,
                distance,
                direction
            );
            if ( distance < ( GetRadius() + headNeedle_radius ) ) { // suture's thread radius + head needle's radius
                direction *= distance * scale / 2.0;
                NodeThread->GetPtrToPrimitive_1()->ExternalForce_Reserved -= direction * NodeThread->GetPtrToPrimitive_1()->Centerline[GetCurrentIndex()].GetMass();
                NodeThread->GetPtrToPrimitive_2()->ExternalForce_Reserved -= direction * NodeThread->GetPtrToPrimitive_2()->Centerline[GetCurrentIndex()].GetMass();
            }
        }
    }

    return bResult;
}
//-----------------------------------------------------------------------------
template <typename T>
int ModelSurgicalSutureWithHeadNeedle<T>::FindTheClosestPointToPoint (
    Vector3<T> const & closeToPoint,    
    T distance_limit                    
) const
{
    int iClosestPointNumber = -1;
    T tClosestDistance = distance_limit;
    T tDistance;

    // Check with suture thread's points
    for ( int i = 0; i < GetNumberOfSutureThreadPoints() - 1; ++i ) {   // minus 1 to prevent boundary errors
        tDistance = ( closeToPoint - RefToCurrentVertexPositionOfNodeNumber( i ) ).Length();
        if ( tClosestDistance > tDistance ) {
            tClosestDistance = tDistance;
            iClosestPointNumber = i;
        }
    }

    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() ) {
        return iClosestPointNumber;
    }
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    // Check with head needle's points
    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();
    Trx.Inversed();     // transformation of closeToPoint in world coordinates to IPs's local coordinates.
    std::vector< InteractionPoint<T> > & IPs = m_pHeadNeedle->RefToInteractionPointGroup().GetTheSetOfInteractionPoints();
    Vector3<T> closePt = (Trx * Vector4<T>( closeToPoint )).GetVector3();
    for ( int i = 0; i < GetTotalPoints() - GetNumberOfSutureThreadPoints(); ++i ) {
        tDistance = ( closePt - IPs[ i ].GetPosition() ).Length();
        if ( tClosestDistance > tDistance ) {
            tClosestDistance = tDistance;
            iClosestPointNumber = i + GetNumberOfSutureThreadPoints();
        }
    }

    // Returns -1 if could not find a closest point
    // The return values is in the range of [0,GetNumberOfSutureThreadPoints()) if the closest point is on the suture thread.
    // The return values is in the range of [GetNumberOfSutureThreadPoints(),GetTotalPoints()) if the closest point is on the head needle.
    return iClosestPointNumber;
}
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> ModelSurgicalSutureWithHeadNeedle<T>::GetPositionOfHeadNeedleSharpPoint () const
{
    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();

    return Trx * m_pNeedleSharpPt->GetPosition();
}
//-----------------------------------------------------------------------------
template <typename T>
Vector3<T> ModelSurgicalSutureWithHeadNeedle<T>::GetPositionOfPointNumberIonHeadNeedle ( unsigned int i ) const
{
    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();

    return (i < GetHeadNeedle().RefToInteractionPointGroup().GetNumOfInteractionPoints() ? Trx * GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex(i).GetPosition() : CGMath<T>::ZeroVector);
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::GetTwoPointsInWorldSpaceFormingLineExtendedFromHeadNeedleSharpPoint ( Vector3<T> & sharpPt, Vector3<T> & extendedPt ) const
{
    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();

    sharpPt = Trx * m_pNeedleSharpPt->GetPosition();
    extendedPt = Trx * m_ExtendedHeadNeedleSharpPtPos;
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::GetTwoInteractionPointPositionsInWorldSpace ( unsigned int i, unsigned int j, Vector3<T> & iPoint, Vector3<T> & jPoint ) const
{
    assert( i < m_pHeadNeedle->RefToInteractionPointGroup().GetNumOfInteractionPoints() );
    assert( j < m_pHeadNeedle->RefToInteractionPointGroup().GetNumOfInteractionPoints() );

    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();

    iPoint = Trx * m_pNeedleSharpPt->RefToInteractionPointGroup().GetInteractionPointAtIndex( i ).GetPosition();
    jPoint = Trx * m_pNeedleSharpPt->RefToInteractionPointGroup().GetInteractionPointAtIndex( j ).GetPosition();
}
//-----------------------------------------------------------------------------
template <typename T>
std::vector< Vector3<T> > ModelSurgicalSutureWithHeadNeedle<T>::GetAllInteractionPointPositionsInWorldSpace () const
{
    TransformationSupport<T> transform;
    Vector3<T> translation = GetHeadNeedle().GetOriginPoint();
    Matrix4x4<T> Trx( 1, 0, 0, translation[0], 0, 1, 0, translation[1], 0, 0, 1, translation[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= m_pHeadNeedle_asHalfEdgeModel->GetTransform().RefToMatrixTransform();

    std::vector< Vector3<T> > positions;
    for ( unsigned int i = 0; i < m_pHeadNeedle->RefToInteractionPointGroup().GetNumOfInteractionPoints(); ++i ) {
        positions.push_back( Trx * m_pHeadNeedle->RefToInteractionPointGroup().GetInteractionPointAtIndex(i).GetPosition() );
    }

    return positions;
}
//-----------------------------------------------------------------------------
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle<T>::PickAt (
    Vector3<T> const & closeToPoint,    
    T distance_limit,                   
    int & pickedID                      
)
{
    pickedID = FindTheClosestPointToPoint( closeToPoint, distance_limit );

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

    if ( pickedID < 0 ) return false;

    // DEBUG
    //pickedID = GetTotalPoints()-1;

    // Suture thread
    if ( pickedID < GetNumberOfSutureThreadPoints() ) {
        if ( m_Nodes[ pickedID ].UseEnforcedPosition )  return false;   // skip if the point is currently picked
        m_Nodes[ pickedID ].UseEnforcedPosition = true;
    }
    else /*if ( pickedID < GetTotalPoints() )*/ {
        if ( GetListOfNodes()[0].UseEnforcedPosition )  return false;   // skip if the point is currently picked
        GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( pickedID - GetNumberOfSutureThreadPoints() ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED );
        ++m_iHeadNeedleIPtSimFlagSetCount;
        GetHeadNeedle().SetPhyPropGravitationalConstant( 0 );

        // Set the constraint that the first point of suture thread must follow the head needle
        GetListOfNodes()[0].UseEnforcedPosition = true;
        GetListOfNodes()[0].EnforcedPosition = GetHeadNeedle().GetOriginPoint();
        GetListOfNodes()[0].UseEnforcedOrientation = true;
        GetListOfNodes()[0].EnforcedOrientation = GetHeadNeedle().GetOrientation();
    }
    return true;
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::PickPoint (
    int pickedID            
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfSutureThreadPoints() );

    

    // Suture thread
    if ( /*0 <= pickedID &&*/ pickedID < GetNumberOfSutureThreadPoints() ) {
        if ( m_Nodes[ pickedID ].UseEnforcedPosition )  return; // skip if the point is currently picked
        m_Nodes[ pickedID ].UseEnforcedPosition = true;
    }
    // Head needle
    else /*if ( pickedID < GetTotalPoints() )*/ {
        if ( GetListOfNodes()[0].UseEnforcedPosition )  return; // skip if the point is currently picked
        GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( pickedID - GetNumberOfSutureThreadPoints() ).GetSimFlags().SetSimulationConstraints( Enum::AddOn::ATTACHED );
        ++m_iHeadNeedleIPtSimFlagSetCount;
        GetHeadNeedle().SetPhyPropGravitationalConstant( 0 );

        // Set the constraint that the first point of suture thread must follow the head needle
        GetListOfNodes()[0].UseEnforcedPosition = true;
        GetListOfNodes()[0].EnforcedPosition = GetHeadNeedle().GetOriginPoint();
        GetListOfNodes()[0].UseEnforcedOrientation = true;
        GetListOfNodes()[0].EnforcedOrientation = GetHeadNeedle().GetOrientation();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::MovePickedPoint (
    int pickedID,           
    Vector3<T> & position   
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfSutureThreadPoints() );
    // Suture thread
    if ( /*0 <= pickedID &&*/ pickedID < GetNumberOfSutureThreadPoints() ) {
        //if ( m_Nodes[ pickedID ].UseEnforcedPosition ) {
            m_Nodes[ pickedID ].EnforcedPosition  = position;
        //}
    }
    // Head needle
    else /*if ( pickedID < GetTotalPoints() )*/ {
        int IPtID = pickedID - GetNumberOfSutureThreadPoints();     // interaction point's id
        GetHeadNeedle().SetOriginPoint(
            position - GetHeadNeedle().GetOrientation().GetRotationMatrix4x4() * 
            ( m_pHeadNeedle->RefToInteractionPointGroup().GetInteractionPointAtIndex( IPtID ).GetPosition() 
            - m_pNeedleSutureConnectingPt->GetPosition() )
        );
        // Move the first point of suture thread to the origin of the head needle
        GetListOfNodes()[0].EnforcedPosition = GetHeadNeedle().GetOriginPoint();
        GetListOfNodes()[0].EnforcedOrientation = GetHeadNeedle().GetOrientation();
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::UnpickPoint (
    int pickedID    
)
{
    assert( 0 <= pickedID && pickedID < GetNumberOfSutureThreadPoints() );
    // Suture thread
    if ( /*0 <= pickedID &&*/ pickedID < GetNumberOfSutureThreadPoints() ) {
        m_Nodes[ pickedID ].UseEnforcedPosition = false;
    }
    // Head needle
    else /*if ( pickedID < GetTotalPoints() )*/ {
        GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( pickedID - GetNumberOfSutureThreadPoints() ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED );
        if ( m_iHeadNeedleIPtSimFlagSetCount > 0 ) {
            --m_iHeadNeedleIPtSimFlagSetCount;
        }

        // Remove the constraint that the first point of suture thread must follow the head needle
        if ( m_iHeadNeedleIPtSimFlagSetCount == 0 ) {
            GetListOfNodes()[0].UseEnforcedPosition = false;
            GetListOfNodes()[0].UseEnforcedOrientation = false;
            GetHeadNeedle().SetPhyPropGravitationalConstant( -GetGravitationalConstant() );
        }
    }
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::UnpickAllPoints ()
{
    std::cout << "UnpickAllPoints\n";

    // Suture thread
    int i = 0;
    for ( ; i < GetNumberOfSutureThreadPoints(); ++i ) {
        m_Nodes[ i ].UseEnforcedPosition = false;
    }
    // Head needle
    int p = 0;
    for ( ; i < GetTotalPoints(); ++i, ++p ) {
        GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( p ).GetSimFlags().ClearSimulationConstraints( Enum::AddOn::ATTACHED );
    }

    // Remove the constraint that the first point of suture thread must follow the head needle
    m_iHeadNeedleIPtSimFlagSetCount = 0;
    GetListOfNodes()[0].UseEnforcedPosition = false;
    GetListOfNodes()[0].UseEnforcedOrientation = false;
    GetHeadNeedle().SetPhyPropGravitationalConstant( -GetGravitationalConstant() );
}
//-----------------------------------------------------------------------------
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle<T>::IsPointID_SutureThread ( int id ) const
{
    if ( 0 <= id && id <= GetNumberOfSutureThreadLinks() )  return true;
    return false;
}
//-----------------------------------------------------------------------------
template <typename T>
bool ModelSurgicalSutureWithHeadNeedle<T>::IsPointID_HeadNeedle ( int id ) const
{
    if ( GetNumberOfSutureThreadLinks() < id && id < m_iTotalPoints )   return true;
    return false;
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::ClearAllExternalForcesAndTorques ()
{
    // Clear suture thread's external forces
    ModelSurgicalSutureThread<T>::ClearExternalForces();
    // Clear head needle external forces and torques
    GetHeadNeedle().ClearAllForcesAndTorques();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::SetHeadNeedlePosition ( Vector3<T> const & position )
{
    GetHeadNeedle().SetOriginPoint( position );
    SetSutureThreadOrienAndPosToFollowHeadNeedle();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::SetHeadNeedleOrientation ( Quaternion<T> const & orientation )
{
    // Set rotation center of the head needle
    GetHeadNeedle().SetOrientationCenter( CGMath<T>::ZeroVector );
    //Quaternion<T> ori( orientation );
    //ori.Normalized();
    GetHeadNeedle().SetOrientation( orientation );
    SetSutureThreadOrienAndPosToFollowHeadNeedle();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::InitHeadNeedleCirclePath ()
{
    //Matrix4x4<T> Trx;
    //Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    //Trx *= GetHeadNeedle().GetPtrToModel()->GetTransform().RefToMatrixTransform();

    int numOfIPs = GetHeadNeedle().RefToInteractionPointGroup().GetNumOfInteractionPoints();
    m_HeadNeedleCirclePath.SharpPt = GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( 0 ).GetPosition();
    m_HeadNeedleCirclePath.EndPt = GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( numOfIPs-1 ).GetPosition();
    m_HeadNeedleCirclePath.SharpPt.SetX( 0 );   // assume the head needle lies in a plane parallel to the local coordinate system where x-coordinate is zero

    //
    // Initialize the path circle
    //
    // Assume:
    //   The end point is at <0,0,0>.
    //   The head needle's shape traces a circle path.
    //   The head needle lies flat on the yz-plane.
    //   The normal plane of the head needle's path circle is <1,0,0>.
    // [after an algebra calculation]
    T k = m_HeadNeedleCirclePath.SharpPt.SquaredLength() / m_HeadNeedleCirclePath.SharpPt[1] * T(0.5);
    m_HeadNeedleCirclePath.Center.SetXYZ( 0, k, 0 );
    m_HeadNeedleCirclePath.Radius = k;  // Remark: k is negative
    m_HeadNeedleCirclePath.NormalPlane.SetXYZ( 1, 0, 0 );

    //
    // Find the cover angle
    m_HeadNeedleCirclePath.CoverAngle = Math<T>::ASin( ( m_HeadNeedleCirclePath.SharpPt.Length() / m_HeadNeedleCirclePath.Radius )*T(0.5) ) * T(2);
    std::cout << "m_HeadNeedleCirclePath.CoverAngle (radians): " << m_HeadNeedleCirclePath.CoverAngle << "\n";
    std::cout << "m_HeadNeedleCirclePath.CoverAngle (degrees): " << m_HeadNeedleCirclePath.CoverAngle * Math<T>::RAD_TO_DEG << "\n";

    // Suture thread' link length conversion to angle
    m_HeadNeedleCirclePath.LinkLenAngle = Math<T>::ASin( ( GetLinkLength() / m_HeadNeedleCirclePath.Radius )*T(0.5) ) * T(2);
    std::cout << "m_HeadNeedleCirclePath.LinkLenAngle (radians): " << m_HeadNeedleCirclePath.LinkLenAngle << "\n";
    std::cout << "m_HeadNeedleCirclePath.LinkLenAngle (degrees): " << m_HeadNeedleCirclePath.LinkLenAngle * Math<T>::RAD_TO_DEG << "\n";
    
    //
    // Interpolate points
    //
    Vector3<T> pos;
    T y, z;
    int i = 0;
    //for ( T a = 0; a > -2*Math<T>::PI; a+=m_HeadNeedleCirclePath.LinkLenAngle ) { // sutureLinkLenAngle is negative
    T angle = 0;
    bool isSet = false;
    for ( T a = Math<T>::PI; a > -Math<T>::PI; a+=m_HeadNeedleCirclePath.LinkLenAngle ) {   // sutureLinkLenAngle is negative
        y = cos( a );
        z = sin( a );
        pos = m_HeadNeedleCirclePath.Center + Vector3<T>( 0, y, z )*m_HeadNeedleCirclePath.Radius;
        m_HeadNeedleCirclePath.InterpolatedPts.push_back( pos );

        if ( !isSet && angle < m_HeadNeedleCirclePath.CoverAngle ) {
            m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt = i-1;
            isSet = true;
        }

        ++i;
        angle += m_HeadNeedleCirclePath.LinkLenAngle;
    }

    m_HeadNeedleCirclePath.Radius *= -1;    // change to positive
    //m_HeadNeedleCirclePath.pCDSphereNode = new BVHNodeLeaf<T>( Enum::BVH_NODE_LEAF_SPHERE );
    //m_HeadNeedleCirclePath.pCDSphereNode->SetRadius( m_HeadNeedleCirclePath.Radius );

    std::cout << "Number of interpolated points: " << m_HeadNeedleCirclePath.InterpolatedPts.size() << "\n";
    std::cout << "ID of the first interpolated points after the sharp point: " << m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt << "\n";
    std::cout << "m_HeadNeedleCirclePath.Radius: " << m_HeadNeedleCirclePath.Radius << "\n";
}
//-----------------------------------------------------------------------------
//=============================================================================




//=============================================================================
// OpenGL
#if defined(__gl_h_) || defined(__GL_H__)
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::Draw ()
{
    #if defined(DEBUG) || defined(_DEBUG)
    if ( m_pHeadNeedle ) {
    #endif
        #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
        if ( !IsHeadNeedleDisabled() ) {
        #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

            m_pHeadNeedle->Draw();

            //DrawClue_GrabbingLocation();
            //DrawClue_CirclePath();
        #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
        }
        #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    #if defined(DEBUG) || defined(_DEBUG)
    }
    #endif

    ModelSurgicalSutureThread<T>::Draw();   // draw the suture's thread
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::DrawOnlyHeadNeedle ()
{
    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() ) { return; }
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    #if defined(DEBUG) || defined(_DEBUG)
    if ( m_pHeadNeedle ) {
    #endif
        m_pHeadNeedle->Draw();
    #if defined(DEBUG) || defined(_DEBUG)
    }
    #endif
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::DrawOnlySutureThread ()
{
    ModelSurgicalSutureThread<T>::Draw();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::DrawForDebugging ()
{
    #if defined(DEBUG) || defined(_DEBUG)
    if ( m_pHeadNeedle ) {
    #endif
        // Drawing for DEBUG
        m_pHeadNeedle->DrawForDebugging();
        //m_pHeadNeedle->DrawInteractionPointGroup();
        //m_pHeadNeedle->DrawCenterOfMass();
        //m_pHeadNeedle->DrawOrigin();
        //m_pHeadNeedle->DrawOrientation();

        //m_CDUnit_HeadNeedle.Draw();   // from interaction point group

    #if defined(DEBUG) || defined(_DEBUG)
    }
    #endif

    ModelSurgicalSutureThread<T>::DrawForDebugging();   // draw the suture's thread
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::DrawClue_GrabbingLocation ()
{
    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() ) return;
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    glPushAttrib( GL_ALL_ATTRIB_BITS );
    glEnable( GL_COLOR_MATERIAL );
    glEnable( GL_BLEND );
    Vector4<T> color( 1, 1, 0.5f, 0.25f );
    //glColor4f( 1, 1, 0.5, 0.25 );

    // Total IP points
    int n = GetHeadNeedle().RefToInteractionPointGroup().GetNumOfInteractionPoints();
    T twoThird = n * T(2) / T(3);   // twoThird is roughly at the 2/3 position from the head needle's sharp point
    n = floor( twoThird );
    int m = ceil( twoThird );

    Matrix4x4<T> Trx( 1, 0, 0, GetHeadNeedle().GetOriginPoint()[0], 0, 1, 0, GetHeadNeedle().GetOriginPoint()[1], 0, 0, 1, GetHeadNeedle().GetOriginPoint()[2], 0, 0, 0, 1 );
    Trx *= GetHeadNeedle().GetOrientation().GetRotationMatrix4x4();
    Trx *= GetHeadNeedle().GetPtrToModel()->GetTransform().RefToMatrixTransform();

    Vector3<T> N = Trx * GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( n ).GetPosition();
    Vector3<T> M = Trx * GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( m ).GetPosition();
    Vector3<T> MnN = M-N;
    Vector3<T> MpN = M+N;

    BoundingCylinder<T> bv;
    bv.SetRadius( GetHeadNeedle().RefToInteractionPointGroup().GetInteractionPointAtIndex( n ).GetRadius() * T(1.5) );
    bv.SetHeight( MnN.Length() );
    //bv.SetCenter( MpN*T(0.5) );
    Trx = Matrix4x4<T>( CGMath<T>::CreateRotationMatrix3x3FromVectorAtoVectorB( Vector3<T>(0,0,1), MnN ) );
    Trx = CGMath<T>::MatrixTranslation( MpN*T(0.5) ) * Trx;
    bv.GetTransform().SetMatrixTransform( Trx );

    //bv.Draw( GLU_POINT );
    //bv.Draw( GLU_LINE );  // default

    bv.Draw( GLU_FILL, color );
    //bv.Draw( GLU_SILHOUETTE );

    glPopAttrib();
}
//-----------------------------------------------------------------------------
template <typename T>
void ModelSurgicalSutureWithHeadNeedle<T>::DrawClue_CirclePath ()
{
    #ifdef  TAPs_ADD_MODEL_DISABLE_OPTIONS
    if ( IsHeadNeedleDisabled() ) return;
    #endif//TAPs_ADD_MODEL_DISABLE_OPTIONS

    glPushAttrib( GL_ALL_ATTRIB_BITS );

    glDisable( GL_LIGHTING );
    glColor3f( 0.3, 0.6, 0.9 );

    /*
    // Draw collided nodes
    glPushMatrix();
    m_HeadNeedleCirclePath.Trx.TransformByOpenGLForDrawing();
    for ( std::vector< BVHNode<T> * >::iterator it = m_HeadNeedleCirclePath.pListOfCDNodes.begin(); it != m_HeadNeedleCirclePath.pListOfCDNodes.end(); ++it ) {
        (*it)->Draw();
    }
    glPopMatrix();
    //*/

    Matrix4x4<T> Trx = GetHeadNeedle().CalMatrixTransform();

    /*
    // Draw main points
    Vector3<T> C = Trx * m_HeadNeedleCirclePath.Center;
    Vector3<T> S = Trx * m_HeadNeedleCirclePath.SharpPt;
    Vector3<T> E = Trx * m_HeadNeedleCirclePath.EndPt;
    Vector3<T> N = (Trx * (m_HeadNeedleCirclePath.NormalPlane + m_HeadNeedleCirclePath.Center)) - C;// * m_HeadNeedleCirclePath.Radius;
    //Vector3<T> D = Trx * (m_HeadNeedleCirclePath.Center*2);
    glPointSize( 2 );
    glBegin( GL_POINTS );
    //glVertex3fv( C.GetDataFloat() );
    glVertex3fv( S.GetDataFloat() );
    glVertex3fv( E.GetDataFloat() );
    glVertex3fv( N.GetDataFloat() );
    //glVertex3fv( D.GetDataFloat() );
    glEnd();
    //*/

    //*
    // Draw interpolated points
    glPointSize( 2 );

    //glDisable( GL_DEPTH_TEST );
    glBegin( GL_POINTS );
    T s = 0;
    //T as = 1.0 / m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt;
    T as = 1.0 / m_HeadNeedleCirclePath.InterpolatedPts.size();
    std::vector< Vector3<T> >::iterator it = m_HeadNeedleCirclePath.InterpolatedPts.begin();
    while ( it != m_HeadNeedleCirclePath.InterpolatedPts.end() ) {
        glColor3f( s, s, s );
        glVertex3fv( (Trx*(*it)).GetDataFloat() );
        ++it;
        s += as;
    }
    glColor3f( 1, 0, 0 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt-1])).GetDataFloat() );
    glColor3f( 0, 1, 0 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt])).GetDataFloat() );
    glEnd();

    /*
    Matrix4x4<T> Translation = CGMath<T>::MatrixTranslation( -C );
    Matrix4x4<T> Mat = CGMath<T>::CreateRotationMatrix4x4FromVectorAtoVectorB( N, CGMath<T>::VectorZ );
    Matrix4x4<T> TrxO = Mat * Translation;
    Vector3<T> O = TrxO * C;
    Vector3<T> P = Mat * N;

    // Draw normal plane
    glBegin( GL_LINES );
    glVertex3fv( (C-C).GetDataFloat() );
    glVertex3fv( N.GetDataFloat() );
    glVertex3fv( (O).GetDataFloat() );
    glVertex3fv( P.GetDataFloat() );
    glEnd();
    //*/

    /*
    glPointSize( 9 );
    glDisable( GL_LIGHTING );
    glDisable( GL_DEPTH_TEST );
    glBegin( GL_POINTS );
    glColor3f( 1, 0, 0 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[0])).GetDataFloat() );
    glColor3f( 0, 1, 0 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[1])).GetDataFloat() );
    glColor3f( 0, 0, 1 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[2])).GetDataFloat() );
    glColor3f( 1, 1, 0 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[16])).GetDataFloat() );
    glColor3f( 0, 1, 1 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[17])).GetDataFloat() );
    glColor3f( 1, 0, 1 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[18])).GetDataFloat() );
    glColor3f( 0.5, 0.5, 0.5 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt-1])).GetDataFloat() );
    glColor3f( 1, 1, 1 );
    glVertex3fv( (Trx*(m_HeadNeedleCirclePath.InterpolatedPts[m_HeadNeedleCirclePath.IDOfFirstInterpolatedPtBeforeSharpPt])).GetDataFloat() );
    glEnd();
    //*/

    //m_HeadNeedleCirclePath.pCDSphereNode->Draw();

    /*
    // Draw as BV
    Matrix4x4<T> Trx2 = Matrix4x4<T>( CGMath<T>::CreateRotationMatrix3x3( 0,1,0, 90 ) );
    Trx2 = GetHeadNeedle().GetOrientation().GetRotationMatrix4x4() * Trx2;
    Trx2 = CGMath<T>::MatrixTranslation( GetHeadNeedle().GetOriginPoint() + GetHeadNeedle().GetOrientation().GetRotationMatrix4x4() *m_HeadNeedleCirclePath.Center ) * Trx2;
    BoundingCylinder<T> bv;
    bv.SetRadius( m_HeadNeedleCirclePath.Radius );
    bv.SetHeight( T(0.001) );
    bv.GetTransform().SetMatrixTransform( Trx2 );
    //bv.Draw( GLU_SILHOUETTE );
    //bv.Draw( GLU_FILL );
    bv.Draw( GLU_LINE );
    //*/

    glPopAttrib();
}
//-----------------------------------------------------------------------------
#endif // OpenGL
//=============================================================================
END_NAMESPACE_TAPs
//34567890123456789012345678901234567890123456789012345678901234567890123456789
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----