TAPsModelSutureMath.cpp

Go to the documentation of this file.

/*******************************************************************************
TAPsModelSutureMath.cpp

A two-headed needle suture

SUKITTI PUNAK   (01/30/2006)
UPDATE          (09/03/2010)
*******************************************************************************/
#include "TAPsModelSutureMath.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__OpenGL
//==============================================================================
//------------------------------------------------------------------------------
// Default constructor
template <typename T>
ModelSutureMath<T>::ModelSutureMath ()
    : ModelStrandMath<T>( 128, 0.1, 0.1, Vector3<T>(), 0.05 ), 
      m_iLevel( 3 )
{
    InitNeedles();
    //----------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "Default ModelSutureMath<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//------------------------------------------------------------------------------
// Constructor
template <typename T>
ModelSutureMath<T>::ModelSutureMath (   int iNoLinks, T tLength, T tWeight, 
                                Vector3<T> & posOfVertex0, T tRadius )

    : ModelStrandMath<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius ), 
      m_iLevel( 3 )
{
    InitNeedles();
    //----------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "ModelSutureMath<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//------------------------------------------------------------------------------
// Constructor
template <typename T>
ModelSutureMath<T>::ModelSutureMath (   int iNoLinks, T tLength, T tWeight, 
                                Vector3<T> & posOfVertex0, T tRadius, 
                                const char * needle1 
                            )

    : ModelStrandMath<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius ), 
      m_iLevel( 3 )
{
    InitNeedles( needle1 );
    //----------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "ModelSutureMath<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//------------------------------------------------------------------------------
// Constructor
template <typename T>
ModelSutureMath<T>::ModelSutureMath (   int iNoLinks, T tLength, T tWeight, 
                                Vector3<T> & posOfVertex0, T tRadius, 
                                T tKRatioOfStretchingAllowed,
                                T tKRatioOfCompressionAllowed,
                                T tKStretching,
                                T tKBending,
                                T tKTwisting,
                                T tKFriction,
                                T tKContact,
                                T tKGravity,
                                const char * needle1 
                            )

    : ModelStrandMath<T>( iNoLinks, tLength, tWeight, posOfVertex0, tRadius, 
            tKRatioOfStretchingAllowed, tKRatioOfCompressionAllowed, 
            tKStretching, tKBending, tKTwisting, 
            tKFriction, tKContact, tKGravity ), 
      m_iLevel( 3 )
{
    InitNeedles( needle1 );
    //----------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout   << "ModelSutureMath<" << typeid(T).name() << "> with:\n"
                << "  " << m_iNoLinks << " links with each link of length "
                << m_tLinkLength << " and weight " << m_tPointWeight << "\n";
    #endif//TAPs_DEBUG_MODE
}
//------------------------------------------------------------------------------
// Destructor
template <typename T>
ModelSutureMath<T>::~ModelSutureMath ()
{
    //if ( m_pNeedleLeft )  delete m_pNeedleLeft;       // ERROR Deleting Object!  FIX IT!
    //if ( m_pNeedleRight ) delete m_pNeedleRight;
    //----------------------------------------------------------------
    if ( m_prPickPts )  delete [] m_prPickPts;
    if ( m_prbIsSharp ) delete [] m_prbIsSharp;
    //----------------------------------------------------------------
    #ifdef  TAPs_ENABLE_DEBUG
    std::cout << "ModelSutureMath<" << typeid(T).name() << "> Destructor\n";
    #endif//TAPs_DEBUG_MODE
}
//------------------------------------------------------------------------------
// Initialize Needles
template <typename T>
void ModelSutureMath<T>::InitNeedles ()
{
    //char fileName1[] = "Data/3dsMax/HeadNeedle.ASE";
    //char fileName1[] = "Data/3dsMax/HeadNeedle2.ASE";
    char needle1[] = "Data/3dsMax/HeadNeedle3.ASE";
    //char fileName2[] = "Data/3dsMax/HeadNeedle.ASE";
    InitNeedles( needle1 );
}
//------------------------------------------------------------------------------
// Initialize Needles
template <typename T>
void ModelSutureMath<T>::InitNeedles ( const char * needle1 )
{
    //m_pNeedleLeft  = NULL;
    //m_pNeedleRight = NULL;
    //----------------------------------------------------------------
    m_pNeedleLeft  = new OpenGLHalfEdgeModel<T>();
    //m_pNeedleRight = new OpenGLHalfEdgeModel<T>();
    //----------------------------------------------------------------

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Create Left Needle" << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( !ReadModels<Real>::readFile( needle1, m_pNeedleLeft ) ) {
        std::cout << "ERROR: In File Reading for creating the left needle!" << std::endl;
        exit ( EXIT_FAILURE );
    }
    else {
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "Creating Left Needle" << std::endl;
        #endif//TAPs_DEBUG_MODE

        m_pNeedleLeft->SetMaterial( OpenGL::Enum::HEAD_NEEDLE );
        m_pNeedleLeft->BuildBVHTree( TAPs::Enum::BVH_TREE_BINARY_SPHERE );
    }
    //----------------------------------------------------------------
    /*

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Create Right Needle" << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( !ReadModels<Real>::readFile( needle2, m_pNeedleRight ) ) {
        std::cout << "ERROR: In File Reading for creating the right needle!" << std::endl;
        exit ( EXIT_FAILURE );
    }
    else {
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "Creating Right Needle" << std::endl;
        #endif//TAPs_DEBUG_MODE

        m_pNeedleRight->SetMaterial( OpenGL::Enum::HEAD_NEEDLE );
        m_pNeedleRight->BuildBVHTree( TAPs::Enum::BVH_TREE_BINARY_SPHERE );
    }
    //*/
    //--------------------------------------------------------------------
    InitNeedlesPickPts();
    SetNeedlesPickPts();
    InitNeedlesSharpPts();
}
//------------------------------------------------------------------------------
// Initialize Needles Pick Points
template <typename T>
void ModelSutureMath<T>::InitNeedlesPickPts ()
{
    // SET Number of Pick Pts HERE! ==> 2^m_iLevel
    m_iLevel = 4;
    //----------------------------------------------------------------
    // Set number of pick able points for needles
    //m_iNoPickPts = static_cast<int>( pow( 2.0, m_iLevel ) ); // * 2;
    m_iNoPickPts = 1 << m_iLevel;

    #ifdef  TAPs_DEBUG_MODE
    std::cout << "Number of pickable points for the needle is set at" << m_iNoPickPts << ".\n";
    #endif//TAPs_DEBUG_MODE

    m_prPickPts = new Vector3<T>*[ m_iNoPickPts ];
    assert( m_prPickPts );
    //----------------------------------------------------------------
    // Use a recursive function
    BVHNode<T> ** L = new BVHNode<T>*[1];
    L[0] = m_pNeedleLeft->GetBVHTree()->Root();
    InitNeedlesPickPts( L, 0 );     // Node List and Level Number
    delete [] L;
}
//------------------------------------------------------------------------------
// Initialize Needles Pick Points
template <typename T>
void ModelSutureMath<T>::InitNeedlesPickPts ( BVHNode<T> ** node, int level )
{
    #ifdef  TAPs_DEBUG_MODE
    std::cout << "InitNeedlesPickPts with LEVEL of " << level << std::endl;
    #endif//TAPs_DEBUG_MODE

    if ( level > m_iLevel ) return;
    //----------------------------------------------------------------
    //int size = static_cast<int>( pow( 2.0, level ) );
    int size = 1 << level;
    BVHNode<T> ** L = new BVHNode<T>*[ size*2 ];
    for ( int i = 0, j = 0; i < size; ++i, j+=2 ) {
        L[j  ] = node[i]->Child(0);
        L[j+1] = node[i]->Child(1);
    }
    InitNeedlesPickPts( L, ++level );
    //----------------------------------------------------------------
    if ( level == m_iLevel ) {
        //----------------------------------------------------------------
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "--------------------   NEEDLE PICK POINTS   --------------------\n";
        #endif//TAPs_DEBUG_MODE

        for ( int i = 0; i < m_iNoPickPts; ++i ) {
            m_prPickPts[i] = &(L[i]->GetCenter());

            #ifdef  TAPs_DEBUG_MODE
            std::cout << *(m_prPickPts[i]) << "\n";
            #endif//TAPs_DEBUG_MODE
        }
        #ifdef  TAPs_DEBUG_MODE
        std::cout << "-------------------- END NEEDLE PICK POINTS --------------------\n";
        #endif//TAPs_DEBUG_MODE
    }
    delete [] L;
}
//------------------------------------------------------------------------------
// SetNeedlesPickPts
//   Align needles with the movement of the strand
template <typename T>
void ModelSutureMath<T>::SetNeedlesPickPts ()
{
    static int i;
    static T angle;
    static Vector3<T> dir, axis;

    //--------------------------------------------------------------------
    // Left Needle
    i = 0;
    dir = m_prVertex[i] - m_prVertex[i+1];
    axis = dir ^ (-CGMath<T>::VectorY);
    angle = acos( (dir * (-CGMath<T>::VectorY)) / dir.Length() ) 
          * Math<T>::RAD_TO_DEG;
    // Translation and Rotation
    //m_vTransForLeftNeedle  = m_prVertex[i];
    m_pNeedleLeft->GetTransform().SetTranslation( m_prVertex[i] );
    //m_mRotMatForLeftNeedle = CGMath<T>::CreateRotationMatrix3x3( axis, -angle );
    if ( !GetFixStatusOfPtNo( 0 ) )
        m_pNeedleLeft->GetTransform().SetMatrixRotation( CGMath<T>::CreateRotationMatrix3x3( axis, -angle ) );
    //std::cout << m_pNeedleLeft->GetTransform().GetMatrixRotation() << "\n";
    /*
    //--------------------------------------------------------------------
    // Get Rotation Matrix using OpenGL
    GLfloat m[16];
    glPushMatrix();
        glLoadIdentity();
        glRotatef( -angle, axis[0], axis[1], axis[2] );
        glGetFloatv( GL_MODELVIEW_MATRIX, m );
    glPopMatrix();
    m_vTransForLeftNeedle  = m_prVertex[i];
    m_mRotMatForLeftNeedle = Matrix3x3<T>(  m[ 0], m[ 4], m[ 8],
                                            m[ 1], m[ 5], m[ 9],
                                            m[ 2], m[ 6], m[10] );
    //*/

    //--------------------------------------------------------------------
    //--------------------------------------------------------------------
    //--------------------------------------------------------------------
    //--------------------------------------------------------------------
}
//------------------------------------------------------------------------------
// InitNeedlesSharpPts
template <typename T>
void ModelSutureMath<T>::InitNeedlesSharpPts ()
{
    //---------------------------------------------------------------
    // For Left Needle
    m_prbIsSharp = new bool[ m_iNoPickPts ];
    for ( int i = 0; i < m_iNoPickPts; ++i ) {
        m_prbIsSharp[i] = false;
    }

    /*
    //---------------------------------------------------------------
    // Sort Needle Points
    int * sortArray = new int[ m_iNoPickPts ];
    for ( int i = 0; i < m_iNoPickPts; ++i ) {
        sortArray[i] = i;
    }
    T minDistance = (*(m_prPickPts[0]) - m_prVertex[0]).Length();
    T distance;
    int minPt;
    //-----------------------------------------------------
    for ( int j = 0; j < m_iNoPickPts; ++j ) {
        distance = (*(m_prPickPts[j]) - m_prVertex[0]).Length();
        if ( distance < minDistance ) {
            minPt = j;
            minDistance = distance;
        }
    }
    sortArray[0]     = minPt;
    sortArray[minPt] = 0;
    /*
    //-----------------------------------------------------
    for ( int i = 1; i < m_iNoPickPts; ++i ) {
        minDistance = (*(m_prPickPts[sortArray[i]]) - *(m_prPickPts[sortArray[i-1]])).Length();
        for ( int j = i+1; j < m_iNoPickPts; ++j ) {
            distance = (*(m_prPickPts[sortArray[j]]) - *(m_prPickPts[sortArray[i-1]])).Length();
            if ( distance < minDistance ) {
                minPt = j;
                minDistance = distance;
            }
        }
        int tmp = sortArray[i];
        sortArray[i]     = sortArray[minPt];
        sortArray[minPt] = tmp;
    }
    //*/
    //-----------------------------------------------------
    //m_prbIsSharp[sortArray[m_iNoPickPts-1]] = true;
    //m_prbIsSharp[sortArray[0]] = true;
    //*/

    //---------------------------------------------------------------
    int needleSharpPt = 0;
    T maxDistance = (*(m_prPickPts[0]) - m_prVertex[0]).Length();
    T distance;
    for ( int i = 1; i < m_iNoPickPts; ++i ) {
        distance = (*(m_prPickPts[i]) - m_prVertex[0]).Length();
        if ( distance > maxDistance ) {
            needleSharpPt = i;
            maxDistance = distance;
        }
    }
    m_prbIsSharp[needleSharpPt] = true;
    m_iNeedleSharpPt = needleSharpPt;
}
//------------------------------------------------------------------------------
template <typename T>
inline Vector3<T> ModelSutureMath<T>::GetPointPosition ( int i ) const
{
    if ( i <= m_iNoLinks )  return m_prVertex[i];
    else                    return GetNeedlesPosition( i-m_iNoLinks-1 );
}
//------------------------------------------------------------------------------
template <typename T>
inline Vector3<T> ModelSutureMath<T>::GetNeedlesPosition ( int i ) const
{
    assert( 0 <= i && i < m_iNoPickPts );
    // Left Needle
    //return m_mRotMatForLeftNeedle * *(m_prPickPts[i]) + m_vTransForLeftNeedle;
    return m_pNeedleLeft->GetTransform().GetMatrixRotation() * 
                *(m_prPickPts[i]) + m_pNeedleLeft->GetTransform().GetTranslation();
}
//------------------------------------------------------------------------------
template <typename T>
inline void ModelSutureMath<T>::SetPointPosition ( 
    int i, const Vector3<T> & position )
{
    if ( i <= m_iNoLinks )  {
        ModelStrandMath<T>::SetPointPosition( i, position );
        SetNeedlesPickPts();
    }
    else {
        //SetNeedlesPosition( i-m_iNoLinks-1, position, m_mRotMatForLeftNeedle );

        SetNeedlesPosition( i-m_iNoLinks-1, position, m_pNeedleLeft->GetTransform().GetMatrixRotation() );

        //ModelStrandMath<T>::SetPointPosition( 0, m_vTransForLeftNeedle );

        //ModelStrandMath<T>::SetPointPosition( 1, position );
        ModelStrandMath<T>::SetPointPosition( 0, m_pNeedleLeft->GetTransform().GetTranslation() );
    }
}
//------------------------------------------------------------------------------
template <typename T>
inline void ModelSutureMath<T>::SetPointPosition ( 
    int i, const Vector3<T> & position, Matrix3x3<T> & rotation, 
    //T tMoveDistanceLimit, 
    MultiBoundingVolume<T> const * const pMBV, 
    TransformationSupport<T> const * const pTransform 
)
{
    if ( i <= m_iNoLinks )  {
        ModelStrandMath<T>::SetPointPosition( i, position, pMBV, pTransform );
        SetNeedlesPickPts();
    }
    else {
        SetNeedlesPosition( i-m_iNoLinks-1, position, rotation );
        //ModelStrandMath<T>::SetPointPosition( 0, m_vTransForLeftNeedle );
        ModelStrandMath<T>::SetPointPosition( 0, m_pNeedleLeft->GetTransform().GetTranslation() );
    }
}
//------------------------------------------------------------------------------
template <typename T>
inline void ModelSutureMath<T>::SetNeedlesPosition ( 
    int i, const Vector3<T> & position, Matrix3x3<T> & rotation )
{
    assert( 0 <= i && i < m_iNoPickPts );
    //--------------------------------------------------------------------
    // Left Needle
    //m_mRotMatForLeftNeedle =  rotation;

    //rotation.MakeIdentity();

    // Rotate with the first link of the strand
    m_pNeedleLeft->GetTransform().SetMatrixRotation( rotation );

    Vector3<T> translation = position - GetNeedlesPosition( i );
    //m_vTransForLeftNeedle += translation;
    m_pNeedleLeft->GetTransform().SetTranslation( 
        m_pNeedleLeft->GetTransform().GetTranslation() + translation
    );
}
//------------------------------------------------------------------------------
//==============================================================================
// Get/Set Fn(s)
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//==============================================================================
// Helper Fn(s)
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//==============================================================================
// For Simulation
//------------------------------------------------------------------------------
// DxDt
template <typename T>
bool ModelSutureMath<T>::DxDt ( 
    T dt,                           // i/p: time step
    Simulation::VectorSet<T> &x,    // i/p: array  x = {pos, vel}
    Simulation::VectorSet<T> &xdot, // o/p: array xdot = {vel, accel}
    void *userData                  // o/p: array of user data
)
{
    return ModelStrandMath<T>::DxDt( dt, x, xdot, userData );
}
//------------------------------------------------------------------------------
// StateToArray
template <typename T>
void ModelSutureMath<T>::StateToArray ( T *dst )
{
    ModelStrandMath<T>::StateToArray( dst );
}
//------------------------------------------------------------------------------
// ArrayToState
template <typename T>
void ModelSutureMath<T>::ArrayToState ( T *src )
{
    ModelStrandMath<T>::ArrayToState( src );
}
//------------------------------------------------------------------------------
// DdtStateToArray
template <typename T>
void ModelSutureMath<T>::DdtStateToArray ( T *xdot, T dt )
{
    ModelStrandMath<T>::DdtStateToArray( xdot, dt );
}
//------------------------------------------------------------------------------
// AdvanceSimulation
template <typename T>
void ModelSutureMath<T>::AdvanceSimulation ( T tCurrent, T tNext )
{
    ModelStrandMath<T>::AdvanceSimulation( tCurrent, tNext );
    SetNeedlesPickPts();
}
//------------------------------------------------------------------------------
// AdvanceSimulation
template <typename T>
void ModelSutureMath<T>::AdvanceSimulation ( Simulation::SimClock<T> & simClock )
{
    ModelStrandMath<T>::AdvanceSimulation( simClock );
    SetNeedlesPickPts();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// SetupShape Fn
template <typename T>
void ModelSutureMath<T>::SetupShape ( Vector3<T> startPosition )
{
    ModelStrandMath<T>::SetupShape( startPosition );
}
//------------------------------------------------------------------------------
//==============================================================================
END_NAMESPACE_TAPs__OpenGL
//------------------------------------------------------------------------------
//345678901234567890123456789012345678901234567890123456789012345678901234567890
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8