MovingSquareForce.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_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H
#define SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H

#include <string>

#include "SurgSim/Input/InputConsumerInterface.h"
#include "SurgSim/Input/OutputProducerInterface.h"
#include "SurgSim/DataStructures/DataGroup.h"

#include "SurgSim/Framework/LockedContainer.h"


class MovingSquareForce : public SurgSim::Input::InputConsumerInterface, public SurgSim::Input::OutputProducerInterface
{
public:
    MovingSquareForce(const std::string& toolDeviceName, const std::string& squareDeviceName);

    void initializeInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;

    void handleInput(const std::string& device, const SurgSim::DataStructures::DataGroup& inputData) override;

    bool requestOutput(const std::string& device, SurgSim::DataStructures::DataGroup* outputData) override;

protected:
    void updateTool(const SurgSim::DataStructures::DataGroup& toolInputData);

    void updateSquare(const SurgSim::DataStructures::DataGroup& squareInputData);

    SurgSim::Math::Vector3d computeForce(const SurgSim::Math::Vector3d& position);

private:
    struct SquarePoseVectors
    {
        SquarePoseVectors();

        SurgSim::Math::Vector3d normal;
        SurgSim::Math::Vector3d edgeDirectionX;
        SurgSim::Math::Vector3d edgeDirectionY;
        SurgSim::Math::Vector3d center;
    };


    const std::string m_toolDeviceName;
    const std::string m_squareDeviceName;

    SurgSim::DataStructures::DataGroup m_outputData;

    double m_squareHalfSize;
    double m_surfaceStiffness;
    double m_forceLimit;

    SurgSim::Framework::LockedContainer<SquarePoseVectors> m_square;
    double m_squareNormalDirection;

    SurgSim::Math::Vector3d m_tipPoint;
};

#endif // SURGSIM_TESTING_VISUALTESTCOMMON_MOVINGSQUAREFORCE_H