ompl/extensions/triangle/src/TriangularDecomposition.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Matt Maly */
00036 
00037 #include "ompl/extensions/triangle/TriangularDecomposition.h"
00038 #include "ompl/base/State.h"
00039 #include "ompl/base/StateSampler.h"
00040 #include "ompl/base/spaces/RealVectorBounds.h"
00041 #include "ompl/control/planners/syclop/Decomposition.h"
00042 #include "ompl/control/planners/syclop/GridDecomposition.h"
00043 #include "ompl/util/RandomNumbers.h"
00044 #include <ostream>
00045 #include <vector>
00046 #include <set>
00047 #include <string>
00048 #include <boost/functional/hash.hpp>
00049 #include <boost/lexical_cast.hpp>
00050 #include <boost/unordered_map.hpp>
00051 #include <cstdlib>
00052 
00053 extern "C"
00054 {
00055     #define REAL double
00056     #define VOID void
00057     #define ANSI_DECLARATORS
00058     #include <triangle.h>
00059 }
00060 
00061 ompl::control::TriangularDecomposition::TriangularDecomposition(const base::RealVectorBounds &bounds,
00062     const std::vector<Polygon> &holes, const std::vector<Polygon> &intRegs) :
00063     Decomposition(2, bounds),
00064     holes_(holes),
00065     intRegs_(intRegs),
00066     triAreaPct_(0.005),
00067     locator(64, this)
00068 {
00069     // TODO: Ensure that no two holes overlap and no two regions of interest overlap.
00070     // Report an error otherwise.
00071 }
00072 
00073 ompl::control::TriangularDecomposition::~TriangularDecomposition(void)
00074 {
00075 }
00076 
00077 void ompl::control::TriangularDecomposition::setup(void)
00078 {
00079     int numTriangles = createTriangles();
00080     OMPL_INFORM("Created %u triangles", numTriangles);
00081     buildLocatorGrid();
00082 }
00083 
00084 void ompl::control::TriangularDecomposition::addHole(const Polygon& hole)
00085 {
00086     holes_.push_back(hole);
00087 }
00088 
00089 void ompl::control::TriangularDecomposition::addRegionOfInterest(const Polygon& region)
00090 {
00091     intRegs_.push_back(region);
00092 }
00093 
00094 int ompl::control::TriangularDecomposition::getNumHoles(void) const
00095 {
00096     return holes_.size();
00097 }
00098 
00099 int ompl::control::TriangularDecomposition::getNumRegionsOfInterest(void) const
00100 {
00101     return intRegs_.size();
00102 }
00103 
00104 const std::vector<ompl::control::TriangularDecomposition::Polygon>&
00105     ompl::control::TriangularDecomposition::getHoles(void) const
00106 {
00107     return holes_;
00108 }
00109 
00110 const std::vector<ompl::control::TriangularDecomposition::Polygon>&
00111     ompl::control::TriangularDecomposition::getAreasOfInterest(void) const
00112 {
00113     return intRegs_;
00114 }
00115 
00116 int ompl::control::TriangularDecomposition::getRegionOfInterestAt(int triID) const
00117 {
00118     return intRegInfo_[triID];
00119 }
00120 
00121 double ompl::control::TriangularDecomposition::getRegionVolume(int triID)
00122 {
00123     Triangle& tri = triangles_[triID];
00124     if (tri.volume < 0)
00125     {
00126         /* This triangle area formula relies on the vertices being
00127          * stored in counter-clockwise order. */
00128         tri.volume = 0.5*(
00129             (tri.pts[0].x-tri.pts[2].x)*(tri.pts[1].y-tri.pts[0].y)
00130           - (tri.pts[0].x-tri.pts[1].x)*(tri.pts[2].y-tri.pts[0].y)
00131         );
00132     }
00133     return tri.volume;
00134 }
00135 
00136 void ompl::control::TriangularDecomposition::getNeighbors(int triID, std::vector<int> &neighbors) const
00137 {
00138     neighbors = triangles_[triID].neighbors;
00139 }
00140 
00141 int ompl::control::TriangularDecomposition::locateRegion(const base::State* s) const
00142 {
00143     std::vector<double> coord(2);
00144     project(s, coord);
00145     const std::vector<int>& gridTriangles = locator.locateTriangles(s);
00146     int triangle = -1;
00147     for (std::vector<int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
00148     {
00149         int triID = *i;
00150         if (triContains(triangles_[triID], coord))
00151         {
00152             if (triangle >= 0)
00153                 OMPL_WARN("Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles. \
00154                     This can happen if the coordinate is located exactly on a triangle segment.\n",
00155                     coord[0], coord[1]);
00156             triangle = triID;
00157         }
00158     }
00159     return triangle;
00160 }
00161 
00162 void ompl::control::TriangularDecomposition::sampleFromRegion(int triID, RNG& rng, std::vector<double>& coord) const
00163 {
00164     /* Uniformly sample a point from within a triangle, using the approach discussed in
00165      * http://math.stackexchange.com/questions/18686/uniform-random-point-in-triangle */
00166     const Triangle& tri = triangles_[triID];
00167     coord.resize(2);
00168     const double r1 = sqrt(rng.uniform01());
00169     const double r2 = rng.uniform01();
00170     coord[0] = (1-r1)*tri.pts[0].x + r1*(1-r2)*tri.pts[1].x + r1*r2*tri.pts[2].x;
00171     coord[1] = (1-r1)*tri.pts[0].y + r1*(1-r2)*tri.pts[1].y + r1*r2*tri.pts[2].y;
00172 }
00173 
00174 void ompl::control::TriangularDecomposition::print(std::ostream& out) const
00175 {
00176     /* For each triangle, print a line of the form
00177        N x1 y1 x2 y2 x3 y3 L1 L2 ... -1
00178        N is the ID of the triangle
00179        L1 L2 ... is the sequence of all regions of interest to which
00180        this triangle belongs. */
00181     for (unsigned int i = 0; i < triangles_.size(); ++i)
00182     {
00183         out << i << " ";
00184         const Triangle& tri = triangles_[i];
00185         for (int v = 0; v < 3; ++v)
00186             out << tri.pts[v].x << " " << tri.pts[v].y << " ";
00187         if (intRegInfo_[i] > -1) out << intRegInfo_[i] << " ";
00188         out << "-1" << std::endl;
00189     }
00190 }
00191 
00192 ompl::control::TriangularDecomposition::Vertex::Vertex(double vx, double vy) : x(vx), y(vy)
00193 {
00194 }
00195 
00196 bool ompl::control::TriangularDecomposition::Vertex::operator==(const Vertex &v) const
00197 {
00198     return x == v.x && y == v.y;
00199 }
00200 
00201 namespace ompl
00202 {
00203     namespace control
00204     {
00205         std::size_t hash_value(const TriangularDecomposition::Vertex &v)
00206         {
00207             std::size_t hash = 0;
00208             boost::hash_combine(hash, v.x);
00209             boost::hash_combine(hash, v.y);
00210             return hash;
00211         }
00212     }
00213 }
00214 
00215 int ompl::control::TriangularDecomposition::createTriangles()
00216 {
00217     /* create a conforming Delaunay triangulation
00218        where each triangle takes up no more than triAreaPct_ percentage of
00219        the total area of the decomposition space */
00220     const base::RealVectorBounds& bounds = getBounds();
00221     const double maxTriangleArea = bounds.getVolume() * triAreaPct_;
00222     std::string triswitches = "pDznQA -a" + boost::lexical_cast<std::string>(maxTriangleArea);
00223     struct triangulateio in;
00224 
00225     /* Some vertices may be duplicates, such as when an obstacle has a vertex equivalent
00226        to one at the corner of the bounding box of the decomposition.
00227        libtriangle does not perform correctly if points are duplicated in the pointlist;
00228        so, to prevent duplicate vertices, we use a hashmap from Vertex to the index for
00229        that Vertex in the pointlist. We'll fill the map with Vertex objects,
00230        and then we'll actually add them to the pointlist. */
00231     boost::unordered_map<Vertex, int> pointIndex;
00232 
00233     // First, add the points from the bounding box
00234     pointIndex[Vertex(bounds.low[0], bounds.low[1])] = 0;
00235     pointIndex[Vertex(bounds.high[0], bounds.low[1])] = 1;
00236     pointIndex[Vertex(bounds.high[0], bounds.high[1])] = 2;
00237     pointIndex[Vertex(bounds.low[0], bounds.high[1])] = 3;
00238 
00239     /* in.numberofpoints equals the total number of unique vertices.
00240        in.numberofsegments is slightly different: it equals the total number of given vertices.
00241        They will both be at least 4, due to the bounding box. */
00242     in.numberofpoints = 4;
00243     in.numberofsegments = 4;
00244 
00245     typedef std::vector<Polygon>::const_iterator PolyIter;
00246     typedef std::vector<Vertex>::const_iterator VertexIter;
00247 
00248     //Run through obstacle vertices in holes_, and tally point and segment counters
00249     for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
00250     {
00251         for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
00252         {
00253             ++in.numberofsegments;
00254             /* Only assign an index to this vertex (and tally the point counter)
00255                if this is a newly discovered vertex. */
00256             if (pointIndex.find(*v) == pointIndex.end())
00257                 pointIndex[*v] = in.numberofpoints++;
00258         }
00259     }
00260 
00261     /* Run through region-of-interest vertices in intRegs_, and tally point and segment counters.
00262        Here we're following the same logic as above with holes_. */
00263     for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
00264     {
00265         for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
00266         {
00267             ++in.numberofsegments;
00268             if (pointIndex.find(*v) == pointIndex.end())
00269                 pointIndex[*v] = in.numberofpoints++;
00270         }
00271     }
00272 
00273     //in.pointlist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of points
00274     in.pointlist = (REAL*) malloc(2*in.numberofpoints*sizeof(REAL));
00275 
00276     //add unique vertices from our map, using their assigned indices
00277     typedef boost::unordered_map<Vertex, int>::const_iterator IndexIter;
00278     for (IndexIter i = pointIndex.begin(); i != pointIndex.end(); ++i)
00279     {
00280         const Vertex& v = i->first;
00281         int index = i->second;
00282         in.pointlist[2*index] = v.x;
00283         in.pointlist[2*index+1] = v.y;
00284     }
00285 
00286     /* in.segmentlist is a sequence (a1 b1 a2 b2 ...) of pairs of indices into
00287        in.pointlist to designate a segment between the respective points. */
00288     in.segmentlist = (int*) malloc(2*in.numberofsegments*sizeof(int));
00289 
00290     //First, add segments for the bounding box
00291     for (int i = 0; i < 4; ++i)
00292     {
00293         in.segmentlist[2*i] = i;
00294         in.segmentlist[2*i+1] = (i+1) % 4;
00295     }
00296 
00297     /* segIndex keeps track of where we are in in.segmentlist,
00298        as we fill it from multiple sources of data. */
00299     int segIndex = 4;
00300 
00301     /* Now, add segments for each obstacle in holes_, using our index map
00302        from before to get the pointlist index for each vertex */
00303     for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
00304     {
00305         for (unsigned int j = 0; j < p->pts.size(); ++j)
00306         {
00307             in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
00308             in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
00309             ++segIndex;
00310         }
00311     }
00312 
00313     /* Now, add segments for each region-of-interest in intRegs_,
00314        using the same logic as before. */
00315     for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
00316     {
00317         for (unsigned int j = 0; j < p->pts.size(); ++j)
00318         {
00319             in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
00320             in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
00321             ++segIndex;
00322         }
00323     }
00324 
00325     /* libtriangle needs an interior point for each obstacle in holes_.
00326        For now, we'll assume that each obstacle is convex, and we'll
00327        generate the interior points ourselves using getPointInPoly. */
00328     in.numberofholes = holes_.size();
00329     in.holelist = NULL;
00330     if (in.numberofholes > 0)
00331     {
00332         /* holelist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of interior points.
00333            The i^th ordered pair is an interior point of the i^th obstacle in holes_. */
00334         in.holelist = (REAL*) malloc(2*in.numberofholes*sizeof(REAL));
00335         for (int i = 0; i < in.numberofholes; ++i)
00336         {
00337             Vertex v = getPointInPoly(holes_[i]);
00338             in.holelist[2*i] = v.x;
00339             in.holelist[2*i+1] = v.y;
00340         }
00341     }
00342 
00343     /* Similar to above, libtriangle needs an interior point for each
00344        region-of-interest in intRegs_. We follow the same assumption as before
00345        that each region-of-interest is convex. */
00346     in.numberofregions = intRegs_.size();
00347     in.regionlist = NULL;
00348     if (in.numberofregions > 0)
00349     {
00350         /* regionlist is a sequence (x1 y1 L1 -1 x2 y2 L2 -1 ...) of ordered triples,
00351            each ended with -1. The i^th ordered pair (xi,yi,Li) is an interior point
00352            of the i^th region-of-interest in intRegs_, which is assigned the integer
00353            label Li. */
00354         in.regionlist = (REAL*) malloc(4*in.numberofregions*sizeof(REAL));
00355         for (unsigned int i = 0; i < intRegs_.size(); ++i)
00356         {
00357             Vertex v = getPointInPoly(intRegs_[i]);
00358             in.regionlist[4*i] = v.x;
00359             in.regionlist[4*i+1] = v.y;
00360             //triangles outside of interesting regions get assigned an attribute of zero by default
00361             //so let's number our attributes from 1 to numProps, then shift it down by 1 when we're done
00362             in.regionlist[4*i+2] = (REAL) (i+1);
00363             in.regionlist[4*i+3] = -1.;
00364         }
00365     }
00366 
00367     //mark remaining input fields as unused
00368     in.segmentmarkerlist = (int*) NULL;
00369     in.numberofpointattributes = 0;
00370     in.pointattributelist = NULL;
00371     in.pointmarkerlist = NULL;
00372 
00373     //initialize output libtriangle structure, which will hold the results of the triangulation
00374     struct triangulateio out;
00375     out.pointlist = (REAL*) NULL;
00376     out.pointattributelist = (REAL*) NULL;
00377     out.pointmarkerlist = (int*) NULL;
00378     out.trianglelist = (int*) NULL;
00379     out.triangleattributelist = (REAL*) NULL;
00380     out.neighborlist = (int*) NULL;
00381     out.segmentlist = (int*) NULL;
00382     out.segmentmarkerlist = (int*) NULL;
00383     out.edgelist = (int*) NULL;
00384     out.edgemarkerlist = (int*) NULL;
00385     out.pointlist = (REAL*) NULL;
00386     out.pointattributelist = (REAL*) NULL;
00387     out.trianglelist = (int*) NULL;
00388     out.triangleattributelist = (REAL*) NULL;
00389 
00390     //call the triangulation routine
00391     triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, NULL);
00392 
00393     triangles_.resize(out.numberoftriangles);
00394     intRegInfo_.resize(out.numberoftriangles);
00395     for (int i = 0; i < out.numberoftriangles; ++i)
00396     {
00397         Triangle& t = triangles_[i];
00398         for (int j = 0; j < 3; ++j)
00399         {
00400             t.pts[j].x = out.pointlist[2*out.trianglelist[3*i+j]];
00401             t.pts[j].y = out.pointlist[2*out.trianglelist[3*i+j]+1];
00402             if (out.neighborlist[3*i+j] >= 0)
00403                 t.neighbors.push_back(out.neighborlist[3*i+j]);
00404         }
00405         t.volume = -1.;
00406 
00407         if (in.numberofregions > 0)
00408         {
00409             int attribute = (int) out.triangleattributelist[i];
00410             /* Shift the region-of-interest ID's down to start from zero. */
00411             intRegInfo_[i] = (attribute > 0 ? attribute-1 : -1);
00412         }
00413     }
00414 
00415     trifree(in.pointlist);
00416     trifree(in.segmentlist);
00417     if (in.numberofholes > 0)
00418         trifree(in.holelist);
00419     if (in.numberofregions > 0)
00420         trifree(in.regionlist);
00421     trifree(out.pointlist);
00422     trifree(out.pointattributelist);
00423     trifree(out.pointmarkerlist);
00424     trifree(out.trianglelist);
00425     trifree(out.triangleattributelist);
00426     trifree(out.neighborlist);
00427     trifree(out.edgelist);
00428     trifree(out.edgemarkerlist);
00429     trifree(out.segmentlist);
00430     trifree(out.segmentmarkerlist);
00431 
00432     return out.numberoftriangles;
00433 }
00434 
00435 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(const std::vector<Triangle>& triangles)
00436 {
00437     regToTriangles_.resize(getNumRegions());
00438     std::vector<double> bboxLow(2);
00439     std::vector<double> bboxHigh(2);
00440     std::vector<int> gridCoord[2];
00441     for (unsigned int i = 0; i < triangles.size(); ++i)
00442     {
00443         /* for Triangle tri, compute the smallest rectangular
00444          * bounding box that contains tri. */
00445         const Triangle& tri = triangles[i];
00446         bboxLow[0] = tri.pts[0].x;
00447         bboxLow[1] = tri.pts[0].y;
00448         bboxHigh[0] = bboxLow[0];
00449         bboxHigh[1] = bboxLow[1];
00450 
00451         for (int j = 1; j < 3; ++j)
00452         {
00453             if (tri.pts[j].x < bboxLow[0])
00454                 bboxLow[0] = tri.pts[j].x;
00455             else if (tri.pts[j].x > bboxHigh[0])
00456                 bboxHigh[0] = tri.pts[j].x;
00457             if (tri.pts[j].y < bboxLow[1])
00458                 bboxLow[1] = tri.pts[j].y;
00459             else if (tri.pts[j].y > bboxHigh[1])
00460                 bboxHigh[1] = tri.pts[j].y;
00461         }
00462 
00463         /* Convert the bounding box into grid cell coordinates */
00464 
00465         coordToGridCoord(bboxLow, gridCoord[0]);
00466         coordToGridCoord(bboxHigh, gridCoord[1]);
00467 
00468         /* Every grid cell within bounding box gets
00469            tri added to its map entry */
00470         std::vector<int> c(2);
00471         for (int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
00472         {
00473             for (int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
00474             {
00475                 c[0] = x;
00476                 c[1] = y;
00477                 int cellID = gridCoordToRegion(c);
00478                 regToTriangles_[cellID].push_back(i);
00479             }
00480         }
00481     }
00482 }
00483 
00484 void ompl::control::TriangularDecomposition::buildLocatorGrid()
00485 {
00486     locator.buildTriangleMap(triangles_);
00487 }
00488 
00489 bool ompl::control::TriangularDecomposition::triContains(const Triangle& tri, const std::vector<double>& coord)
00490 {
00491     for (int i = 0; i < 3; ++i)
00492     {
00493         /* point (coord[0],coord[1]) needs to be to the left of
00494            the vector from (ax,ay) to (bx,by) */
00495         const double ax = tri.pts[i].x;
00496         const double ay = tri.pts[i].y;
00497         const double bx = tri.pts[(i+1)%3].x;
00498         const double by = tri.pts[(i+1)%3].y;
00499 
00500         // return false if the point is instead to the right of the vector
00501         if ((coord[0]-ax)*(by-ay) - (bx-ax)*(coord[1]-ay) > 0.)
00502             return false;
00503     }
00504     return true;
00505 }
00506 
00507 ompl::control::TriangularDecomposition::Vertex ompl::control::TriangularDecomposition::getPointInPoly(const Polygon& poly)
00508 {
00509     Vertex p;
00510     p.x = 0.;
00511     p.y = 0.;
00512     for (std::vector<Vertex>::const_iterator i = poly.pts.begin(); i != poly.pts.end(); ++i)
00513     {
00514         p.x += i->x;
00515         p.y += i->y;
00516     }
00517     p.x /= poly.pts.size();
00518     p.y /= poly.pts.size();
00519     return p;
00520 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines