H3DHapticsDevice.h

Go to the documentation of this file.

//    Copyright 2004-2019, SenseGraphics AB
//
//    This file is part of H3D API.
//
//    H3D API is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    H3D API is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with H3D API; if not, write to the Free Software
//    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//    A commercial license is also available. Please contact us at 
//    www.sensegraphics.com for more information.
//
//
//
#ifndef __H3DHAPTICSDEVICE_H__
#define __H3DHAPTICSDEVICE_H__
 
// H3D includes
#include <H3D/FieldTemplates.h>
#include <H3D/SFNode.h>
#include <H3D/SFRotation.h>
#include <H3D/SFFloat.h>
#include <H3D/SFInt32.h>
#include <H3D/SFBool.h>
#include <H3D/SFString.h>
#include <H3D/SFMatrix4f.h>
#include <H3D/SFVec3f.h>
#include <H3D/MFVec3f.h>
#include <H3D/SFTime.h>
#include <H3D/H3DHapticsRendererNode.h>
 
// H3DUtil includes
#include <H3DUtil/Threads.h>
 
// HAPI includes
#include <HAPI/HAPIHapticShape.h>
#include <HAPI/HAPIHapticsDevice.h>
#include <HAPI/HAPIForceEffect.h>
 
 
namespace H3D {
 
  class H3DAPI_API H3DHapticsDevice: public Node {
  protected:
    H3DUniquePtr< HAPI::HAPIHapticsDevice > hapi_device;
 
  public:
 
    typedef HAPI::HAPIHapticsDevice::ErrorCode ErrorCode;
    typedef HAPI::HAPIHapticsDevice::DeviceState DeviceState;
    typedef SFBool MainButton;
    typedef SFBool SecondaryButton;
 
    typedef
    DEPRECATED("SFMatrix4f", )
           SFMatrix4f PosCalibration;
 
    typedef
    DEPRECATED("SFRotation", )
           SFRotation OrnCalibration;
 
    class H3DAPI_API TrackerPosition: 
      public TypedField< SFVec3f, Types< SFMatrix4f, SFVec3f > > {
    protected:
 
      virtual void update() {
        H3DHapticsDevice *hd = static_cast< H3DHapticsDevice *>(owner);
        Matrix4f m;
        if( hd->followViewpoint->getValue() ) {
          m = hd->adjustedPositionCalibration->getValue();
        }
        else {
          m = static_cast< SFMatrix4f * >( routes_in[0] )->getValue();
        }
        const Vec3f &d_pos = 
          static_cast< SFVec3f * >( routes_in[1] )->getValue();
        value = m * d_pos;
      }
    };
 
    class H3DAPI_API TrackerVelocity: 
      public TypedField< SFVec3f, Types< SFMatrix4f, SFVec3f > > {
      
      virtual void update() {
        H3DHapticsDevice *hd = static_cast< H3DHapticsDevice *>(owner);
        Matrix4f m;
        if( hd->followViewpoint->getValue() ) {
          m = hd->adjustedPositionCalibration->getValue();
        }
        else {
          m = static_cast< SFMatrix4f * >( routes_in[0] )->getValue();
        }
        const Vec3f &d_vel = 
          static_cast< SFVec3f * >( routes_in[1] )->getValue();
        
        value = m.getScaleRotationPart() * d_vel;
      }
    };
 
    class H3DAPI_API TrackerOrientation: 
      public TypedField< SFRotation, Types< SFRotation, SFRotation > > {
    protected:
 
      virtual void update() {
        H3DHapticsDevice *hd = static_cast< H3DHapticsDevice *>(owner);
        Rotation cal;
        if( hd->followViewpoint->getValue() ) {
          cal = hd->adjustedOrnCalibration->getValue();
        }
        else {
          cal  = 
          static_cast< SFRotation * >( routes_in[0] )->getValue();
        }
 
        const Rotation &d_orn = 
          static_cast< SFRotation * >( routes_in[1] )->getValue();
        value = cal * d_orn;
      }
  
    };
 
    class H3DAPI_API WeightedProxy:
      public TypedField< SFVec3f, Types< SFVec3f, SFVec3f, SFFloat > > {
      virtual void update() {
        const Vec3f &proxy_pos   = 
          static_cast< SFVec3f * >( routes_in[0] )->getValue();
        const Vec3f &tracker_pos = 
          static_cast< SFVec3f * >( routes_in[1] )->getValue();
        H3DFloat weighting = 
          static_cast< SFFloat * >( routes_in[2] )->getValue();
        value =  tracker_pos + weighting * ( proxy_pos - tracker_pos );
      }
    };
 
