Room.cpp

Go to the documentation of this file.

#include "Room.h"
 
#include <tuple>
 
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/detail/distance/interface.hpp>
#include <boost/geometry/algorithms/detail/envelope/interface.hpp>
#include <boost/geometry/algorithms/detail/within/interface.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/linestring.hpp>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/util/geometry.h>
 
namespace armarx::navigation::algorithms
{
 
    bool
    Room::isInside(const Eigen::Vector3f& point, const bool restrictTo2dCheck) const
    {
        const util::geometry::polygon_type poly = util::geometry::toPolygon(polygon);
        const util::geometry::point_type pt = util::geometry::toPoint(conv::to2D(point));
 
        const bool isInsideXY = boost::geometry::within(pt, poly);
 
        if (restrictTo2dCheck)
        {
            return isInsideXY;
        }
 
        const bool isInsideZ = point.z() >= zFloor and point.z() <= (zFloor + height);
        return isInsideXY and isInsideZ;
    }
 
    Eigen::Vector2f
    Room::center() const
    {
        const util::geometry::polygon_type poly = util::geometry::toPolygon(polygon);
        util::geometry::point_type center{0, 0};
 
        boost::geometry::centroid(poly, center);
 
        return util::geometry::fromPoint(center);
    }
 
    std::tuple<Eigen::Vector2f, Eigen::Vector2f>
    Room::aabb() const
    {
        const util::geometry::polygon_type poly = util::geometry::toPolygon(polygon);
        boost::geometry::model::box<util::geometry::point_type> box;
 
        boost::geometry::envelope(poly, box);
 
        return {util::geometry::fromPoint(box.min_corner()),
                util::geometry::fromPoint(box.max_corner())};
    }
 
    bool
    Room::isInside(const Eigen::Vector2f& point, const float margin) const
    {
        const util::geometry::polygon_type poly = util::geometry::toPolygon(polygon);
        const util::geometry::point_type pt = util::geometry::toPoint(point);
 
        if (not boost::geometry::within(pt, poly))
        {
            return false;
        }
        if (margin > 0.F)
        {
            const boost::geometry::model::linestring<util::geometry::point_type> boundary(
                poly.outer().begin(), poly.outer().end());
            if (boost::geometry::distance(pt, boundary) < margin)
            {
                return false;
            }
        }
        return true;
    }
 
} // namespace armarx::navigation::algorithms