imstkTrackingDeviceControl.h

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#pragma once

#include "imstkDeviceControl.h"
#include "imstkMath.h"

namespace imstk
{
class TrackingDeviceControl : public DeviceControl
{
public:
    enum InvertFlag
    {
        transX = 0x01,
        transY = 0x02,
        transZ = 0x04,
        rotX = 0x08,
        rotY = 0x10,
        rotZ = 0x20
    };

protected:
    TrackingDeviceControl(const std::string& name = "TrackingDeviceControl");

public:
    ~TrackingDeviceControl() override = default;

    virtual void applyForces() { }

    const Vec3d& getPosition() const;
    void setPosition(const Vec3d& pos);

    const Quatd& getOrientation() const;;
    void setOrientation(const Quatd& orientation);

    void setComputeVelocity(const bool computeVelocity);
    bool getComputeVelocity() const;

    void setComputeAngularVelocity(const bool computeAngularVelocity);
    bool getComputeAngularVelocity() const;

    const Vec3d& getAngularVelocity() const;
    void setAngularVelocity(const Vec3d& angularVelocity);

    const Vec3d& getVelocity() const;
    void setVelocity(const Vec3d& velocity);

    double getTranslationScaling() const;
    void setTranslationScaling(const double scaling);

    const Vec3d& getTranslationOffset() const;
    void setTranslationOffset(const Vec3d& t);

    const Quatd& getRotationOffset();
    void setRotationOffset(const Quatd& r);

    const Quatd& getEffectorRotationOffset();
    void setEffectorRotationOffset(const Quatd& r);

    unsigned char getInversionFlags();
    void setInversionFlags(const unsigned char f);

    virtual bool updateTrackingData(const double dt);

protected:
    double m_scaling = 1.0;                                
    Vec3d  m_translationOffset      = Vec3d::Zero();       
    Quatd  m_rotationOffset         = Quatd::Identity();   
    Quatd  m_effectorRotationOffset = Quatd::Identity();   
    unsigned char m_invertFlags     = 0x00;                

    Vec3d m_currentPos = Vec3d::Zero();
    Quatd m_currentOrientation     = Quatd::Identity();
    Vec3d m_currentVelocity        = Vec3d::Zero();
    Vec3d m_currentAngularVelocity = Vec3d::Zero();

    Vec3d m_currentDisplacement = Vec3d::Zero();
    Quatd m_currentRotation     = Quatd::Identity();

    bool m_computeVelocity = false;
    bool m_computeAngularVelocity = false;
};
} // namespace imstk