TriangularDecomposition.cpp

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Matt Maly */

#include "ompl/extensions/triangle/TriangularDecomposition.h"
#include "ompl/base/State.h"
#include "ompl/base/StateSampler.h"
#include "ompl/base/spaces/RealVectorBounds.h"
#include "ompl/control/planners/syclop/Decomposition.h"
#include "ompl/control/planners/syclop/GridDecomposition.h"
#include "ompl/util/RandomNumbers.h"
#include "ompl/util/Hash.h"
#include <ostream>
#include <vector>
#include <set>
#include <string>
#include <unordered_map>
#include <cstdlib>

extern "C"
{
    #define REAL double
    #define VOID void
    #define ANSI_DECLARATORS
    #include <triangle.h>
}

namespace std
{
    template<>
    struct hash<ompl::control::TriangularDecomposition::Vertex>
    {
        size_t operator()(const ompl::control::TriangularDecomposition::Vertex &v) const
        {
            std::size_t hash = std::hash<double>()(v.x);
            ompl::hash_combine(hash, v.y);
            return hash;
        }
    };
}

ompl::control::TriangularDecomposition::TriangularDecomposition(const base::RealVectorBounds &bounds,
    const std::vector<Polygon> &holes, const std::vector<Polygon> &intRegs) :
    Decomposition(2, bounds),
    holes_(holes),
    intRegs_(intRegs),
    triAreaPct_(0.005),
    locator(64, this)
{
    // \todo: Ensure that no two holes overlap and no two regions of interest overlap.
    // Report an error otherwise.
}

ompl::control::TriangularDecomposition::~TriangularDecomposition(void)
{
}

void ompl::control::TriangularDecomposition::setup(void)
{
    int numTriangles = createTriangles();
    OMPL_INFORM("Created %u triangles", numTriangles);
    buildLocatorGrid();
}

void ompl::control::TriangularDecomposition::addHole(const Polygon& hole)
{
    holes_.push_back(hole);
}

void ompl::control::TriangularDecomposition::addRegionOfInterest(const Polygon& region)
{
    intRegs_.push_back(region);
}

int ompl::control::TriangularDecomposition::getNumHoles(void) const
{
    return holes_.size();
}

int ompl::control::TriangularDecomposition::getNumRegionsOfInterest(void) const
{
    return intRegs_.size();
}

const std::vector<ompl::control::TriangularDecomposition::Polygon>&
    ompl::control::TriangularDecomposition::getHoles(void) const
{
    return holes_;
}

const std::vector<ompl::control::TriangularDecomposition::Polygon>&
    ompl::control::TriangularDecomposition::getAreasOfInterest(void) const
{
    return intRegs_;
}

int ompl::control::TriangularDecomposition::getRegionOfInterestAt(int triID) const
{
    return intRegInfo_[triID];
}

double ompl::control::TriangularDecomposition::getRegionVolume(int triID)
{
    Triangle& tri = triangles_[triID];
    if (tri.volume < 0)
    {
        /* This triangle area formula relies on the vertices being
         * stored in counter-clockwise order. */
        tri.volume = 0.5*(
            (tri.pts[0].x-tri.pts[2].x)*(tri.pts[1].y-tri.pts[0].y)
          - (tri.pts[0].x-tri.pts[1].x)*(tri.pts[2].y-tri.pts[0].y)
        );
    }
    return tri.volume;
}

void ompl::control::TriangularDecomposition::getNeighbors(int triID, std::vector<int> &neighbors) const
{
    neighbors = triangles_[triID].neighbors;
}