    class H3DAPI_API SFHapticsRendererNode: 
      public TypedSFNode< H3DHapticsRendererNode > {
      virtual void onAdd( Node *n ) {
        TypedSFNode< H3DHapticsRendererNode >::onAdd( n );
        H3DHapticsRendererNode *renderer = 
          static_cast< H3DHapticsRendererNode * >( n );
        H3DHapticsDevice *device = 
          static_cast< H3DHapticsDevice * >( getOwner() );
        if( renderer && device->hapi_device.get() ) {
          for( unsigned int i = 0; i < device->hapi_device->nrLayers(); ++i )
            device->hapi_device->setHapticsRenderer( 
                                     renderer->getHapticsRenderer( i ), i );
        }
      }
      virtual void onRemove( Node *n ) {
        H3DHapticsRendererNode *renderer = static_cast< H3DHapticsRendererNode * >( n );
        H3DHapticsDevice *device = static_cast< H3DHapticsDevice * >( getOwner() );
        if( renderer && device->hapi_device.get() ) {
          for( unsigned int i = 0; i < device->hapi_device->nrLayers(); ++i ) {
            renderer->hapticsRendererRemoved( i );
            device->hapi_device->setHapticsRenderer( NULL, i );
          }
        }
        TypedSFNode< H3DHapticsRendererNode >::onRemove( n );
      }
 
    };
 
 
    class H3DAPI_API SetEnabled: public OnValueChangeSField< SFBool > {
      virtual void onValueChange( const bool &v ) {
        H3DHapticsDevice *hd = 
          static_cast< H3DHapticsDevice * >( getOwner() );
        if( v ) hd->enableDevice();
        else hd->disableDevice();
      }
    };
 
    class TrackerAngularVelocity;
 
    H3DHapticsDevice( Inst< SFVec3f         > _devicePosition         = 0,
                      Inst< SFRotation      > _deviceOrientation      = 0,
                      Inst< TrackerPosition > _trackerPosition        = 0,
                      Inst< TrackerOrientation > _trackerOrientation  = 0,
                      Inst< SFMatrix4f  > _positionCalibration    = 0,
                      Inst< SFRotation  > _orientationCalibration = 0,
                      Inst< SFVec3f         > _proxyPosition          = 0,
                      Inst< WeightedProxy   > _weightedProxyPosition  = 0,
                      Inst< SFFloat         > _proxyWeighting         = 0,
                      Inst< SFBool          > _mainButton             = 0,
                      Inst< SFBool          > _secondaryButton        = 0,
                      Inst< SFInt32         > _buttons                = 0,
                      Inst< SFVec3f         > _force                  = 0,
                      Inst< SFVec3f         > _torque                 = 0,
                      Inst< SFInt32         > _inputDOF               = 0,
                      Inst< SFInt32         > _outputDOF              = 0,
                      Inst< SFInt32         > _hapticsRate            = 0,
                      Inst< SFInt32         > _desiredHapticsRate     = 0,
                      Inst< SFNode          > _stylus                 = 0,
                      Inst< SFHapticsRendererNode > _hapticsRenderer  = 0,
                      Inst< MFVec3f         > _proxyPositions         = 0,
                      Inst< SFBool          > _followViewpoint        = 0,
                      Inst< SFVec3f         > _deviceVelocity         = 0,
                      Inst< TrackerVelocity > _trackerVelocity        = 0,
                      #ifdef HAVE_PROFILER
                      Inst< SFString        > _profiledResult         = 0,
                      #endif
                      Inst< SFVec3f         > _deviceAngularVelocity         = 0,
                      Inst< TrackerAngularVelocity > _trackerAngularVelocity = 0
                      );
 
    virtual ~H3DHapticsDevice() {
      releaseDevice();
    }
 
    inline Vec3f getPreviousProxyPosition( unsigned int layer = 0 ) {
      return previous_proxy_pos[layer];
    }
 
    inline const vector< Vec3f > &getPreviousProxyPositions() {
      return previous_proxy_pos;
    }
 
    virtual string defaultXMLContainerField() {
      return "device";
    }
 
    inline HAPI::HAPIHapticsDevice *getHAPIDevice() {
      return hapi_device.get();
    }
 
    virtual ErrorCode initDevice();
 
    virtual ErrorCode releaseDevice();
 
    virtual ErrorCode enableDevice();
 
    virtual ErrorCode disableDevice();
 
    inline H3DUtil::PeriodicThreadBase *getThread() { 
      if( hapi_device.get() ) {
        return hapi_device->getThread();
      } else {
        return NULL;
      }
    }
 
    virtual void postInit() {}
 
    virtual void updateDeviceValues();
 
    virtual void preRender() {}
 
