geometry.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <se2ez/core.h>

using namespace se2ez;

int main(int /*argc*/, char ** /*argv*/)
{
    // tf::EigenVector<Eigen::Vector2d> points = {
    //     {0.0, 0.0}, {0.5, 0.0}, {1.0, 0.0}, {1.5, 0.0}, {2.0, 0.0}, {2.0, 0.5}, {2.0, 1.0}, {2.0, 1.5},
    //     {2.0, 2.0}, {1.5, 2.0}, {1.0, 2.0}, {0.5, 2.0}, {0.0, 2.0}, {0.0, 1.5}, {0.0, 1.0}, {0.0, 0.5},
    // };

    tf::EigenVector<Eigen::Vector2d> points = {
        {391, 374}, {240, 431}, {252, 340}, {374, 320}, {289, 214}, {134, 390}, {68, 186},
        {154, 259}, {161, 107}, {435, 108}, {208, 148}, {295, 160}, {421, 212}, {441, 303},
    };

    auto geometry = std::make_shared<Geometry>(Geometry::SIMPLE, points, Eigen::Vector3d{0, 0, 0});
    std::cout << "Original:" << std::endl;
    std::cout << geometry->printGeometry() << std::endl;
    std::cout << std::endl;

    auto partitions = geo::convexifyOptimal(geometry);
    for (const auto &convex : partitions)
    {
        std::cout << "Convex Partition:" << std::endl;
        std::cout << convex->printGeometry() << std::endl;
        std::cout << std::endl;

        auto triangles = geo::shatter(convex);
        std::cout << "Shattered:" << std::endl;
        for (const auto &tri : triangles)
            std::cout << tri->printGeometry() << std::endl;
        std::cout << std::endl;
    }

    return 0;
}