geometry.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

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

using namespace se2ez;

const std::string Geometry::TYPE_STRINGS[] = {"box", "circle", "convex", "simple"};

Geometry::Type Geometry::stringToType(const std::string &type)
{
    for (unsigned int i = 0; i <= TYPE_MAX; ++i)
        if (type == TYPE_STRINGS[i])
            return static_cast<Type>(i);

    throw std::invalid_argument("Not a valid Geometry  type");
}

const std::string &Geometry::typeToString(const Geometry::Type &type)
{
    if (type <= TYPE_MAX && type >= 0)
        return TYPE_STRINGS[type];

    throw std::invalid_argument("Not a valid Geometry type");
}

Geometry::Geometry(const GeometryPtr &g)
  : type(g->type), dimensions(g->dimensions), points(g->points), offset(g->offset), color(g->color)
{
}

Geometry::Geometry(Type type, const Eigen::VectorXd &dimensions, const Eigen::Isometry2d &offset,
                   const Eigen::Vector4d &color)
  : type(type), dimensions(dimensions), offset(offset), isOffset(tf::transformDistance(offset) > 1e-8), color(color)
{
    switch (type)
    {
        case BOX:
            if (dimensions.size() != 2)
                throw std::runtime_error("Wrong number of dimensions for box!");
            break;
        case CIRCLE:
            if (dimensions.size() != 1)
                throw std::runtime_error("Wrong number of dimensions for circle!");
            break;
        case CONVEX:
        case SIMPLE:
            throw std::runtime_error("Wrong constructor for shape type!");
        default:
            throw std::runtime_error("Shape type not implemented!");
    }
}

Geometry::Geometry(Type type, const Eigen::VectorXd &dimensions, const Eigen::Vector3d &offset,
                   const Eigen::Vector4d &color)
  : Geometry(type, dimensions, tf::toIsometry(offset), color)
{
}

Geometry::Geometry(Type type, const tf::EigenVector<Eigen::Vector2d> &points, const Eigen::Isometry2d &offset,
                   const Eigen::Vector4d &color)
  : type(type), points(points), offset(offset), isOffset(tf::transformDistance(offset) > 1e-8), color(color)
{
    if (points.size() < 3)
        throw std::runtime_error("Polygon must have at least 3 points!");

    // For convention, we counterclockwise order the vertices.
    switch (type)
    {
        case CONVEX:
            if (!geo::isConvex(*this))
                throw std::runtime_error("Polygon is not convex!");

            geo::makeCounterClockwise(*this);
            break;

        case SIMPLE:
            if (!geo::isSimple(*this))
                throw std::runtime_error("Polygon is not simple!");

            geo::makeCounterClockwise(*this);
            break;

        case BOX:
        case CIRCLE:
            throw std::runtime_error("Wrong constructor for shape type!");
        default:
            throw std::runtime_error("Shape type not implemented!");
    }
}

Geometry::Geometry(Type type, const tf::EigenVector<Eigen::Vector2d> &points, const Eigen::Vector3d &offset,
                   const Eigen::Vector4d &color)
  : Geometry(type, points, tf::toIsometry(offset), color)
{
}

std::string Geometry::printGeometry() const
{
    std::stringstream ss;
    ss << typeToString(type) << tf::printFrame(offset);

    switch (type)
    {
        case BOX:
        case CIRCLE:
            ss << tf::printVector(dimensions);
            break;
        case CONVEX:
        case SIMPLE:
            for (const auto &p : points)
                ss << "(" << p(0) << "," << p(1) << ")";
            break;
        default:
            throw std::runtime_error("Shape type not implemented!");
    }

    return ss.str();
}