    virtual void postRender() {}
 
    virtual void renderShapes( const HapticShapeVector &shapes, 
                               unsigned int layer = 0 );
 
    virtual void renderStylus();
 
    virtual void renderEffects( const HapticEffectVector &effects );
    
    H3DUniquePtr< SFVec3f >    devicePosition;
 
    H3DUniquePtr< SFRotation > deviceOrientation;
    
    H3DUniquePtr< TrackerPosition >    trackerPosition;
#ifdef HAVE_PROFILER
    H3DUniquePtr< SFString >  profiledResult;
#endif
    H3DUniquePtr< TrackerOrientation > trackerOrientation;
 
    H3DUniquePtr< SFMatrix4f > positionCalibration;
    
    H3DUniquePtr< SFMatrix4f > adjustedPositionCalibration;
 
    H3DUniquePtr< SFRotation > orientationCalibration;
 
    H3DUniquePtr< SFRotation > adjustedOrnCalibration;
 
    H3DUniquePtr< SFVec3f >   proxyPosition;
 
    H3DUniquePtr< WeightedProxy >   weightedProxyPosition;
 
    H3DUniquePtr< SFFloat >   proxyWeighting;
 
    H3DUniquePtr< SFBool >   mainButton;
 
    H3DUniquePtr< SFBool >   secondaryButton;
 
    H3DUniquePtr< SFInt32 > buttons;
 
    H3DUniquePtr< SFVec3f >    force;
 
    H3DUniquePtr< SFVec3f >    torque;
 
    H3DUniquePtr< SFInt32 >   inputDOF;
 
    H3DUniquePtr< SFInt32 >   outputDOF;
 
    H3DUniquePtr< SFInt32 >   hapticsRate;
 
    H3DUniquePtr< SFInt32 >   desiredHapticsRate;
 
    H3DUniquePtr< SFTime > hapticsLoopTime;
        
    H3DUniquePtr< SFNode > stylus;
 
    H3DUniquePtr< SFBool > initialized;
 
    H3DUniquePtr< SFHapticsRendererNode > hapticsRenderer;
 
    H3DUniquePtr< MFVec3f >   proxyPositions;
 
    H3DUniquePtr< SetEnabled > set_enabled;
 
    H3DUniquePtr< SFBool > enabled;
 
    H3DUniquePtr< SFBool > followViewpoint;
 
    H3DUniquePtr< SFVec3f >    deviceVelocity;
 
    H3DUniquePtr< TrackerVelocity >  trackerVelocity;
 
    H3DUniquePtr< SFFloat > forceLimit;
 
    H3DUniquePtr < SFFloat > forceScale;
 
    H3DUniquePtr< SFBool > deadmansSwitch;
 
    H3DUniquePtr< SFFloat > torqueLimit;
    
    static H3DNodeDatabase database;
  protected:
    vector< Vec3f > previous_proxy_pos;
 
    // int because traverseSG is not run for the viewpoint
    // before the first call to updateDeviceValues
    // Over 1 when a default_vp_pos is found.
    int vp_initialized;
 
    // the position in global coordinates of the viewpoint which should
    // be used as default reference if the haptic device should follow
    // viewpoint movement.
    Matrix4f default_vp_pos_mtx;
    Vec3f default_vp_scaling;
 
    // the orientation in global coordinates of the viewpoint which should
    // be used as default reference if the haptic device should follow
    // viewpoint movement.
    Matrix3f default_vp_orn_mtx;
 
    // The time of the last call to updateDeviceValues.
    TimeStamp last_update_values;
 
    TimeStamp last_effect_change;
 
    HAPI::HAPIHapticsRenderer::Contacts last_contacts;
 
    bool error_msg_printed;
 
    // Used to set the haptics renderer for a layer.
    void setHapticsRenderer( unsigned int layer );
 
  public:
    class H3DAPI_API TrackerAngularVelocity: 
      public TypedField< SFVec3f, Types< SFRotation, SFVec3f > > {
    protected:
 
      virtual void update() {
        H3DHapticsDevice *hd = static_cast< H3DHapticsDevice *>(owner);
        Rotation cal;
        if( hd->followViewpoint->getValue() ) {
          cal = hd->adjustedOrnCalibration->getValue();
        } else {
          cal  = static_cast< SFRotation * >( routes_in[0] )->getValue();
        }
 
        value = cal * static_cast< SFVec3f * >( routes_in[1] )->getValue();
      }
  
    };
 
    H3DUniquePtr< SFVec3f > deviceAngularVelocity;
 
    H3DUniquePtr< TrackerAngularVelocity > trackerAngularVelocity;
  };
}
 
#endif