TPolygon2D.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "math-precomp.h"  // Precompiled headers

#include <mrpt/math/TLine2D.h>
#include <mrpt/math/TPolygon2D.h>
#include <mrpt/math/TPolygon3D.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/TSegment2D.h>
#include <mrpt/math/epsilon.h>
#include "polygons_utils.h"

using namespace mrpt::math;

double TPolygon2D::distance(const TPoint2D& point) const
{
    if (contains(point)) return 0;
    std::vector<TSegment2D> sgs;
    getAsSegmentList(sgs);

    if (sgs.empty())
        THROW_EXCEPTION("Cannot compute distance to an empty polygon.");

    double distance = std::numeric_limits<double>::max();

    for (auto it = sgs.begin(); it != sgs.end(); ++it)
    {
        double d = (*it).distance(point);
        if (d < distance) distance = d;
    }
    return distance;
}

void TPolygon2D::getBoundingBox(
    TPoint2D& min_coords, TPoint2D& max_coords) const
{
    ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
    min_coords.x = min_coords.y = std::numeric_limits<double>::max();
    max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
    for (size_t i = 0; i < size(); i++)
    {
        mrpt::keep_min(min_coords.x, (*this)[i].x);
        mrpt::keep_min(min_coords.y, (*this)[i].y);
        mrpt::keep_max(max_coords.x, (*this)[i].x);
        mrpt::keep_max(max_coords.y, (*this)[i].y);
    }
}

// isLeft(): tests if a point is Left|On|Right of an infinite line.
//    Input:  three points P0, P1, and P2
//    Return: >0 for P2 left of the line through P0 and P1
//            =0 for P2  on the line
//            <0 for P2  right of the line
//    See: Algorithm 1 "Area of Triangles and Polygons"
inline double isLeft(
    const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
    const mrpt::math::TPoint2D& P2)
{
    return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
}

bool TPolygon2D::contains(const TPoint2D& P) const
{
    int wn = 0;  // the  winding number counter

    // loop through all edges of the polygon
    const size_t n = this->size();
    for (size_t i = 0; i < n; i++)  // edge from V[i] to  V[i+1]
    {
        if ((*this)[i].y <= P.y)
        {
            // start y <= P.y
            if ((*this)[(i + 1) % n].y > P.y)  // an upward crossing
                if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
                    0)  // P left of  edge
                    ++wn;  // have  a valid up intersect
        }
        else
        {
            // start y > P.y (no test needed)
            if ((*this)[(i + 1) % n].y <= P.y)  // a downward crossing
                if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
                    0)  // P right of  edge
                    --wn;  // have  a valid down intersect
        }
    }

    return wn != 0;
}
void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
{
    size_t N = size();
    v.resize(N);
    for (size_t i = 0; i < N - 1; i++)
        v[i] = TSegment2D(operator[](i), operator[](i + 1));
    v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
}

void TPolygon2D::generate3DObject(TPolygon3D& p) const
{
    p = TPolygon3D(*this);
}
void TPolygon2D::getCenter(TPoint2D& p) const
{
    for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
    size_t N = size();
    p.x /= N;
    p.y /= N;
}
bool TPolygon2D::isConvex() const
{
    size_t N = size();
    if (N <= 3) return true;
    vector<TSegment2D> sgms;
    getAsSegmentList(sgms);
    for (size_t i = 0; i < N; i++)
    {
        char s = 0;
        auto l = TLine2D(sgms[i]);
        for (size_t j = 0; j < N; j++)
        {
            double d = l.evaluatePoint(operator[](j));
            if (std::abs(d) < getEpsilon())
                continue;
            else if (!s)
                s = (d > 0) ? 1 : -1;
            else if (s != ((d > 0) ? 1 : -1))
                return false;
        }
    }
    return true;
}
void TPolygon2D::removeRepeatedVertices() { removeRepVertices(*this); }
void TPolygon2D::removeRedundantVertices()
{
    removeRepeatedVertices();
    removeUnusedVertices(*this);
}
void TPolygon2D::getPlotData(
    std::vector<double>& x, std::vector<double>& y) const
{
    size_t N = size();
    x.resize(N + 1);
    y.resize(N + 1);
    for (size_t i = 0; i < N; i++)
    {
        x[i] = operator[](i).x;
        y[i] = operator[](i).y;
    }
    x[N] = operator[](0).x;
    y[N] = operator[](0).y;
}
TPolygon2D::TPolygon2D(const TPolygon3D& p) : std::vector<TPoint2D>()
{
    size_t N = p.size();
    resize(N);
    for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
}
void TPolygon2D::createRegularPolygon(
    size_t numEdges, double radius, TPolygon2D& poly)
{
    if (numEdges < 3 || std::abs(radius) < getEpsilon())
        throw std::logic_error(
            "Invalid arguments for regular polygon creations");
    poly.resize(numEdges);
    for (size_t i = 0; i < numEdges; i++)
    {
        double angle = i * M_PI * 2 / numEdges;
        poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
    }
}

inline void TPolygon2D::createRegularPolygon(
    size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
{
    createRegularPolygon(numEdges, radius, poly);
    for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
}