FramedOrientedPoint.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author     Martin Miller (martin dot miller at student dot kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "FramedOrientedPoint.h"
 
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
namespace armarx
{
    FramedOrientedPoint::FramedOrientedPoint()
        = default;
 
    FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        OrientedPointBase(source),
        FramedOrientedPointBase(source),
        OrientedPoint(source)
    {
    }
 
    FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent) :
        OrientedPoint(position, normal)
    {
        this->frame = frame;
        this->agent = agent;
 
    }
 
    FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent) :
        FramedOrientedPoint(pointWithNormal.positionToEigen(), pointWithNormal.normalToEigen(), frame, agent)
    {
    }
 
    FramedOrientedPoint::FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent) :
        OrientedPoint(px, py, pz, nx, ny, nz)
    {
        this->frame = frame;
        this->agent = agent;
    }
 
    std::string FramedOrientedPoint::getFrame() const
    {
        return frame;
    }
 
 
    void FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
    {
        FramedPosition  framedPos(positionToEigen(), frame, agent);
        FramedDirection framedDir(normalToEigen(), frame, agent);
 
        framedPos.changeFrame(robot, newFrame);
        framedDir.changeFrame(robot, newFrame);
        this->px = framedPos.x;
        this->py = framedPos.y;
        this->pz = framedPos.z;
 
        this->nx = framedDir.x;
        this->ny = framedDir.y;
        this->nz = framedDir.z;
    }
 
    void FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
    {
        VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
        changeFrame(sharedRobot, GlobalFrame);
    }
 
    void FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
    {
        changeFrame(referenceRobot, GlobalFrame);
    }
 
    Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedPosition().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedNormal().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedPosition().toGlobalEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedNormal().toGlobalEigen(referenceRobot);
    }
 
 
    FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
    }
 
    FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
    }
 
    Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedPosition().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
    {
        return getFramedNormal().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedPosition().toRootEigen(referenceRobot);
    }
 
    Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
    {
        return getFramedNormal().toRootEigen(referenceRobot);
    }
 
    FramedPosition FramedOrientedPoint::getFramedPosition() const
    {
        return FramedPosition(positionToEigen(), frame, agent);
    }
 
    FramedDirection FramedOrientedPoint::getFramedNormal() const
    {
        return FramedDirection(normalToEigen(), frame, agent);
    }
 
 
    std::string FramedOrientedPoint::output(const Ice::Current& c) const
    {
        std::stringstream s;
        s << OrientedPoint::output(c) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
        return s.str();
    }
 
 
    void FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        OrientedPoint::serialize(serializer);
        obj->setString("frame", frame);
        obj->setString("agent", agent);
 
    }
 
    void FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        OrientedPoint::deserialize(serializer);
        frame = obj->getString("frame");
 
        if (obj->hasElement("agent"))
        {
            agent = obj->getString("agent");
        }
    }
}