int ompl::control::TriangularDecomposition::locateRegion(const base::State* s) const
{
    std::vector<double> coord(2);
    project(s, coord);
    const std::vector<int>& gridTriangles = locator.locateTriangles(s);
    int triangle = -1;
    for (std::vector<int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
    {
        int triID = *i;
        if (triContains(triangles_[triID], coord))
        {
            if (triangle >= 0)
                OMPL_WARN("Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles. \
                    This can happen if the coordinate is located exactly on a triangle segment.\n",
                    coord[0], coord[1]);
            triangle = triID;
        }
    }
    return triangle;
}

void ompl::control::TriangularDecomposition::sampleFromRegion(int triID, RNG& rng, std::vector<double>& coord) const
{
    /* Uniformly sample a point from within a triangle, using the approach discussed in
     * http://math.stackexchange.com/questions/18686/uniform-random-point-in-triangle */
    const Triangle& tri = triangles_[triID];
    coord.resize(2);
    const double r1 = sqrt(rng.uniform01());
    const double r2 = rng.uniform01();
    coord[0] = (1-r1)*tri.pts[0].x + r1*(1-r2)*tri.pts[1].x + r1*r2*tri.pts[2].x;
    coord[1] = (1-r1)*tri.pts[0].y + r1*(1-r2)*tri.pts[1].y + r1*r2*tri.pts[2].y;
}

void ompl::control::TriangularDecomposition::print(std::ostream& out) const
{
    /* For each triangle, print a line of the form
       N x1 y1 x2 y2 x3 y3 L1 L2 ... -1
       N is the ID of the triangle
       L1 L2 ... is the sequence of all regions of interest to which
       this triangle belongs. */
    for (unsigned int i = 0; i < triangles_.size(); ++i)
    {
        out << i << " ";
        const Triangle& tri = triangles_[i];
        for (int v = 0; v < 3; ++v)
            out << tri.pts[v].x << " " << tri.pts[v].y << " ";
        if (intRegInfo_[i] > -1) out << intRegInfo_[i] << " ";
        out << "-1" << std::endl;
    }
}

ompl::control::TriangularDecomposition::Vertex::Vertex(double vx, double vy) : x(vx), y(vy)
{
}

bool ompl::control::TriangularDecomposition::Vertex::operator==(const Vertex &v) const
{
    return x == v.x && y == v.y;
}

int ompl::control::TriangularDecomposition::createTriangles()
{
    /* create a conforming Delaunay triangulation
       where each triangle takes up no more than triAreaPct_ percentage of
       the total area of the decomposition space */
    const base::RealVectorBounds& bounds = getBounds();
    const double maxTriangleArea = bounds.getVolume() * triAreaPct_;
    std::string triswitches = "pDznQA -a" + std::to_string(maxTriangleArea);
    struct triangulateio in;

    /* Some vertices may be duplicates, such as when an obstacle has a vertex equivalent
       to one at the corner of the bounding box of the decomposition.
       libtriangle does not perform correctly if points are duplicated in the pointlist;
       so, to prevent duplicate vertices, we use a hashmap from Vertex to the index for
       that Vertex in the pointlist. We'll fill the map with Vertex objects,
       and then we'll actually add them to the pointlist. */
    std::unordered_map<Vertex, int> pointIndex;

    // First, add the points from the bounding box
    pointIndex[Vertex(bounds.low[0], bounds.low[1])] = 0;
    pointIndex[Vertex(bounds.high[0], bounds.low[1])] = 1;
    pointIndex[Vertex(bounds.high[0], bounds.high[1])] = 2;
    pointIndex[Vertex(bounds.low[0], bounds.high[1])] = 3;

    /* in.numberofpoints equals the total number of unique vertices.
       in.numberofsegments is slightly different: it equals the total number of given vertices.
       They will both be at least 4, due to the bounding box. */
    in.numberofpoints = 4;
    in.numberofsegments = 4;

    typedef std::vector<Polygon>::const_iterator PolyIter;
    typedef std::vector<Vertex>::const_iterator VertexIter;

    //Run through obstacle vertices in holes_, and tally point and segment counters
    for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
    {
        for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
        {
            ++in.numberofsegments;
            /* Only assign an index to this vertex (and tally the point counter)
               if this is a newly discovered vertex. */
            if (pointIndex.find(*v) == pointIndex.end())
                pointIndex[*v] = in.numberofpoints++;
        }
    }

    /* Run through region-of-interest vertices in intRegs_, and tally point and segment counters.
       Here we're following the same logic as above with holes_. */
    for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
    {
        for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
        {
            ++in.numberofsegments;
            if (pointIndex.find(*v) == pointIndex.end())
                pointIndex[*v] = in.numberofpoints++;
        }
    }

    //in.pointlist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of points
    in.pointlist = (REAL*) malloc(2*in.numberofpoints*sizeof(REAL));

    //add unique vertices from our map, using their assigned indices
    typedef std::unordered_map<Vertex, int>::const_iterator IndexIter;
    for (IndexIter i = pointIndex.begin(); i != pointIndex.end(); ++i)
    {
        const Vertex& v = i->first;
        int index = i->second;
        in.pointlist[2*index] = v.x;
        in.pointlist[2*index+1] = v.y;
    }

    /* in.segmentlist is a sequence (a1 b1 a2 b2 ...) of pairs of indices into
       in.pointlist to designate a segment between the respective points. */
    in.segmentlist = (int*) malloc(2*in.numberofsegments*sizeof(int));

    //First, add segments for the bounding box
    for (int i = 0; i < 4; ++i)
    {
        in.segmentlist[2*i] = i;
        in.segmentlist[2*i+1] = (i+1) % 4;
    }

    /* segIndex keeps track of where we are in in.segmentlist,
       as we fill it from multiple sources of data. */
    int segIndex = 4;

    /* Now, add segments for each obstacle in holes_, using our index map
       from before to get the pointlist index for each vertex */
    for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
    {
        for (unsigned int j = 0; j < p->pts.size(); ++j)
        {
            in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
            in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
            ++segIndex;
        }
    }

    /* Now, add segments for each region-of-interest in intRegs_,
       using the same logic as before. */
    for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
    {
        for (unsigned int j = 0; j < p->pts.size(); ++j)
        {
            in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
            in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
            ++segIndex;
        }
    }

    /* libtriangle needs an interior point for each obstacle in holes_.
       For now, we'll assume that each obstacle is convex, and we'll
       generate the interior points ourselves using getPointInPoly. */
    in.numberofholes = holes_.size();
    in.holelist = nullptr;
    if (in.numberofholes > 0)
    {
        /* holelist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of interior points.
           The i^th ordered pair is an interior point of the i^th obstacle in holes_. */
        in.holelist = (REAL*) malloc(2*in.numberofholes*sizeof(REAL));
        for (int i = 0; i < in.numberofholes; ++i)
        {
            Vertex v = getPointInPoly(holes_[i]);
            in.holelist[2*i] = v.x;
            in.holelist[2*i+1] = v.y;
        }
    }

    /* Similar to above, libtriangle needs an interior point for each
       region-of-interest in intRegs_. We follow the same assumption as before
       that each region-of-interest is convex. */
    in.numberofregions = intRegs_.size();
    in.regionlist = nullptr;
    if (in.numberofregions > 0)
    {
        /* regionlist is a sequence (x1 y1 L1 -1 x2 y2 L2 -1 ...) of ordered triples,
           each ended with -1. The i^th ordered pair (xi,yi,Li) is an interior point
           of the i^th region-of-interest in intRegs_, which is assigned the integer
           label Li. */
        in.regionlist = (REAL*) malloc(4*in.numberofregions*sizeof(REAL));
        for (unsigned int i = 0; i < intRegs_.size(); ++i)
        {
            Vertex v = getPointInPoly(intRegs_[i]);
            in.regionlist[4*i] = v.x;
            in.regionlist[4*i+1] = v.y;
            //triangles outside of interesting regions get assigned an attribute of zero by default
            //so let's number our attributes from 1 to numProps, then shift it down by 1 when we're done
            in.regionlist[4*i+2] = (REAL) (i+1);
            in.regionlist[4*i+3] = -1.;
        }
    }

    //mark remaining input fields as unused
    in.segmentmarkerlist = (int*) nullptr;
    in.numberofpointattributes = 0;
    in.pointattributelist = nullptr;
    in.pointmarkerlist = nullptr;

    //initialize output libtriangle structure, which will hold the results of the triangulation
    struct triangulateio out;
    out.pointlist = (REAL*) nullptr;
    out.pointattributelist = (REAL*) nullptr;
    out.pointmarkerlist = (int*) nullptr;
    out.trianglelist = (int*) nullptr;
    out.triangleattributelist = (REAL*) nullptr;
    out.neighborlist = (int*) nullptr;
    out.segmentlist = (int*) nullptr;
    out.segmentmarkerlist = (int*) nullptr;
    out.edgelist = (int*) nullptr;
    out.edgemarkerlist = (int*) nullptr;
    out.pointlist = (REAL*) nullptr;
    out.pointattributelist = (REAL*) nullptr;
    out.trianglelist = (int*) nullptr;
    out.triangleattributelist = (REAL*) nullptr;

    //call the triangulation routine
    triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, nullptr);

    triangles_.resize(out.numberoftriangles);
    intRegInfo_.resize(out.numberoftriangles);
    for (int i = 0; i < out.numberoftriangles; ++i)
    {
        Triangle& t = triangles_[i];
        for (int j = 0; j < 3; ++j)
        {
            t.pts[j].x = out.pointlist[2*out.trianglelist[3*i+j]];
            t.pts[j].y = out.pointlist[2*out.trianglelist[3*i+j]+1];
            if (out.neighborlist[3*i+j] >= 0)
                t.neighbors.push_back(out.neighborlist[3*i+j]);
        }
        t.volume = -1.;

        if (in.numberofregions > 0)
        {
            int attribute = (int) out.triangleattributelist[i];
            /* Shift the region-of-interest ID's down to start from zero. */
            intRegInfo_[i] = (attribute > 0 ? attribute-1 : -1);
        }
    }

    trifree(in.pointlist);
    trifree(in.segmentlist);
    if (in.numberofholes > 0)
        trifree(in.holelist);
    if (in.numberofregions > 0)
        trifree(in.regionlist);
    trifree(out.pointlist);
    trifree(out.pointattributelist);
    trifree(out.pointmarkerlist);
    trifree(out.trianglelist);
    trifree(out.triangleattributelist);
    trifree(out.neighborlist);
    trifree(out.edgelist);
    trifree(out.edgemarkerlist);
    trifree(out.segmentlist);
    trifree(out.segmentmarkerlist);

    return out.numberoftriangles;
}

