Box.hpp

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/>.
*
* @package    ArmarX::
* @author     Valerij Wittenbeck ( valerij.wittenbeck at kit dot edu)
* @date       2015
* @copyLeft  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#ifndef MEMORYX_WSO_Box_H
#define MEMORYX_WSO_Box_H
 
namespace memoryx
{
 
    struct Box
    {
        Box(float xCenter,
            float yCenter,
            float zCenter,
            float xExtent,
            float yExtent,
            float zExtent)
        {
            Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
            m.block<3, 1>(0, 3) << xCenter, yCenter, zCenter;
            pose = new armarx::Pose(m);
 
            extents = new armarx::Vector3(xExtent, yExtent, zExtent);
        }
 
        armarx::PosePtr pose;
        armarx::Vector3Ptr extents;
 
        inline bool
        inside(const armarx::FramedPositionBasePtr& p) const
        {
            return (pose->position->x - extents->x / 2.f) <= p->x &&
                   p->x <= (pose->position->x + extents->x / 2.f) &&
                   (pose->position->y - extents->y / 2.f) <= p->y &&
                   p->y <= (pose->position->y + extents->y / 2.f) &&
                   (pose->position->z - extents->z / 2.f) <= p->z &&
                   p->z <= (pose->position->z + extents->z / 2.f);
        }
 
        inline void
        drawTo(const armarx::DebugDrawerInterfacePrx& debugDrawer,
               const std::string& layerName,
               const std::string& boxName,
               const armarx::DrawColor& color) const
        {
            debugDrawer->setBoxVisu(layerName, boxName, pose, extents, color);
        }
    };
 
} // namespace memoryx
 
#endif