geometry.h

Go to the documentation of this file.

#pragma once
 
// TODO To be moved to Simox after ROBDEKON demo
 
#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/within/interface.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/strategies/strategies.hpp>
 
#include <VirtualRobot/MathTools.h>
 
BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2f, float, cs::cartesian, x(), y())
 
namespace armarx::geometry
{
    namespace bg = boost::geometry;
 
    class ConvexHull
    {
    public:
        using Point = bg::model::d2::point_xy<float>;
 
        ConvexHull(const VirtualRobot::MathTools::ConvexHull2D& hull) : ConvexHull(hull.vertices)
        {
        }
 
        ConvexHull(const std::vector<Eigen::Vector2f>& hull)
        {
            boost::geometry::assign_points(polygon, hull);
        }
 
        bool contains(const Eigen::Vector2f& point) const
        {
            return bg::within(Point(point.x(), point.y()), polygon);
        }
 
        float area() const
        {
            return static_cast<float>(bg::area(polygon));
        }
 
    private:
        bg::model::polygon<Point> polygon;
    };
 
    // inline float area()
    // {
 
    //     return bg::area(polygon);
    // }
 
} // namespace armarx::geometry