VirtualCouplingEffectorFilter.h

Go to the documentation of this file.

#ifndef VIRTUAL_COUPLING_EFFECTOR_FILTER_H
#define VIRTUAL_COUPLING_EFFECTOR_FILTER_H

#include <vreckoDP/PHANToM/PHANToM_Device.h>

#ifdef COMPILE_PHANTOM_DEVICE

namespace vreckoDP_PHANToM {


//#define PHANTOM_VCEF_MAX_DERIVATIVE       2
    // 0: position
    // 1st: velocity
    // 2nd: acceleration


/* PHANToM filter to perform the virtual coupling -> drag the phantom handle to the object */
class VirtualCouplingEffectorFilter : public PHANToMFilter {
public:
    VirtualCouplingEffectorFilter();

    virtual bool loadXMLParameters(XERCES_CPP_NAMESPACE_QUALIFIER DOMNode *parameters);
    virtual void apply(PHANToMSTATE &pState, osg::Vec3 &force, osg::Vec3 &torque);
protected:

    template<class _Type, int _MaxData> class OldDataStorage {
    public:
        _Type data[_MaxData];
            // 0 = current
            // 1 = 1-frame old data
            // 2 = 2-frames old data
            // etc.

        inline void getMaxData() { return _MaxData; }
        
        inline _Type &getValue() { return data[0]; }
        inline const _Type &getValue() const { return data[0]; }

        inline _Type getVelocity() { return (data[0] - data[1]); }
        inline _Type getAccel() { return (data[0] - 2 * data[1] + data[2]); }

        inline void fillWithSingleValue(_Type& newData) {
            for (int i = 0; i < _MaxData; ++i)
                data[i] = newData;
        }

        inline void addNew(_Type& newData) {
            for (int i = _MaxData - 1; i > 1; --i)
                data[i] = data[i - 1];
            data[0] = newData;
        }


    };

    template<class _Type, int _MaxData> class StorageWithDamping : public OldDataStorage<_Type, _MaxData> {
    public:
        StorageWithDamping(float _springDamping, float _velocityDamping) : OldDataStorage() {
            springDamping = _springDamping;
            velocityDamping = _velocityDamping;
        }

        void dampAndStoreValue(_Type *value) {
            *value = (*value * springDamping + getVelocity() * velocityDamping);
            addNew(*value);
        }

        void setDamping(float newSpringDamping, float newVelocityDamping) {
            springDamping = newSpringDamping;
            velocityDamping = newVelocityDamping;
        }

        float getSpringDamping() { return springDamping; }
        float getVelocityDamping() { return velocityDamping; }

    protected:
        float springDamping;
        float velocityDamping;
    };

    unsigned int realPhantomPosObjectID; // secondary (optional) object, that shows the real phantom position

    DynamicArray<osg::Matrix> originalScales; // original scaling of the held objects (TODO - probably not necessary, remove)
//  osg::Matrix cachedPreTransform; // scaling and center shifting at once - TODO

    osg::Vec3 offsetFromCenter; // Offset of the phantom reference point

    OldDataStorage<osg::Vec3, 4> lastRealPos;
    OldDataStorage<osg::Vec3, 4> lastVirtualPos;
        // RealPos and VirtualPos are shifted from the "Phantom".pPosition by [offsetFromCenter] * "rotation"

    OldDataStorage<osg::Quat, 4> lastRealRot;
    OldDataStorage<osg::Quat, 4> lastVirtualRot;

    void inverseQuadraticScale(osg::Vec3 *valueToRescale, float inputMaxLength, float outputMaxLength);
        // Recalculates linear interpolation to inverse quadratic
        // If the input vector is longer than [inputMaxLength], it will be shortened.
        // If it is smaller, it will be rescaled.

    void linearScale(osg::Vec3 *valueToRescale, float inputMaxLength, float outputMaxLength);
        // If the input vector is longer than [inputMaxLength], it will be shortened.
        // If it is smaller, it will be rescaled.

    void forceScale(osg::Vec3 *valueToRescale, float inputMaxLength, float outputMaxLength);

    float virtToolWeight;

    // dynamic parameters
    osg::Vec3 virtToolLinMomentum; // (kg?) . (distance unit) / SECOND  (not millisecond)
    osg::Vec3 virtToolAngMomentum;

    // fixed parameters, can be changed in the config file
    float fSpringLinDamping;
    float fSpringLinFriction;
    float fSpringAngDamping;
    float fSpringAngFriction;
    
    float fDragLinForceTool; // Translational force that drags to tool to the user pos
    float fDragLinDistTool; //      ... and distance
    float fDragLinForceUser; // Translational force that drags user to the tool pos
    float fDragLinDistUser; //      ... and distance

    float fDragAngForceTool; // Rotational force force that drags to tool to the user pos
    float fDragAngForceUser; // Translational force that drags user to the tool pos

    float fMaxLinDistTool; // maximum distance, that the virtual tool is allowed to move in one haptic frame
                            // (1/1000 s OR LONGER if the processing takes too long)
    float fMaxAngDistTool; // maximum distance, that the virtual tool is allowed to rotate in one haptic frame

    float fDistanceToReset; // Distance between two (haptic) frames that will cause the forces to be reseted

    float fNoMoveTime; // [in s] Time after Reset in which the moved object remain motionless and the user is dragged to its position

    bool bPauseIfColStructsAreNotReady;
        // If the virtual coupling detects that the held object(s) contain structures that are still being created (in different thread)
        // the simulation will fix the held object(s) and disable any movement.
        // NOTE: It only supports certain collision structures, support for other structures have to be added into this Ability.

    // Used for sticky materials
    int computeCollisionThroughtLastXFrame;
    int collisionsFrontPosition;
    bool collisionsFront[5000];
    bool collisionsOccuredLastXFrame(int X);
    void pushCollisionToFront(bool value);

    bool bStickyObjectsEnabled;
    float fStickyObjectsDistance;

    // Used for damping
    struct dampingValues {
        osg::Vec3f force;
        float fRotAngle;
    };

    int iDampingValuesX;
    int iDampingEndValuesX;
    dampingValues dampingField[5000];
    int iDampingValuesFieldPosition;
    void getAvarageLastXDampingValues(osg::Vec3f *force, float *angle);
    float getAvarageLastXDampingValuesRotAngle(int count);
    void pushValuesToDamping(osg::Vec3f force, float fRotAngle);

};

}

#endif

#endif