polygon.cpp

Go to the documentation of this file.

// #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Partition_traits_2.h>
#include <CGAL/Partition_is_valid_traits_2.h>
#include <CGAL/polygon_function_objects.h>
#include <CGAL/partition_2.h>

#include <se2ez/core/geometry.h>
#include <se2ez/core/cgal/polygon.h>

using namespace se2ez;

namespace cgal
{
    // using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
    using Kernel = CGAL::Simple_cartesian<double>;
    using PartitionTraits = CGAL::Partition_traits_2<Kernel>;
    using ValidityTraits =
        CGAL::Partition_is_valid_traits_2<PartitionTraits, CGAL::Is_convex_2<PartitionTraits>>;

    using Point = PartitionTraits::Point_2;
    using Polygon = PartitionTraits::Polygon_2;
}  // namespace cgal

namespace
{
    void toCGAL(cgal::Polygon &polygon, const Geometry &geometry)
    {
        polygon.clear();

        if (!(geometry.type == Geometry::CONVEX || geometry.type == Geometry::SIMPLE))
            throw std::runtime_error("Geometry must be a polygon!");

        for (const auto &point : geometry.points)
            polygon.push_back(cgal::Point(point[0], point[1]));
    }

    void fillPoints(tf::EigenVector<Eigen::Vector2d> &points, const cgal::Polygon &polygon)
    {
        for (auto it = polygon.vertices_begin(); it != polygon.vertices_end(); ++it)
            points.emplace_back(it->x(), it->y());
    }

    GeometryPtr toSE2EZConvex(const cgal::Polygon &polygon)
    {
        tf::EigenVector<Eigen::Vector2d> points;
        fillPoints(points, polygon);

        return std::make_shared<Geometry>(Geometry::CONVEX, points, Eigen::Vector3d{0, 0, 0});
    }

    bool collinear(const tf::EigenVector<Eigen::Vector2d> &points)
    {
        bool collinear = false;
        for (unsigned int i = 0; i < points.size() - 2 && !collinear; ++i)
        {
            cgal::Point p(points[i][0], points[i][1]);
            cgal::Point q(points[i + 1][0], points[i + 1][1]);
            cgal::Point r(points[i + 2][0], points[i + 2][1]);

            collinear |= CGAL::collinear(p, q, r);
        }

        return collinear;
    }
}  // namespace

bool geo::isConvex(const Geometry &geometry)
{
    cgal::Polygon poly;
    toCGAL(poly, geometry);
    return poly.is_convex();
}

bool geo::isSimple(const Geometry &geometry)
{
    cgal::Polygon poly;
    toCGAL(poly, geometry);
    return poly.is_simple();
}

void geo::makeClockwise(Geometry &geometry)
{
    cgal::Polygon poly;
    toCGAL(poly, geometry);

    if (poly.is_counterclockwise_oriented())
        poly.reverse_orientation();

    geometry.points.clear();
    fillPoints(geometry.points, poly);
}

void geo::makeCounterClockwise(Geometry &geometry)
{
    cgal::Polygon poly;
    toCGAL(poly, geometry);

    if (poly.is_clockwise_oriented())
        poly.reverse_orientation();

    geometry.points.clear();
    fillPoints(geometry.points, poly);
}

double geo::area(const Geometry &geometry)
{
    cgal::Polygon poly;
    toCGAL(poly, geometry);
    return poly.area();
}

std::vector<GeometryPtr> geo::convexifyApproximate(const GeometryPtr &geometry)
{
    cgal::Polygon poly;
    std::vector<cgal::Polygon> partitions;

    cgal::PartitionTraits pt;
    cgal::ValidityTraits vt;

    toCGAL(poly, *geometry);

    CGAL::approx_convex_partition_2(poly.vertices_begin(), poly.vertices_end(),
                                    std::back_inserter(partitions), pt);

    bool valid = CGAL::partition_is_valid_2(poly.vertices_begin(), poly.vertices_end(),  //
                                            partitions.begin(), partitions.end(), vt);

    if (!valid)
        throw std::runtime_error("Invalid convex partition found!");

    std::vector<GeometryPtr> convert;
    for (const auto &p : partitions)
        convert.emplace_back(toSE2EZConvex(p));

    return convert;
}

// TODO: Template out

std::vector<GeometryPtr> geo::convexifyOptimal(const GeometryPtr &geometry)
{
    cgal::Polygon poly;
    std::vector<cgal::Polygon> partitions;

    cgal::PartitionTraits pt;
    cgal::ValidityTraits vt;

    toCGAL(poly, *geometry);

    CGAL::optimal_convex_partition_2(poly.vertices_begin(), poly.vertices_end(),
                                     std::back_inserter(partitions), pt);

    bool valid = CGAL::partition_is_valid_2(poly.vertices_begin(), poly.vertices_end(),  //
                                            partitions.begin(), partitions.end(), vt);

    if (!valid)
        throw std::runtime_error("Invalid convex partition found!");

    std::vector<GeometryPtr> convert;
    for (const auto &p : partitions)
        convert.emplace_back(toSE2EZConvex(p));

    return convert;
}

std::vector<GeometryPtr> geo::shatter(const GeometryPtr &geometry)
{
    if (geometry->type != Geometry::CONVEX)
        throw std::runtime_error("Geometry must be convex!");

    std::vector<GeometryPtr> triangles;
    tf::EigenVector<Eigen::Vector2d> points(3);
    points[0] = geometry->points[0];
    for (unsigned int i = 1; i < geometry->points.size() - 1; ++i)
    {
        points[1] = geometry->points[i];
        points[2] = geometry->points[i + 1];

        if (collinear(points))
            continue;

        triangles.emplace_back(
            std::make_shared<Geometry>(Geometry::CONVEX, points, geometry->offset, geometry->color));
    }

    return triangles;
}