VirtualToolCoupler.h

// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H
#define SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H

#include <memory>

#include "SurgSim/DataStructures/DataGroup.h"
#include "SurgSim/DataStructures/DataGroupBuilder.h"
#include "SurgSim/DataStructures/OptionalValue.h"
#include "SurgSim/Framework/Behavior.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Framework/ObjectFactory.h"

namespace SurgSim
{

namespace Input
{
class InputComponent;
class OutputComponent;
}

namespace Physics
{

class RigidRepresentation;

SURGSIM_STATIC_REGISTRATION(VirtualToolCoupler);

class VirtualToolCoupler : public Framework::Behavior
{
public:
    explicit VirtualToolCoupler(const std::string& name);

    ~VirtualToolCoupler();

    SURGSIM_CLASSNAME(SurgSim::Physics::VirtualToolCoupler);

    const std::shared_ptr<Framework::Component> getInput();

    void setInput(const std::shared_ptr<Framework::Component> input);

    const std::shared_ptr<Framework::Component> getOutput();

    void setOutput(const std::shared_ptr<Framework::Component> output);

    const std::shared_ptr<Framework::Component> getRepresentation();

    void setRepresentation(const std::shared_ptr<Framework::Component> rigid);

    bool isHapticOutputOnlyWhenColliding() const;

    void setHapticOutputOnlyWhenColliding(bool haptic);

    const std::string& getPoseName();

    void setPoseName(const std::string& poseName = DataStructures::Names::POSE);

    void update(double dt) override;

    void overrideLinearStiffness(double linearStiffness);

    double getLinearStiffness();

    void overrideLinearDamping(double linearDamping);

    double getLinearDamping();

    void overrideAngularStiffness(double angularStiffness);

    double getAngularStiffness();

    void overrideAngularDamping(double angularDamping);

    double getAngularDamping();

    void overrideAttachmentPoint(const Math::Vector3d& attachment);

    const Math::Vector3d& getAttachmentPoint();

    void setCalculateInertialTorques(bool calculateInertialTorques);

    bool getCalculateInertialTorques() const;

    void setInitializeRigidWithInputPose(bool val);

    bool isInitializingRigidWithInputPose() const;

    void doRetire() override;

    void setLocalActive(bool val) override;

protected:
    bool doInitialize() override;
    bool doWakeUp() override;
    int getTargetManagerType() const override;
    void setInitialInputPose(const SurgSim::Math::RigidTransform3d& pose);

    virtual DataStructures::DataGroup buildOutputData();

    void setOptionalLinearStiffness(const DataStructures::OptionalValue<double>& linearStiffness);

    const DataStructures::OptionalValue<double>& getOptionalLinearStiffness() const;

    void setOptionalLinearDamping(const DataStructures::OptionalValue<double>& linearDamping);

    const DataStructures::OptionalValue<double>& getOptionalLinearDamping() const;

    void setOptionalAngularStiffness(const DataStructures::OptionalValue<double>& angularStiffness);

    const DataStructures::OptionalValue<double>& getOptionalAngularStiffness() const;

    void setOptionalAngularDamping(const DataStructures::OptionalValue<double>& angularDamping);

    const DataStructures::OptionalValue<double>& getOptionalAngularDamping() const;

    void setOptionalAttachmentPoint(const DataStructures::OptionalValue<Math::Vector3d>& attachmentPoint);

    const DataStructures::OptionalValue<Math::Vector3d>& getOptionalAttachmentPoint() const;

    DataStructures::OptionalValue<double> m_optionalLinearStiffness;

    DataStructures::OptionalValue<double> m_optionalLinearDamping;

    DataStructures::OptionalValue<double> m_optionalAngularStiffness;

    DataStructures::OptionalValue<double> m_optionalAngularDamping;

    DataStructures::OptionalValue<Math::Vector3d> m_optionalAttachmentPoint;

    DataStructures::DataGroup m_outputData;

    std::shared_ptr<Input::InputComponent> m_input;

    std::shared_ptr<Input::OutputComponent> m_output;

private:
    std::shared_ptr<RigidRepresentation> m_rigid;
    std::string m_poseName;

    double m_linearStiffness;

    double m_linearDamping;

    double m_angularStiffness;

    double m_angularDamping;

    Math::Vector3d m_localAttachmentPoint;

    bool m_calculateInertialTorques;

    bool m_initializeRigidWithInputPose;

    std::shared_ptr<Framework::Logger> m_logger;

    bool m_hapticOutputOnlyWhenColliding;

    Math::RigidTransform3d m_previousInputPose;

    int m_poseIndex;
    int m_linearVelocityIndex;
    int m_angularVelocityIndex;
    int m_forceIndex;
    int m_torqueIndex;
    int m_inputLinearVelocityIndex;
    int m_inputAngularVelocityIndex;
    int m_inputPoseIndex;
    int m_springJacobianIndex;
    int m_damperJacobianIndex;
};

}; // Physics

}; // SurgSim

#endif // SURGSIM_PHYSICS_VIRTUALTOOLCOUPLER_H