TriangularDecomposition.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2012, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Matt Maly */
36
37#include "ompl/extensions/triangle/TriangularDecomposition.h"
38#include "ompl/base/State.h"
39#include "ompl/base/StateSampler.h"
40#include "ompl/base/spaces/RealVectorBounds.h"
41#include "ompl/control/planners/syclop/Decomposition.h"
42#include "ompl/control/planners/syclop/GridDecomposition.h"
43#include "ompl/util/RandomNumbers.h"
44#include "ompl/util/Hash.h"
45#include "ompl/util/String.h"
46#include <ostream>
47#include <utility>
48#include <vector>
49#include <set>
50#include <string>
51#include <unordered_map>
52#include <cstdlib>
53
54extern "C" {
55#define REAL double
56#define VOID void
57#define ANSI_DECLARATORS
58#include <triangle.h>
59}
60
61namespace std
62{
63 template <>
64 struct hash<ompl::control::TriangularDecomposition::Vertex>
65 {
66 size_t operator()(const ompl::control::TriangularDecomposition::Vertex &v) const
67 {
68 std::size_t hash = std::hash<double>()(v.x);
69 ompl::hash_combine(hash, v.y);
70 return hash;
71 }
72 };
73}
74
76 std::vector<Polygon> holes,
77 std::vector<Polygon> intRegs)
78 : Decomposition(2, bounds)
79 , holes_(std::move(holes))
80 , intRegs_(std::move(intRegs))
81 , triAreaPct_(0.005)
82 , locator(64, this)
83{
84 // \todo: Ensure that no two holes overlap and no two regions of interest overlap.
85 // Report an error otherwise.
86}
87
88ompl::control::TriangularDecomposition::~TriangularDecomposition() = default;
89
90void ompl::control::TriangularDecomposition::setup()
91{
92 int numTriangles = createTriangles();
93 OMPL_INFORM("Created %u triangles", numTriangles);
94 buildLocatorGrid();
95}
96
97void ompl::control::TriangularDecomposition::addHole(const Polygon &hole)
98{
99 holes_.push_back(hole);
100}
101
102void ompl::control::TriangularDecomposition::addRegionOfInterest(const Polygon &region)
103{
104 intRegs_.push_back(region);
105}
106
107int ompl::control::TriangularDecomposition::getNumHoles() const
108{
109 return holes_.size();
110}
111
112int ompl::control::TriangularDecomposition::getNumRegionsOfInterest() const
113{
114 return intRegs_.size();
115}
116
117const std::vector<ompl::control::TriangularDecomposition::Polygon> &
118ompl::control::TriangularDecomposition::getHoles() const
119{
120 return holes_;
121}
122
123const std::vector<ompl::control::TriangularDecomposition::Polygon> &
124ompl::control::TriangularDecomposition::getAreasOfInterest() const
125{
126 return intRegs_;
127}
128
130{
131 return intRegInfo_[triID];
132}
133
135{
136 Triangle &tri = triangles_[triID];
137 if (tri.volume < 0)
138 {
139 /* This triangle area formula relies on the vertices being
140 * stored in counter-clockwise order. */
141 tri.volume = 0.5 * ((tri.pts[0].x - tri.pts[2].x) * (tri.pts[1].y - tri.pts[0].y) -
142 (tri.pts[0].x - tri.pts[1].x) * (tri.pts[2].y - tri.pts[0].y));
143 }
144 return tri.volume;
145}
146
147void ompl::control::TriangularDecomposition::getNeighbors(int triID, std::vector<int> &neighbors) const
148{
149 neighbors = triangles_[triID].neighbors;
150}
151
153{
154 std::vector<double> coord(2);
155 project(s, coord);
156 const std::vector<int> &gridTriangles = locator.locateTriangles(s);
157 int triangle = -1;
158 for (int triID : gridTriangles)
159 {
160 if (triContains(triangles_[triID], coord))
161 {
162 if (triangle >= 0)
163 OMPL_WARN("Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles. \
164 This can happen if the coordinate is located exactly on a triangle segment.\n",
165 coord[0], coord[1]);
166 triangle = triID;
167 }
168 }
169 return triangle;
170}
171
172void ompl::control::TriangularDecomposition::sampleFromRegion(int triID, RNG &rng, std::vector<double> &coord) const
173{
174 /* Uniformly sample a point from within a triangle, using the approach discussed in
175 * http://math.stackexchange.com/questions/18686/uniform-random-point-in-triangle */
176 const Triangle &tri = triangles_[triID];
177 coord.resize(2);
178 const double r1 = sqrt(rng.uniform01());
179 const double r2 = rng.uniform01();
180 coord[0] = (1 - r1) * tri.pts[0].x + r1 * (1 - r2) * tri.pts[1].x + r1 * r2 * tri.pts[2].x;
181 coord[1] = (1 - r1) * tri.pts[0].y + r1 * (1 - r2) * tri.pts[1].y + r1 * r2 * tri.pts[2].y;
182}
183
184void ompl::control::TriangularDecomposition::print(std::ostream &out) const
185{
186 /* For each triangle, print a line of the form
187 N x1 y1 x2 y2 x3 y3 L1 L2 ... -1
188 N is the ID of the triangle
189 L1 L2 ... is the sequence of all regions of interest to which
190 this triangle belongs. */
191 for (unsigned int i = 0; i < triangles_.size(); ++i)
192 {
193 out << i << " ";
194 const Triangle &tri = triangles_[i];
195 for (int v = 0; v < 3; ++v)
196 out << tri.pts[v].x << " " << tri.pts[v].y << " ";
197 if (intRegInfo_[i] > -1)
198 out << intRegInfo_[i] << " ";
199 out << "-1" << std::endl;
200 }
201}
202
203ompl::control::TriangularDecomposition::Vertex::Vertex(double vx, double vy) : x(vx), y(vy)
204{
205}
206
207bool ompl::control::TriangularDecomposition::Vertex::operator==(const Vertex &v) const
208{
209 return x == v.x && y == v.y;
210}
211
213{
214 /* create a conforming Delaunay triangulation
215 where each triangle takes up no more than triAreaPct_ percentage of
216 the total area of the decomposition space */
217 const base::RealVectorBounds &bounds = getBounds();
218 const double maxTriangleArea = bounds.getVolume() * triAreaPct_;
219 std::string triswitches = "pDznQA -a" + ompl::toString(maxTriangleArea);
220 struct triangulateio in;
221
222 /* Some vertices may be duplicates, such as when an obstacle has a vertex equivalent
223 to one at the corner of the bounding box of the decomposition.
224 libtriangle does not perform correctly if points are duplicated in the pointlist;
225 so, to prevent duplicate vertices, we use a hashmap from Vertex to the index for
226 that Vertex in the pointlist. We'll fill the map with Vertex objects,
227 and then we'll actually add them to the pointlist. */
228 std::unordered_map<Vertex, int> pointIndex;
229
230 // First, add the points from the bounding box
231 pointIndex[Vertex(bounds.low[0], bounds.low[1])] = 0;
232 pointIndex[Vertex(bounds.high[0], bounds.low[1])] = 1;
233 pointIndex[Vertex(bounds.high[0], bounds.high[1])] = 2;
234 pointIndex[Vertex(bounds.low[0], bounds.high[1])] = 3;
235
236 /* in.numberofpoints equals the total number of unique vertices.
237 in.numberofsegments is slightly different: it equals the total number of given vertices.
238 They will both be at least 4, due to the bounding box. */
239 in.numberofpoints = 4;
240 in.numberofsegments = 4;
241
242 // Run through obstacle vertices in holes_, and tally point and segment counters
243 for (auto &p : holes_)
244 {
245 for (auto &pt : p.pts)
246 {
247 ++in.numberofsegments;
248 /* Only assign an index to this vertex (and tally the point counter)
249 if this is a newly discovered vertex. */
250 if (pointIndex.find(pt) == pointIndex.end())
251 pointIndex[pt] = in.numberofpoints++;
252 }
253 }
254
255 /* Run through region-of-interest vertices in intRegs_, and tally point and segment counters.
256 Here we're following the same logic as above with holes_. */
257 for (auto &p : intRegs_)
258 {
259 for (auto &pt : p.pts)
260 {
261 ++in.numberofsegments;
262 if (pointIndex.find(pt) == pointIndex.end())
263 pointIndex[pt] = in.numberofpoints++;
264 }
265 }
266
267 // in.pointlist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of points
268 in.pointlist = (REAL *)malloc(2 * in.numberofpoints * sizeof(REAL));
269
270 // add unique vertices from our map, using their assigned indices
271 for (const auto &i : pointIndex)
272 {
273 const Vertex &v = i.first;
274 int index = i.second;
275 in.pointlist[2 * index] = v.x;
276 in.pointlist[2 * index + 1] = v.y;
277 }
278
279 /* in.segmentlist is a sequence (a1 b1 a2 b2 ...) of pairs of indices into
280 in.pointlist to designate a segment between the respective points. */
281 in.segmentlist = (int *)malloc(2 * in.numberofsegments * sizeof(int));
282
283 // First, add segments for the bounding box
284 for (int i = 0; i < 4; ++i)
285 {
286 in.segmentlist[2 * i] = i;
287 in.segmentlist[2 * i + 1] = (i + 1) % 4;
288 }
289
290 /* segIndex keeps track of where we are in in.segmentlist,
291 as we fill it from multiple sources of data. */
292 int segIndex = 4;
293
294 /* Now, add segments for each obstacle in holes_, using our index map
295 from before to get the pointlist index for each vertex */
296 for (auto &p : holes_)
297 {
298 for (unsigned int j = 0; j < p.pts.size(); ++j)
299 {
300 in.segmentlist[2 * segIndex] = pointIndex[p.pts[j]];
301 in.segmentlist[2 * segIndex + 1] = pointIndex[p.pts[(j + 1) % p.pts.size()]];
302 ++segIndex;
303 }
304 }
305
306 /* Now, add segments for each region-of-interest in intRegs_,
307 using the same logic as before. */
308 for (auto &p : intRegs_)
309 {
310 for (unsigned int j = 0; j < p.pts.size(); ++j)
311 {
312 in.segmentlist[2 * segIndex] = pointIndex[p.pts[j]];
313 in.segmentlist[2 * segIndex + 1] = pointIndex[p.pts[(j + 1) % p.pts.size()]];
314 ++segIndex;
315 }
316 }
317
318 /* libtriangle needs an interior point for each obstacle in holes_.
319 For now, we'll assume that each obstacle is convex, and we'll
320 generate the interior points ourselves using getPointInPoly. */
321 in.numberofholes = holes_.size();
322 in.holelist = nullptr;
323 if (in.numberofholes > 0)
324 {
325 /* holelist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of interior points.
326 The i^th ordered pair is an interior point of the i^th obstacle in holes_. */
327 in.holelist = (REAL *)malloc(2 * in.numberofholes * sizeof(REAL));
328 for (int i = 0; i < in.numberofholes; ++i)
329 {
330 Vertex v = getPointInPoly(holes_[i]);
331 in.holelist[2 * i] = v.x;
332 in.holelist[2 * i + 1] = v.y;
333 }
334 }
335
336 /* Similar to above, libtriangle needs an interior point for each
337 region-of-interest in intRegs_. We follow the same assumption as before
338 that each region-of-interest is convex. */
339 in.numberofregions = intRegs_.size();
340 in.regionlist = nullptr;
341 if (in.numberofregions > 0)
342 {
343 /* regionlist is a sequence (x1 y1 L1 -1 x2 y2 L2 -1 ...) of ordered triples,
344 each ended with -1. The i^th ordered pair (xi,yi,Li) is an interior point
345 of the i^th region-of-interest in intRegs_, which is assigned the integer
346 label Li. */
347 in.regionlist = (REAL *)malloc(4 * in.numberofregions * sizeof(REAL));
348 for (unsigned int i = 0; i < intRegs_.size(); ++i)
349 {
350 Vertex v = getPointInPoly(intRegs_[i]);
351 in.regionlist[4 * i] = v.x;
352 in.regionlist[4 * i + 1] = v.y;
353 // triangles outside of interesting regions get assigned an attribute of zero by default
354 // so let's number our attributes from 1 to numProps, then shift it down by 1 when we're done
355 in.regionlist[4 * i + 2] = (REAL)(i + 1);
356 in.regionlist[4 * i + 3] = -1.;
357 }
358 }
359
360 // mark remaining input fields as unused
361 in.segmentmarkerlist = (int *)nullptr;
362 in.numberofpointattributes = 0;
363 in.pointattributelist = nullptr;
364 in.pointmarkerlist = nullptr;
365
366 // initialize output libtriangle structure, which will hold the results of the triangulation
367 struct triangulateio out;
368 out.pointlist = (REAL *)nullptr;
369 out.pointattributelist = (REAL *)nullptr;
370 out.pointmarkerlist = (int *)nullptr;
371 out.trianglelist = (int *)nullptr;
372 out.triangleattributelist = (REAL *)nullptr;
373 out.neighborlist = (int *)nullptr;
374 out.segmentlist = (int *)nullptr;
375 out.segmentmarkerlist = (int *)nullptr;
376 out.edgelist = (int *)nullptr;
377 out.edgemarkerlist = (int *)nullptr;
378 out.pointlist = (REAL *)nullptr;
379 out.pointattributelist = (REAL *)nullptr;
380 out.trianglelist = (int *)nullptr;
381 out.triangleattributelist = (REAL *)nullptr;
382
383 // call the triangulation routine
384 triangulate(const_cast<char *>(triswitches.c_str()), &in, &out, nullptr);
385
386 triangles_.resize(out.numberoftriangles);
387 intRegInfo_.resize(out.numberoftriangles);
388 for (int i = 0; i < out.numberoftriangles; ++i)
389 {
390 Triangle &t = triangles_[i];
391 for (int j = 0; j < 3; ++j)
392 {
393 t.pts[j].x = out.pointlist[2 * out.trianglelist[3 * i + j]];
394 t.pts[j].y = out.pointlist[2 * out.trianglelist[3 * i + j] + 1];
395 if (out.neighborlist[3 * i + j] >= 0)
396 t.neighbors.push_back(out.neighborlist[3 * i + j]);
397 }
398 t.volume = -1.;
399
400 if (in.numberofregions > 0)
401 {
402 auto attribute = (int)out.triangleattributelist[i];
403 /* Shift the region-of-interest ID's down to start from zero. */
404 intRegInfo_[i] = (attribute > 0 ? attribute - 1 : -1);
405 }
406 }
407
408 trifree(in.pointlist);
409 trifree(in.segmentlist);
410 if (in.numberofholes > 0)
411 trifree(in.holelist);
412 if (in.numberofregions > 0)
413 trifree(in.regionlist);
414 trifree(out.pointlist);
415 trifree(out.pointattributelist);
416 trifree(out.pointmarkerlist);
417 trifree(out.trianglelist);
418 trifree(out.triangleattributelist);
419 trifree(out.neighborlist);
420 trifree(out.edgelist);
421 trifree(out.edgemarkerlist);
422 trifree(out.segmentlist);
423 trifree(out.segmentmarkerlist);
424
425 return out.numberoftriangles;
426}
427
428void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(const std::vector<Triangle> &triangles)
429{
430 regToTriangles_.resize(getNumRegions());
431 std::vector<double> bboxLow(2);
432 std::vector<double> bboxHigh(2);
433 std::vector<int> gridCoord[2];
434 for (unsigned int i = 0; i < triangles.size(); ++i)
435 {
436 /* for Triangle tri, compute the smallest rectangular
437 * bounding box that contains tri. */
438 const Triangle &tri = triangles[i];
439 bboxLow[0] = tri.pts[0].x;
440 bboxLow[1] = tri.pts[0].y;
441 bboxHigh[0] = bboxLow[0];
442 bboxHigh[1] = bboxLow[1];
443
444 for (int j = 1; j < 3; ++j)
445 {
446 if (tri.pts[j].x < bboxLow[0])
447 bboxLow[0] = tri.pts[j].x;
448 else if (tri.pts[j].x > bboxHigh[0])
449 bboxHigh[0] = tri.pts[j].x;
450 if (tri.pts[j].y < bboxLow[1])
451 bboxLow[1] = tri.pts[j].y;
452 else if (tri.pts[j].y > bboxHigh[1])
453 bboxHigh[1] = tri.pts[j].y;
454 }
455
456 /* Convert the bounding box into grid cell coordinates */
457
458 coordToGridCoord(bboxLow, gridCoord[0]);
459 coordToGridCoord(bboxHigh, gridCoord[1]);
460
461 /* Every grid cell within bounding box gets
462 tri added to its map entry */
463 std::vector<int> c(2);
464 for (int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
465 {
466 for (int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
467 {
468 c[0] = x;
469 c[1] = y;
470 int cellID = gridCoordToRegion(c);
471 regToTriangles_[cellID].push_back(i);
472 }
473 }
474 }
475}
476
477void ompl::control::TriangularDecomposition::buildLocatorGrid()
478{
479 locator.buildTriangleMap(triangles_);
480}
481
482bool ompl::control::TriangularDecomposition::triContains(const Triangle &tri, const std::vector<double> &coord)
483{
484 for (int i = 0; i < 3; ++i)
485 {
486 /* point (coord[0],coord[1]) needs to be to the left of
487 the vector from (ax,ay) to (bx,by) */
488 const double ax = tri.pts[i].x;
489 const double ay = tri.pts[i].y;
490 const double bx = tri.pts[(i + 1) % 3].x;
491 const double by = tri.pts[(i + 1) % 3].y;
492
493 // return false if the point is instead to the right of the vector
494 if ((coord[0] - ax) * (by - ay) - (bx - ax) * (coord[1] - ay) > 0.)
495 return false;
496 }
497 return true;
498}
499
501ompl::control::TriangularDecomposition::getPointInPoly(const Polygon &poly)
502{
503 Vertex p;
504 p.x = 0.;
505 p.y = 0.;
506 for (auto pt : poly.pts)
507 {
508 p.x += pt.x;
509 p.y += pt.y;
510 }
511 p.x /= poly.pts.size();
512 p.y /= poly.pts.size();
513 return p;
514}
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:67
The lower and upper bounds for an Rn space.
Definition of an abstract state.
Definition: State.h:50
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
Definition: Decomposition.h:63
TriangularDecomposition(const base::RealVectorBounds &bounds, std::vector< Polygon > holes=std::vector< Polygon >(), std::vector< Polygon > intRegs=std::vector< Polygon >())
Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional....
int locateRegion(const base::State *s) const override
Returns the index of the region containing a given State. Most often, this is obtained by first calli...
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
STL namespace.