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 }