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:89
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
Definition of an abstract state.
Definition: State.h:113
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
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...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
double uniform01()
Generate a random real between 0 and 1.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
Main namespace. Contains everything in this library.
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....
The lower and upper bounds for an Rn space.