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