imstkLaparoscopicToolController.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 "imstkTrackingDeviceControl.h"

namespace imstk
{
class CollidingObject;
class Geometry;

class LaparoscopicToolController : public TrackingDeviceControl
{
public:
    enum class JawState
    {
        Closed,
        Opened
    };

public:
    LaparoscopicToolController(const std::string& name = "LaparoscopicToolController") : TrackingDeviceControl(name) { }
    ~LaparoscopicToolController() override = default;

    void setParts(
        std::shared_ptr<CollidingObject> shaft,
        std::shared_ptr<CollidingObject> upperJaw,
        std::shared_ptr<CollidingObject> lowerJaw,
        std::shared_ptr<Geometry>        pickGeom);

    void setDevice(std::shared_ptr<DeviceClient> device) override;

    // *INDENT-OFF*
    SIGNAL(LaparoscopicToolController, JawClosed);

    SIGNAL(LaparoscopicToolController, JawOpened);
    // *INDENT-ON*

public:
    void update(const double& dt) override;

    void setMaxJawAngle(const double maxAngle) { m_maxJawAngle = maxAngle; }

    void setJawAngleChange(const double dAngle) { m_change = dAngle; }

    void setJawRotationAxis(const Vec3d& axis) { m_jawRotationAxis = axis; }

    double getJawAngle() const { return m_jawAngle; }

    double getMaxJawAngle() const { return m_maxJawAngle; }

    JawState getJawState() const { return m_jawState; }

protected:
    std::shared_ptr<CollidingObject> m_shaft;               
    std::shared_ptr<CollidingObject> m_upperJaw;            
    std::shared_ptr<CollidingObject> m_lowerJaw;            
    std::shared_ptr<Geometry> m_pickGeom;

    double   m_jawAngle    = PI / 6.0;                      
    double   m_change      = 6.0e-5;                        
    double   m_maxJawAngle = PI / 6.0;                      
    JawState m_jawState    = JawState::Opened;

    Vec3d m_jawRotationAxis = Vec3d(1.0, 0.0, 0.0);         

    Mat4d m_controllerWorldTransform = Mat4d::Identity();   // Final world transform of the controller
    Mat4d m_pickGeomTransform = Mat4d::Identity();

    Mat4d m_shaftVisualTransform    = Mat4d::Identity();    // Initial local transform of the visual shaft
    Mat4d m_upperJawVisualTransform = Mat4d::Identity();    // Initial local transform of the visual upper jaw
    Mat4d m_lowerJawVisualTransform = Mat4d::Identity();    // Initial local transform of the visual lower jaw

    Mat4d m_shaftCollidingTransform    = Mat4d::Identity(); // Initial local transform of the colliding shaft
    Mat4d m_upperJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding upper jaw
    Mat4d m_lowerJawCollidingTransform = Mat4d::Identity(); // Initial local transform of the colliding lower jaw

    Mat4d m_upperJawLocalTransform = Mat4d::Identity();     // upperJawWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisual/CollidingTransform
    Mat4d m_lowerJawLocalTransform = Mat4d::Identity();     // lowerJawWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawVisual/CollidingTransform
};
} // namespace imstk