void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(const std::vector<Triangle>& triangles)
{
    regToTriangles_.resize(getNumRegions());
    std::vector<double> bboxLow(2);
    std::vector<double> bboxHigh(2);
    std::vector<int> gridCoord[2];
    for (unsigned int i = 0; i < triangles.size(); ++i)
    {
        /* for Triangle tri, compute the smallest rectangular
         * bounding box that contains tri. */
        const Triangle& tri = triangles[i];
        bboxLow[0] = tri.pts[0].x;
        bboxLow[1] = tri.pts[0].y;
        bboxHigh[0] = bboxLow[0];
        bboxHigh[1] = bboxLow[1];

        for (int j = 1; j < 3; ++j)
        {
            if (tri.pts[j].x < bboxLow[0])
                bboxLow[0] = tri.pts[j].x;
            else if (tri.pts[j].x > bboxHigh[0])
                bboxHigh[0] = tri.pts[j].x;
            if (tri.pts[j].y < bboxLow[1])
                bboxLow[1] = tri.pts[j].y;
            else if (tri.pts[j].y > bboxHigh[1])
                bboxHigh[1] = tri.pts[j].y;
        }

        /* Convert the bounding box into grid cell coordinates */

        coordToGridCoord(bboxLow, gridCoord[0]);
        coordToGridCoord(bboxHigh, gridCoord[1]);

        /* Every grid cell within bounding box gets
           tri added to its map entry */
        std::vector<int> c(2);
        for (int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
        {
            for (int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
            {
                c[0] = x;
                c[1] = y;
                int cellID = gridCoordToRegion(c);
                regToTriangles_[cellID].push_back(i);
            }
        }
    }
}

void ompl::control::TriangularDecomposition::buildLocatorGrid()
{
    locator.buildTriangleMap(triangles_);
}

bool ompl::control::TriangularDecomposition::triContains(const Triangle& tri, const std::vector<double>& coord)
{
    for (int i = 0; i < 3; ++i)
    {
        /* point (coord[0],coord[1]) needs to be to the left of
           the vector from (ax,ay) to (bx,by) */
        const double ax = tri.pts[i].x;
        const double ay = tri.pts[i].y;
        const double bx = tri.pts[(i+1)%3].x;
        const double by = tri.pts[(i+1)%3].y;

        // return false if the point is instead to the right of the vector
        if ((coord[0]-ax)*(by-ay) - (bx-ax)*(coord[1]-ay) > 0.)
            return false;
    }
    return true;
}

ompl::control::TriangularDecomposition::Vertex ompl::control::TriangularDecomposition::getPointInPoly(const Polygon& poly)
{
    Vertex p;
    p.x = 0.;
    p.y = 0.;
    for (std::vector<Vertex>::const_iterator i = poly.pts.begin(); i != poly.pts.end(); ++i)
    {
        p.x += i->x;
        p.y += i->y;
    }
    p.x /= poly.pts.size();
    p.y /= poly.pts.size();
    return p;
}