Loading...
Searching...
No Matches
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
500ompl::control::TriangularDecomposition::Vertex
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...
double uniform01()
Generate a random real between 0 and 1.
The lower and upper bounds for an Rn space.
Definition of an abstract state.
Definition State.h:50
Decomposition(int dim, const base::RealVectorBounds &b)
Constructor. Creates a Decomposition with a given dimension and a given set of bounds....
virtual void project(const base::State *s, std::vector< double > &coord) const =0
Project a given State to a set of coordinates in R^k, where k is the dimension of this Decomposition.
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
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.
std::vector< int > intRegInfo_
Maps from triangle ID to index of Polygon in intReg_ that contains the triangle ID....
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
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
STL namespace.