TAPsHapticManager.cpp

Go to the documentation of this file.

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

Inherited from class BaseHapticManager

SUKITTI PUNAK   (07/21/2005)
UPDATE          (07/30/2005)
******************************************************************************/
#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
//=============================================================================
//-----------------------------------------------------------------------------
// Default Constructor
template <typename T>
HapticManager<T>::HapticManager ()
    : BaseHapticManager<T>(),
      m_pPointManager( 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 )
{}
//-----------------------------------------------------------------------------
// 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::BasePointManager<T> * pPointManager,
        TAPs::OpenGL::OpenGLBaseViewManager<T>  * pViewManager )
{
    m_pPointManager = pPointManager;
    assert( m_pPointManager );
    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 );
    int iNumPoints = pPointManager->GetNumPoints();
    m_uiPointNameBase = hlGenShapes( iNumPoints );
    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 );
    }
    //----------------------------------------------------------------
    hlAddEventCallback( HL_EVENT_1BUTTONUP, HL_OBJECT_ANY,
                        HL_CLIENT_THREAD, hlButtonUpCB, this );
    //----------------------------------------------------------------
    // 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();
    //----------------------------------------------------------------
}
//-----------------------------------------------------------------------------
// Cleanup
template <typename T>
void HapticManager<T>::Cleanup ()
{
    if ( m_hHLRC != NULL ) {
        assert( m_pPointManager );
        //hlBeginFrame();
        //  hlStopEffect( m_uiEffectName );
        //hlEndFrame();
        //------------------------------------------------------
        //hlDeleteEffects( m_uiEffectName, 1 );
        int iNumPoints = m_pPointManager->GetNumPoints();
        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 );
}
//-----------------------------------------------------------------------------
// 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 );
}
//-----------------------------------------------------------------------------
// 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 ()
{
    static const double K_CURSOR_SIZE_PIXELS = 15;
    static const double K_CURSOR_RADIUS = 0.5;
    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( 1 );
        glNewList( m_uiCursorDisplayList, GL_COMPILE );
        qObj = gluNewQuadric();
        gluSphere( qObj, K_CURSOR_RADIUS, K_CURSOR_TESS, K_CURSOR_TESS );
        gluDeleteQuadric( qObj );
        glEndList();
    }
    //-------------------------------------------------------------------
    // Apply the local position/rotation transform of the haptic device 
    // proxy
    hlGetDoublev( HL_PROXY_TRANSFORM, transform );
    glMultMatrixd( transform );
    //-------------------------------------------------------------------
    // Apply the local cursor scale factor
    double scale = m_pViewManager->GetWindowToWorldScale() * K_CURSOR_SIZE_PIXELS;
    glScaled( scale, scale, scale );
    glEnable( GL_NORMALIZE );
    glEnable( GL_COLOR_MATERIAL );
    glColor3f( 1.0, 1.0, 1.0 );
    glCallList( m_uiCursorDisplayList );
    //-------------------------------------------------------------------
    glPopMatrix();
    glPopAttrib();
}
//-----------------------------------------------------------------------------
// FeelPoints
// draw points and lines for haptic device
template <typename T>
void HapticManager<T>::FeelPoints ()
{
    hlBeginFrame();
    //-------------------------------------------------------------------
    int iNumPoints = m_pPointManager->GetNumPoints();
    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, 3.5 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_STIFFNESS, 0.5 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_DAMPING, 0.0 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_STATIC_FRICTION, 0.0 );
            hlMaterialf( HL_FRONT_AND_BACK, HL_DYNAMIC_FRICTION, 0.0 );
            const TAPs::Vector3<T> &pt = m_pPointManager->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();
            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_pPointManager->GetNumPoints() );
    m_pPointManager->SetPointSelected( i, true );
    hlAddEventCallback( HL_EVENT_MOTION, HL_OBJECT_ANY,
                        HL_CLIENT_THREAD, hlMotionCB, this );
    m_iManipPoint = i;
    TAPs::Vector3<T> proxyPos;
    hlGetDoublev( HL_PROXY_POSITION, proxyPos );
    TAPs::Vector3<T> offsetWC = m_pPointManager->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() );
    m_pPointManager->SetPointSelected( m_iManipPoint, false );
    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 )
{
    m_pPointManager->SetPointPosition( m_iManipPoint, proxyPos );
}
//-----------------------------------------------------------------------------
// 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_pPointManager->GetNumPoints() );
    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_pPointManager->SetPointHighlighted( i, true );
    }
    else if ( event == HL_EVENT_UNTOUCH ) {
        pThis->m_pPointManager->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() ) {
        pThis->StartManipulating( i );
    }
}
//-----------------------------------------------------------------------------
// 
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->StopManipulating();
    }
}
//-----------------------------------------------------------------------------
// 
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;
        hlCacheGetDoublev( cache, HL_PROXY_POSITION, proxyPos );
        pThis->UpdateManipPoint( proxyPos );
    }
}
//-----------------------------------------------------------------------------
//=============================================================================
END_NAMESPACE_TAPs__Haptic
//-----------------------------------------------------------------------------
//345678901234567890123456789012345678901234567890123456789012345678901234567890
//--+----1----+----2----+----3----+----4----+----5----+----6----+----7----+----8