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 
54 extern "C" {
55 #define REAL double
56 #define VOID void
57 #define ANSI_DECLARATORS
58 #include <triangle.h>
59 }
60 
61 namespace 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 
88 ompl::control::TriangularDecomposition::~TriangularDecomposition() = default;
89 
90 void ompl::control::TriangularDecomposition::setup()
91 {
92  int numTriangles = createTriangles();
93  OMPL_INFORM("Created %u triangles", numTriangles);
94  buildLocatorGrid();
95 }
96 
97 void ompl::control::TriangularDecomposition::addHole(const Polygon &hole)
98 {
99  holes_.push_back(hole);
100 }
101 
102 void ompl::control::TriangularDecomposition::addRegionOfInterest(const Polygon &region)
103 {
104  intRegs_.push_back(region);
105 }
106 
107 int ompl::control::TriangularDecomposition::getNumHoles() const
108 {
109  return holes_.size();
110 }
111 
112 int ompl::control::TriangularDecomposition::getNumRegionsOfInterest() const
113 {
114  return intRegs_.size();
115 }
116 
117 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
118 ompl::control::TriangularDecomposition::getHoles() const
119 {
120  return holes_;
121 }
122 
123 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
124 ompl::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 
147 void 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 
172 void 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 
184 void 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 
203 ompl::control::TriangularDecomposition::Vertex::Vertex(double vx, double vy) : x(vx), y(vy)
204 {
205 }
206 
207 bool 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 
428 void 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 
477 void ompl::control::TriangularDecomposition::buildLocatorGrid()
478 {
479  locator.buildTriangleMap(triangles_);
480 }
481 
482 bool 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 
501 ompl::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.
Definition: AppBase.h:22
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82