geometry.cpp

Go to the documentation of this file.

#include "geometry.h"
 
#include <algorithm>
#include <limits>
#include <vector>
 
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/distance/interface.hpp>
 
#include <SimoxUtility/algorithm/apply.hpp>
 
namespace armarx::navigation::util::geometry
{
 
    float
    computeDistance(const std::vector<polygon_type>& obstacles, const Eigen::Vector2f& pt)
    {
        const point_type point(pt.x(), pt.y());
 
        float dist = std::numeric_limits<float>::max();
        for (const auto& obstacle : obstacles)
        {
            const float d = boost::geometry::distance(point, obstacle);
            dist = std::min(d, dist);
        }
 
        return dist;
    }
 
    polygon_type
    toPolygon(const std::vector<Eigen::Vector2f>& hull)
    {
        std::vector<point_type> points = simox::alg::apply(hull, toPoint);
        points.push_back(points.front()); // close polygon
 
        polygon_type polygon;
        boost::geometry::assign_points(polygon, points);
 
        return polygon;
    }
 
    point_type
    toPoint(const Eigen::Vector2f& pt)
    {
        return {pt.x(), pt.y()};
    }
 
    Eigen::Vector2f
    fromPoint(const point_type& pt)
    {
        return {pt.x(), pt.y()};
    }
 
} // namespace armarx::navigation::util::geometry