TAPsHapticManager.cpp

Go to the documentation of this file.

/******************************************************************************
TAPsHapticManager.cpp

Inherited from class BasePointManager

SUKITTI PUNAK   (05/07/2006)
UPDATE          (09/03/2010)
******************************************************************************/
#include "TAPsHapticManager.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.

//-----------------------------------------------------------------------------
// DEBUG ENABLE
#ifdef TAPs_ENABLE_DEBUG
    #define DEBUG_MESSAGE_TAPs_HAPTIC_MANAGER_HPP
#endif
//-----------------------------------------------------------------------------

BEGIN_NAMESPACE_TAPs__Haptic
//=============================================================================
//-----------------------------------------------------------------------------
template <typename T> const T HapticManager<T>::g_tSnapDistanceConst = 2.0;
template <typename T> T HapticManager<T>::g_tSnapDistance = g_tSnapDistanceConst;
//=============================================================================
//-----------------------------------------------------------------------------
// Default Constructor
template <typename T>
HapticManager<T>::HapticManager ()
    : BaseHapticManager<T>(),
      m_pModelManager( NULL ),
      m_pViewManager( NULL ),
      m_iManipPoint( -1 ), 
      m_uiPointNameBase( 0 ), 
      m_uiEffectName( 0 ),
      m_tCursorScale( 1.0 ),
      m_uiCursorDisplayList( 0 ),
      m_hHD( HD_INVALID_HANDLE ),
      m_hHLRC( NULL )
{
    m_uiHapticObjects = 0;
    InitHapticData();
    InitCursor();
}
//-----------------------------------------------------------------------------
// Constructor
template <typename T>
HapticManager<T>::HapticManager ( const char * toolOneFileName )
    : BaseHapticManager<T>(),
      m_pModelManager( NULL ),
      m_pViewManager( NULL ),
      m_iManipPoint( -1 ), 
      m_uiPointNameBase( 0 ),
      m_uiEffectName( 0 ),
      m_tCursorScale( 1.0 ),
      m_uiCursorDisplayList( 0 ),
      m_hHD( HD_INVALID_HANDLE ),
      m_hHLRC( NULL )
{
    m_uiHapticObjects = 0;
    InitHapticData();
    InitCursor( toolOneFileName );
}
//-----------------------------------------------------------------------------
// Destructor
template <typename T>
HapticManager<T>::~HapticManager ()
{}
//-----------------------------------------------------------------------------
// Setup
// creates a random cloud of points with random colors within a canonical view.
template <typename T>
void HapticManager<T>::Setup (  
        TAPs::OpenGL::BaseModelManager<T> *         pModelManager,
        TAPs::OpenGL::BaseOpenGLViewManager<T> *    pViewManager )
{
    m_pModelManager = pModelManager;
    assert( m_pModelManager );
    m_pViewManager = pViewManager;
    assert( m_pViewManager );
    //----------------------------------------------------------------
    // Check Haptic Device
    HDErrorInfo error;
    m_hHD = hdInitDevice( HD_DEFAULT_DEVICE );
    if ( HD_DEVICE_ERROR( error = hdGetError() ) ) {
        hduPrintError( stderr, &error, 
            "Failed to initialize haptic device!" );
        fprintf( stderr, "Press any key to exit" );
        getchar();
        exit(1);
    }
    //----------------------------------------------------------------
    // Create a haptic context for the device.
    // The haptic context maintains the state that persists between 
    // frame intervals and is used for haptic rendering.
    m_hHLRC = hlCreateContext( m_hHD );
    hlMakeCurrent( m_hHLRC );
    //----------------------------------------------------------------
    // Enable optimization of the viewing parameters when rendering
    // geometry for OpenHaptics.
    //hlEnable(HL_HAPTIC_CAMERA_VIEW);
    //----------------------------------------------------------------
    // Front face touchable only - no need to feel back faces
    hlTouchableFace( HL_FRONT );
    //----------------------------------------------------------------
    int iNumPoints = pModelManager->strandObj->GetNumberOfLinks() + 1;
    m_uiPointNameBase = hlGenShapes( iNumPoints + 2 );
    for ( int i = 0; i < iNumPoints; ++i ) {
        // Register event callbacks for all of the points, so we can 
        // be notified when the points are touched and untouched.
        HLuint uiPointName = m_uiPointNameBase + i;
        hlAddEventCallback( HL_EVENT_TOUCH, uiPointName, 
                            HL_CLIENT_THREAD, hlTouchCB, this );
        hlAddEventCallback( HL_EVENT_UNTOUCH, uiPointName, 
                            HL_CLIENT_THREAD, hlTouchCB, this );
        hlAddEventCallback( HL_EVENT_1BUTTONDOWN, uiPointName, 
                            HL_CLIENT_THREAD, hlButtonDownCB, this );
    }
    //----------------------------------------------------------------
    // Add a callback to handle button up in the collision thread
    hlAddEventCallback( HL_EVENT_1BUTTONUP, HL_OBJECT_ANY,
                        HL_CLIENT_THREAD, hlButtonUpCB, this );
    //----------------------------------------------------------------
    // Add a callback to handle button down in the collision thread
//  hlAddEventCallback( HL_EVENT_1BUTTONDOWN, HL_OBJECT_ANY, 
//                      HL_COLLISION_THREAD, 
//                      buttonDownCollisionThreadCallback, NULL );
    //----------------------------------------------------------------
    // Start an ambient drag friction effect
    m_uiEffectName = hlGenEffects( 1 );
    hlBeginFrame();
        hlEffectd( HL_EFFECT_PROPERTY_GAIN, 0.4 );
        hlEffectd( HL_EFFECT_PROPERTY_MAGNITUDE, 0.1 );
        hlStartEffect( HL_EFFECT_FRICTION, m_uiEffectName );
    hlEndFrame();
    //----------------------------------------------------------------
    // Tie Cursor with ToolOne of pModelManager (BaseModelManager)
    pModelManager->ToolOne = ToolOne;
    //----------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// Cleanup
template <typename T>
void HapticManager<T>::Cleanup ()
{
    if ( m_hHLRC != NULL ) {
        assert( m_pModelManager );
        hlBeginFrame();
            hlStopEffect( m_uiEffectName );
        hlEndFrame();
        //------------------------------------------------------
        hlDeleteEffects( m_uiEffectName, 1 );
        int iNumPoints = m_pModelManager->strandObj->GetNumberOfLinks() + 1;
        for ( int i = 0; i < iNumPoints; ++i ) {
            // Unregister event callbacks for all of the points
            HLuint uiPointName = m_uiPointNameBase + i;
            hlRemoveEventCallback(  HL_EVENT_TOUCH, uiPointName,
                                    HL_CLIENT_THREAD, hlTouchCB );
            hlRemoveEventCallback(  HL_EVENT_UNTOUCH, uiPointName,
                                    HL_CLIENT_THREAD, hlTouchCB );
            hlRemoveEventCallback(  HL_EVENT_1BUTTONDOWN, uiPointName,
                                    HL_CLIENT_THREAD, hlButtonDownCB );
        }
        //------------------------------------------------------
        hlRemoveEventCallback(  HL_EVENT_1BUTTONUP, HL_OBJECT_ANY, 
                                HL_CLIENT_THREAD, hlButtonUpCB );
        hlDeleteShapes( m_uiPointNameBase, iNumPoints );
        hlMakeCurrent( NULL );
        hlDeleteContext( m_hHLRC );
    }
    //----------------------------------------------------------------
    if ( m_hHD != HD_INVALID_HANDLE )   hdDisableDevice( m_hHD );
    //----------------------------------------------------------------
    // Deallocate the object ID(s)
    //hlDeleteShapes( m_uiHapticObjects, 2 );
}
//-----------------------------------------------------------------------------
// UpdateWorkSpace
// uses the current OpenGL viewing transforms to initialize a transform for the 
// haptic device workspace so that it's properly mapped to world coordinates.
template <typename T>
void HapticManager<T>::UpdateWorkSpace ()
{
    GLdouble modelview[16];
    GLdouble projection[16];
    GLint viewport[4];
    glGetDoublev( GL_MODELVIEW_MATRIX, modelview );
    glGetDoublev( GL_PROJECTION_MATRIX, projection );
    glGetIntegerv( GL_VIEWPORT, viewport );
    hlMatrixMode( HL_TOUCHWORKSPACE );
    hlLoadIdentity();
    // Use of a non-uniform workspace fit, which offer a maximal fit to the view 
    // frustum but introduces non-uniform scale in the workspace.
    // This non-uniform scale is fine for point and line based haptics, like 
    // this example, but can introduct distortion when actually feeling surfaces 
    // (i.e. a sphere might feel like an ellipsoid)
    //hluFitWorkspaceNonUniform( projection );
    hluFitWorkspace( projection );
}
//-----------------------------------------------------------------------------
// DrawCursor
// displays a cursor using the current haptic device proxy transform and the 
// mapping between the workspace and the world coordinates
template <typename T>
void HapticManager<T>::DrawCursor ( OpenGL::Enum::DrawMode DM )
{
    static const double K_CURSOR_SIZE_PIXELS = 15;
    static const double K_CURSOR_RADIUS = 0.5;
    static const double K_CURSOR_HEIGHT = 5.0;
    static const int K_CURSOR_TESS = 15;
    HLdouble transform[16];
    GLUquadricObj *qObj = NULL;
    //--------------------------------------------------------------------
    glPushAttrib( GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT );
    glPushMatrix();
    //--------------------------------------------------------------------
    if ( !m_uiCursorDisplayList ) {
        m_uiCursorDisplayList = glGenLists( 2 );
        //------------------------------------------------------------
        // Cone Cursor
        glNewList( m_uiCursorDisplayList, GL_COMPILE );
            qObj = gluNewQuadric();
            glScaled( 0.2, 0.2, 0.2 );
            //gluSphere( qObj, K_CURSOR_RADIUS, K_CURSOR_TESS, K_CURSOR_TESS );
            gluCylinder( qObj, 0.0, K_CURSOR_RADIUS, K_CURSOR_HEIGHT, 
                                    K_CURSOR_TESS, K_CURSOR_TESS );
            glTranslated( 0.0, 0.0, K_CURSOR_HEIGHT );
            gluCylinder( qObj, K_CURSOR_RADIUS, 0.0, K_CURSOR_HEIGHT/5.0, 
                                    K_CURSOR_TESS, K_CURSOR_TESS );
            gluDeleteQuadric( qObj );
        glEndList();
        //------------------------------------------------------------
        // Tool One Cursor
        glNewList( m_uiCursorDisplayList+1, GL_COMPILE );
            ToolOne->DisplayGL( DM );
        glEndList();
    }
    //--------------------------------------------------------------------
    // Apply the local position/rotation transform of the haptic device 
    // proxy
    // I.e. get the proxy transform in world coordinates
    hlGetDoublev( HL_PROXY_TRANSFORM, transform );
    //glCallList( m_uiCursorDisplayList+1 );
    Matrix4x4<T> tooloneTransformMatrix( 
        transform[ 0], transform[ 4], transform[ 8], transform[12], 
        transform[ 1], transform[ 5], transform[ 9], transform[13], 
        transform[ 2], transform[ 6], transform[10], transform[14], 
        transform[ 3], transform[ 7], transform[11], transform[15]
    );
    ToolOne->GetTransform().SetMatrixTransform( tooloneTransformMatrix );
    glMultMatrixd( transform );
    //--------------------------------------------------------------------
    // Apply the local cursor scale factor
    //double scale = m_pViewManager->GetWindowToWorldScale() * K_CURSOR_SIZE_PIXELS;
    //glScaled( scale, scale, scale );
    //glEnable( GL_NORMALIZE );
    //ToolOne->DisplayGL( DM );
    glCallList( m_uiCursorDisplayList+1 );          // Tool One Cursor
    //ToolOne->GetBVHTree()->DrawByOpenGL( 0, 0, 10 );
    //glPushAttrib( GL_DEPTH_BUFFER_BIT );
    //glEnable( GL_COLOR_MATERIAL );
    //glColor3f( 0.0, 0.5, 1.0 );
    //glCallList( m_uiCursorDisplayList );          // Cone Cursor
    //glDisable( GL_COLOR_MATERIAL );
    //glPopAttrib();
    //--------------------------------------------------------------------
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
// DrawTooltipSphere
// displays a tooltip sphere using the current haptic device proxy transform and the 
// mapping between the workspace and the world coordinates
template <typename T>
void HapticManager<T>::DrawTooltipSphere ( OpenGL::Enum::DrawMode DM )
{
    //--------------------------------------------------------------------
    // Apply the local position/rotation transform of the haptic device 
    // proxy
    // I.e. get the proxy transform in world coordinates
    hlGetDoublev( HL_PROXY_TRANSFORM, transform );
    //glCallList( m_uiCursorDisplayList+1 );
    Matrix4x4<T> tooloneTransformMatrix( 
        transform[ 0], transform[ 4], transform[ 8], transform[12], 
        transform[ 1], transform[ 5], transform[ 9], transform[13], 
        transform[ 2], transform[ 6], transform[10], transform[14], 
        transform[ 3], transform[ 7], transform[11], transform[15]
    );
    //--------------------------------------------------------------------
    glPushAttrib( GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT );
    glPushMatrix();
    //--------------------------------------------------------------------
    glMultMatrixd( transform );
    glTranslatef( 0, 0, ToolOneSphereTip[3] / 2 );
    pModelManager->ReturnPtrToOpenGLUsefulObj()->DrawSphere( 
        Vector3<Real>( ToolOneSphereTip[3], ToolOneSphereTip[3], ToolOneSphereTip[3] ) );
    //--------------------------------------------------------------------
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
// DrawHapticObjects
// DESC: Uses OpenGL to draw objects that can be feel by the haptic device.
template <typename T>
void HapticManager<T>::DrawHapticObjects ( OpenGL::Enum::DrawMode DM )
{
    //---------------------------------------------------------------
    hlBeginFrame();
    //---------------------------------------------------------------
    FeelPoints();
    FeelObjects();
    //---------------------------------------------------------------
    hlEndFrame();
}
//-----------------------------------------------------------------------------
// FeelObjects
// DESC: Uses OpenGL to draw objects that can be feel by the haptic device.
template <typename T>
void HapticManager<T>::FeelObjects ( OpenGL::Enum::DrawMode DM )
{
    //hlBeginFrame();
    int iNumPoints = m_pModelManager->strandObj->GetNumberOfLinks() + 1;
    hlBeginShape( HL_SHAPE_FEEDBACK_BUFFER, m_uiPointNameBase + iNumPoints );
        hlTouchModel( HL_CONTACT );
        hlMaterialf( HL_FRONT, HL_STIFFNESS, 0.5 );
        hlMaterialf( HL_FRONT, HL_DAMPING, 0.5 );
        hlMaterialf( HL_FRONT, HL_STATIC_FRICTION, 0.3 );
        hlMaterialf( HL_FRONT, HL_DYNAMIC_FRICTION, 0.2 );
        // Use OpenGL commands to create geometry.
        m_pModelManager->leftTube->DisplayGL( DM );
    hlEndShape();
    hlBeginShape( HL_SHAPE_FEEDBACK_BUFFER, m_uiPointNameBase + iNumPoints + 1);
        hlTouchModel( HL_CONTACT );
        hlMaterialf( HL_FRONT, HL_STIFFNESS, 0.7 );
        hlMaterialf( HL_FRONT, HL_DAMPING, 0.1 );
        hlMaterialf( HL_FRONT, HL_STATIC_FRICTION, 0.2 );
        hlMaterialf( HL_FRONT, HL_DYNAMIC_FRICTION, 0.3 );
        // Use OpenGL commands to create geometry.
        m_pModelManager->rightTube->DisplayGL( DM );
    hlEndShape();
    //-------------------------------------------------------------------
    //hlEndFrame();
}
//-----------------------------------------------------------------------------
// FeelPoints
// draw points and lines for haptic device
template <typename T>
void HapticManager<T>::FeelPoints ()
{
    //hlBeginFrame();
    //-------------------------------------------------------------------
    int iNumPoints = m_pModelManager->strandObj->GetNumberOfLinks() + 1;
    for ( int i = 0; i < iNumPoints; ++i ) {
        //------------------------------------------------------
        // Don't haptically draw a selected point, since it's 
        // being manipulated
        if ( m_iManipPoint == i )   continue;
        //------------------------------------------------------
        // Draw a shape that represents the projection of the 
        // points to the near and far planes
        //hlHinti( HL_SHAPE_FEEDBACK_BUFFER_VERTICES, 3 );
        hlBeginShape( HL_SHAPE_FEEDBACK_BUFFER, m_uiPointNameBase + i );
            hlTouchModel( HL_CONSTRAINT );
            hlTouchModelf( HL_SNAP_DISTANCE, g_tSnapDistance );
            //hlMaterialf( HL_FRONT_AND_BACK, HL_STIFFNESS, 0.5 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_STIFFNESS, 0.5 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_DAMPING, 0.25 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_STATIC_FRICTION, 0.5 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_DYNAMIC_FRICTION, 0.25 );
            TAPs::Vector3<T> pt = m_pModelManager->strandObj->GetPointPosition(i);
            //---------------------------------------------
            // Draw a line that represents the projection 
            // of the point to the near and far of the view 
            // volume
            TAPs::Vector3<T> winPt, nearPt, farPt;
            m_pViewManager->ToScreen( pt, winPt );
            winPt[2] = 0;
            m_pViewManager->FromScreen( winPt, nearPt );
            winPt[2] = 1;
            m_pViewManager->FromScreen( winPt, farPt );
            glBegin( GL_LINES );
                glVertex3dv( nearPt );
                glVertex3dv( farPt );
            glEnd();
            glPointSize( 10 );
            glBegin( GL_POINTS );
                glVertex3dv( pt );
            glEnd();
        hlEndShape();
    }
    //-------------------------------------------------------------------
    // Call any event callbacks that have been triggered
    hlCheckEvents();
    //-------------------------------------------------------------------
    //hlEndFrame();
}
//-----------------------------------------------------------------------------
// IsManipulating
template <typename T>
bool HapticManager<T>::IsManipulating () const
{
    return  m_iManipPoint != -1;
}
//-----------------------------------------------------------------------------
//=============================================================================
// Helper Fn(s)
//-----------------------------------------------------------------------------
// StartManipulating
template <typename T>
void HapticManager<T>::StartManipulating ( int i )
{
    assert( !IsManipulating() );
    assert( 0 <= i && i <= m_pModelManager->strandObj->GetNumberOfLinks() );
    hlAddEventCallback( HL_EVENT_MOTION, HL_OBJECT_ANY,
                        HL_CLIENT_THREAD, hlMotionCB, this );
    m_iManipPoint = i;
    static TAPs::Vector3<T> proxyPos;
    //----------------------------------------------------------------
    // Store off proxy position so we can compute how much it moves each
    // frame which is how much the drag object should also move
    hlGetDoublev( HL_PROXY_POSITION, proxyPos );
    //----------------------------------------------------------------
    // Current Rotation at the start of dragging
    hlGetDoublev( HL_PROXY_TRANSFORM, startDragProxyTransform );
    // Current Proxy Rotation (Transposed)
    startDragProxyRotation = Matrix3x3<T>( 
        startDragProxyTransform[0], startDragProxyTransform[1], startDragProxyTransform[2], 
        startDragProxyTransform[4], startDragProxyTransform[5], startDragProxyTransform[6], 
        startDragProxyTransform[8], startDragProxyTransform[9], startDragProxyTransform[10] 
    );
    // [Start Drag Rotation] = [Current Proxy Rotation]transpose * [Needle Rotation]
    //startDragProxyRotation *= m_pModelManager->strandObj->GetLeftNeedleRotationMatrix();
    //startDragProxyRotation *= m_pModelManager->strandObj->GetTransform().GetMatrixTransform().GetMatrix3x3();
    startDragProxyRotation *= m_pModelManager->strandObj->GetLeftNeedleTransform().GetMatrixRotation();
    //----------------------------------------------------------------
    // Store off initial position and orientation of drag object
    //startDragObjTransform = draggableObjects[currentDragObj].transform;
    //----------------------------------------------------------------

    //TAPs::Vector3<T> offsetWC = m_pModelManager->strandObj->GetPointPosition(i) - proxyPos;
    //----------------------------------------------------------------
    // This is a very useful technique for offsetting the user in 
    // space so that they begin their manipulation exactly at the 
    // location of the selected point.
    //PushWorkSpaceOffset( offsetWC );
}
//-----------------------------------------------------------------------------
// StopManipulating
template <typename T>
void HapticManager<T>::StopManipulating ()
{
    assert( IsManipulating() );
    hlRemoveEventCallback(  HL_EVENT_MOTION, HL_OBJECT_ANY,
                            HL_CLIENT_THREAD, hlMotionCB );
    m_iManipPoint = -1;
    PopWorkSpaceOffset();
}
//-----------------------------------------------------------------------------
// UpdateManipPoint
template <typename T>
void HapticManager<T>::UpdateManipPoint ( 
                const TAPs::Vector3<T> &    proxyPos, 
                const TAPs::Matrix4x4<T> &  proxyTransform )
{
    // Current Rotation of this motion
    TAPs::Matrix3x3<T> currRotation( 
        proxyTransform[0], proxyTransform[4], proxyTransform[8], 
        proxyTransform[1], proxyTransform[5], proxyTransform[9], 
        proxyTransform[2], proxyTransform[6], proxyTransform[10] 
    );
    //--------------------------------------------------------------------
    TAPs::Matrix3x3<T> rotation =  currRotation * startDragProxyRotation;
    //rotation = m_pModelManager->strandObj->m_mRotMatForLeftNeedle * rotation;
    //rotation = m_pModelManager->strandObj->m_mRotMatForLeftNeedle.GetTranspose();
    m_pModelManager->strandObj->SetPointPosition( m_iManipPoint, proxyPos, rotation );
    if ( m_pModelManager->CollisionDetection() ) {
        //gModelManager->strandObj->RestorePreviousPositions();
        //gModelManager->strandObj->SetPointPosition( gManipPoint, oldPosition );
    }
}
//-----------------------------------------------------------------------------
// PushWorkSpaceOffset
template <typename T>
void HapticManager<T>::PushWorkSpaceOffset ( const TAPs::Vector3<T> &offsetWC )
{
    const TAPs::Matrix4x4<T> &mWorldToView = m_pViewManager->GetViewTransform();
    TAPs::Matrix4x4<T> mViewToTouch;
    TAPs::Matrix4x4<T> mTouchToWorkSpace;
    hlGetDoublev( HL_VIEWTOUCH_MATRIX, mViewToTouch );
    hlGetDoublev( HL_TOUCHWORKSPACE_MATRIX, mTouchToWorkSpace );
    TAPs::Matrix4x4<T> mWorldToWorkSpace = mWorldToView * mViewToTouch * mTouchToWorkSpace;
    TAPs::Vector3<T> offsetWS;
    //mWorldToWorkSpace.multDirMatrix( offsetWC, offsetWS );
    offsetWS[0] =   mWorldToWorkSpace[ 0]*offsetWC[0] + 
                    mWorldToWorkSpace[ 4]*offsetWC[1] + 
                    mWorldToWorkSpace[ 8]*offsetWC[2];
    offsetWS[1] =   mWorldToWorkSpace[ 1]*offsetWC[0] + 
                    mWorldToWorkSpace[ 5]*offsetWC[1] + 
                    mWorldToWorkSpace[ 9]*offsetWC[2];
    offsetWS[2] =   mWorldToWorkSpace[ 2]*offsetWC[0] + 
                    mWorldToWorkSpace[ 6]*offsetWC[1] + 
                    mWorldToWorkSpace[10]*offsetWC[2];
    //----------------------------------------------------------------
    // Apply the translation in the local coordinates of the workspace
    //TAPs::Matrix4x4<T> mTranslateWS = hduMatrix::createTranslation( -offsetWS );
    TAPs::Matrix4x4<T> mTranslateWS( 1, 0, 0, 0, 
                                     0, 1, 0, 0,
                                     0, 0, 1, 0,
                                     -offsetWS[0], -offsetWS[1], -offsetWS[2], 1 );
    mTouchToWorkSpace.MultRight( mTranslateWS );
    hlMatrixMode( HL_TOUCHWORKSPACE );
    hlPushMatrix();
    hlLoadMatrixd( mTouchToWorkSpace );
}
//-----------------------------------------------------------------------------
// PopWorkSpaceOffset
template <typename T>
void HapticManager<T>::PopWorkSpaceOffset ()
{
    hlMatrixMode( HL_TOUCHWORKSPACE );
    hlPopMatrix();
}
//-----------------------------------------------------------------------------
// GetPointIndexFromTouchID
template <typename T>
int HapticManager<T>::GetPointIndexFromTouchID ( HLuint id ) const
{
    int i = id - m_uiPointNameBase;
    assert( 0 <= i && i <= m_pModelManager->strandObj->GetNumberOfLinks() );
    return i;
}
//-----------------------------------------------------------------------------
// 
template <typename T>
void HLCALLBACK HapticManager<T>::hlTouchCB ( 
        HLenum event, HLuint object, HLenum thread,
        HLcache *cache, void *userData )
{
    HapticManager *pThis = static_cast<HapticManager *>(userData);
    int i = pThis->GetPointIndexFromTouchID( object );
    if ( event == HL_EVENT_TOUCH ) {
        //pThis->m_pModelManager->SetPointHighlighted( i, true );
    }
    else if ( event == HL_EVENT_UNTOUCH ) {
        //pThis->m_pModelManager->SetPointHighlighted( i, false );
    }
}
//-----------------------------------------------------------------------------
// 
template <typename T>
void HLCALLBACK HapticManager<T>::hlButtonDownCB ( 
        HLenum event, HLuint object, HLenum thread,
        HLcache *cache, void *userData )
{
    HapticManager *pThis = static_cast<HapticManager *>(userData);
    int i = pThis->GetPointIndexFromTouchID( object );
    if ( !pThis->IsManipulating() ) {
        //glutIdleFunc( NULL );
        //pThis->m_pModelManager->strandObj->SetFixStatusOfPtNo( i, true );
        pThis->StartManipulating( i );
        g_tSnapDistance = 0.0;
    }
}
//-----------------------------------------------------------------------------
// 
template <typename T>
void HLCALLBACK HapticManager<T>::hlButtonUpCB ( 
        HLenum event, HLuint object, HLenum thread,
        HLcache *cache, void *userData )
{
    HapticManager *pThis = static_cast<HapticManager *>(userData);
    if ( pThis->IsManipulating() ) {
        //pThis->m_pModelManager->strandObj->SetFixStatusOfPtNo( pThis->m_iManipPoint, false );
        pThis->StopManipulating();
        g_tSnapDistance = g_tSnapDistanceConst;
    }
}
//-----------------------------------------------------------------------------
// 
template <typename T>
void HLCALLBACK HapticManager<T>::hlMotionCB ( 
        HLenum event, HLuint object, HLenum thread,
        HLcache *cache, void *userData )
{
    HapticManager *pThis = static_cast<HapticManager *>(userData);

    if ( pThis->IsManipulating() ) {
        // Get the position of the proxy when the motion was detected
        TAPs::Vector3<T> proxyPos;
        TAPs::Matrix4x4<T> proxyTransform;
        hlCacheGetDoublev( cache, HL_PROXY_POSITION, proxyPos );
        hlGetDoublev( HL_PROXY_TRANSFORM, proxyTransform );
        pThis->UpdateManipPoint( proxyPos, proxyTransform );
    }
}
//-----------------------------------------------------------------------------
// InitHapticData
// initializes haptic data
template <typename T>
void HapticManager<T>::InitHapticData ()
{
    // Flag for enabling/disabling axis snap on drag
    m_bHapticAxisSnapOn = true;
    // Flag for enabling/disabling rotation
    m_bHapticRotateOn = true;

    // Generate ID's for haptic objects
    //m_uiHapticObjects = hlGenShapes(2);
}
//-----------------------------------------------------------------------------
// InitCursor
// initializes a cursor reading from a data file 
template <typename T>
void HapticManager<T>::InitCursor ()
{
    char toolOneFile[] = "Data/3dsMax/Tweezers.ASE";
    InitCursor( toolOneFile );
}
//-----------------------------------------------------------------------------
// InitCursor
// initializes a cursor reading from a data file 
template <typename T>
void HapticManager<T>::InitCursor ( const char * toolOneFile )
{
    //----------------------------------------------------------------
    // Set Center, then Rotations of x,y,z axes, and Scale
    T scale = 1.5;
    Vector3<T> vCenter( 0, 0, 0 );
    Vector3<T> vRotationAngle( 0, 0, 0 );
    Vector3<T> vScale( scale, scale, scale);
    //----------------------------------------------------------------
    ToolOne  = new TAPs::OpenGL::OpenGLHalfEdgeModel<T>();
    //ToolRight = new OpenGLHalfEdgeModel<T>();
    //----------------------------------------------------------------
    if ( !ReadModels<Real>::readFile( toolOneFile, ToolOne ) ) {
        std::cout << "ERROR: In File Reading! for Tool#1" << std::endl;
        exit ( EXIT_FAILURE );
    }
    else {
        std::cout << "Create Tool#1" << std::endl;
        ToolOne->SetMaterial( OpenGL::Enum::METAL_SILVER );
        ToolOne->GetTransform().SetTranslation( vCenter );
        ToolOne->GetTransform().SetRotationAngles( vRotationAngle );
        ToolOne->GetTransform().SetScale( vScale );
        ToolOne->TransformByTranslationRatationAndScale();
        ToolOne->BuildBVHTree( TAPs::Enum::BVH_TREE_BINARY_SPHERE );
    }
}
//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs__Haptic
//-----------------------------------------------------------------------------
//345678901234567890123456789012345678901234567890123456789012345678901234567890
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8