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  using PolyIter = std::vector<Polygon>::const_iterator;
242  using VertexIter = std::vector<Vertex>::const_iterator;
243 
244  // Run through obstacle vertices in holes_, and tally point and segment counters
245  for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
246  {
247  for (auto pt : p->pts)
248  {
249  ++in.numberofsegments;
250  /* Only assign an index to this vertex (and tally the point counter)
251  if this is a newly discovered vertex. */
252  if (pointIndex.find(pt) == pointIndex.end())
253  pointIndex[pt] = in.numberofpoints++;
254  }
255  }
256 
257  /* Run through region-of-interest vertices in intRegs_, and tally point and segment counters.
258  Here we're following the same logic as above with holes_. */
259  for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
260  {
261  for (auto pt : p->pts)
262  {
263  ++in.numberofsegments;
264  if (pointIndex.find(pt) == pointIndex.end())
265  pointIndex[pt] = in.numberofpoints++;
266  }
267  }
268 
269  // in.pointlist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of points
270  in.pointlist = (REAL *)malloc(2 * in.numberofpoints * sizeof(REAL));
271 
272  // add unique vertices from our map, using their assigned indices
273  for (const auto &i : pointIndex)
274  {
275  const Vertex &v = i.first;
276  int index = i.second;
277  in.pointlist[2 * index] = v.x;
278  in.pointlist[2 * index + 1] = v.y;
279  }
280 
281  /* in.segmentlist is a sequence (a1 b1 a2 b2 ...) of pairs of indices into
282  in.pointlist to designate a segment between the respective points. */
283  in.segmentlist = (int *)malloc(2 * in.numberofsegments * sizeof(int));
284 
285  // First, add segments for the bounding box
286  for (int i = 0; i < 4; ++i)
287  {
288  in.segmentlist[2 * i] = i;
289  in.segmentlist[2 * i + 1] = (i + 1) % 4;
290  }
291 
292  /* segIndex keeps track of where we are in in.segmentlist,
293  as we fill it from multiple sources of data. */
294  int segIndex = 4;
295 
296  /* Now, add segments for each obstacle in holes_, using our index map
297  from before to get the pointlist index for each vertex */
298  for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
299  {
300  for (unsigned int j = 0; j < p->pts.size(); ++j)
301  {
302  in.segmentlist[2 * segIndex] = pointIndex[p->pts[j]];
303  in.segmentlist[2 * segIndex + 1] = pointIndex[p->pts[(j + 1) % p->pts.size()]];
304  ++segIndex;
305  }
306  }
307 
308  /* Now, add segments for each region-of-interest in intRegs_,
309  using the same logic as before. */
310  for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
311  {
312  for (unsigned int j = 0; j < p->pts.size(); ++j)
313  {
314  in.segmentlist[2 * segIndex] = pointIndex[p->pts[j]];
315  in.segmentlist[2 * segIndex + 1] = pointIndex[p->pts[(j + 1) % p->pts.size()]];
316  ++segIndex;
317  }
318  }
319 
320  /* libtriangle needs an interior point for each obstacle in holes_.
321  For now, we'll assume that each obstacle is convex, and we'll
322  generate the interior points ourselves using getPointInPoly. */
323  in.numberofholes = holes_.size();
324  in.holelist = nullptr;
325  if (in.numberofholes > 0)
326  {
327  /* holelist is a sequence (x1 y1 x2 y2 ...) of ordered pairs of interior points.
328  The i^th ordered pair is an interior point of the i^th obstacle in holes_. */
329  in.holelist = (REAL *)malloc(2 * in.numberofholes * sizeof(REAL));
330  for (int i = 0; i < in.numberofholes; ++i)
331  {
332  Vertex v = getPointInPoly(holes_[i]);
333  in.holelist[2 * i] = v.x;
334  in.holelist[2 * i + 1] = v.y;
335  }
336  }
337 
338  /* Similar to above, libtriangle needs an interior point for each
339  region-of-interest in intRegs_. We follow the same assumption as before
340  that each region-of-interest is convex. */
341  in.numberofregions = intRegs_.size();
342  in.regionlist = nullptr;
343  if (in.numberofregions > 0)
344  {
345  /* regionlist is a sequence (x1 y1 L1 -1 x2 y2 L2 -1 ...) of ordered triples,
346  each ended with -1. The i^th ordered pair (xi,yi,Li) is an interior point
347  of the i^th region-of-interest in intRegs_, which is assigned the integer
348  label Li. */
349  in.regionlist = (REAL *)malloc(4 * in.numberofregions * sizeof(REAL));
350  for (unsigned int i = 0; i < intRegs_.size(); ++i)
351  {
352  Vertex v = getPointInPoly(intRegs_[i]);
353  in.regionlist[4 * i] = v.x;
354  in.regionlist[4 * i + 1] = v.y;
355  // triangles outside of interesting regions get assigned an attribute of zero by default
356  // so let's number our attributes from 1 to numProps, then shift it down by 1 when we're done
357  in.regionlist[4 * i + 2] = (REAL)(i + 1);
358  in.regionlist[4 * i + 3] = -1.;
359  }
360  }
361 
362  // mark remaining input fields as unused
363  in.segmentmarkerlist = (int *)nullptr;
364  in.numberofpointattributes = 0;
365  in.pointattributelist = nullptr;
366  in.pointmarkerlist = nullptr;
367 
368  // initialize output libtriangle structure, which will hold the results of the triangulation
369  struct triangulateio out;
370  out.pointlist = (REAL *)nullptr;
371  out.pointattributelist = (REAL *)nullptr;
372  out.pointmarkerlist = (int *)nullptr;
373  out.trianglelist = (int *)nullptr;
374  out.triangleattributelist = (REAL *)nullptr;
375  out.neighborlist = (int *)nullptr;
376  out.segmentlist = (int *)nullptr;
377  out.segmentmarkerlist = (int *)nullptr;
378  out.edgelist = (int *)nullptr;
379  out.edgemarkerlist = (int *)nullptr;
380  out.pointlist = (REAL *)nullptr;
381  out.pointattributelist = (REAL *)nullptr;
382  out.trianglelist = (int *)nullptr;
383  out.triangleattributelist = (REAL *)nullptr;
384 
385  // call the triangulation routine
386  triangulate(const_cast<char *>(triswitches.c_str()), &in, &out, nullptr);
387 
388  triangles_.resize(out.numberoftriangles);
389  intRegInfo_.resize(out.numberoftriangles);
390  for (int i = 0; i < out.numberoftriangles; ++i)
391  {
392  Triangle &t = triangles_[i];
393  for (int j = 0; j < 3; ++j)
394  {
395  t.pts[j].x = out.pointlist[2 * out.trianglelist[3 * i + j]];
396  t.pts[j].y = out.pointlist[2 * out.trianglelist[3 * i + j] + 1];
397  if (out.neighborlist[3 * i + j] >= 0)
398  t.neighbors.push_back(out.neighborlist[3 * i + j]);
399  }
400  t.volume = -1.;
401 
402  if (in.numberofregions > 0)
403  {
404  auto attribute = (int)out.triangleattributelist[i];
405  /* Shift the region-of-interest ID's down to start from zero. */
406  intRegInfo_[i] = (attribute > 0 ? attribute - 1 : -1);
407  }
408  }
409 
410  trifree(in.pointlist);
411  trifree(in.segmentlist);
412  if (in.numberofholes > 0)
413  trifree(in.holelist);
414  if (in.numberofregions > 0)
415  trifree(in.regionlist);
416  trifree(out.pointlist);
417  trifree(out.pointattributelist);
418  trifree(out.pointmarkerlist);
419  trifree(out.trianglelist);
420  trifree(out.triangleattributelist);
421  trifree(out.neighborlist);
422  trifree(out.edgelist);
423  trifree(out.edgemarkerlist);
424  trifree(out.segmentlist);
425  trifree(out.segmentmarkerlist);
426 
427  return out.numberoftriangles;
428 }
429 
430 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(const std::vector<Triangle> &triangles)
431 {
432  regToTriangles_.resize(getNumRegions());
433  std::vector<double> bboxLow(2);
434  std::vector<double> bboxHigh(2);
435  std::vector<int> gridCoord[2];
436  for (unsigned int i = 0; i < triangles.size(); ++i)
437  {
438  /* for Triangle tri, compute the smallest rectangular
439  * bounding box that contains tri. */
440  const Triangle &tri = triangles[i];
441  bboxLow[0] = tri.pts[0].x;
442  bboxLow[1] = tri.pts[0].y;
443  bboxHigh[0] = bboxLow[0];
444  bboxHigh[1] = bboxLow[1];
445 
446  for (int j = 1; j < 3; ++j)
447  {
448  if (tri.pts[j].x < bboxLow[0])
449  bboxLow[0] = tri.pts[j].x;
450  else if (tri.pts[j].x > bboxHigh[0])
451  bboxHigh[0] = tri.pts[j].x;
452  if (tri.pts[j].y < bboxLow[1])
453  bboxLow[1] = tri.pts[j].y;
454  else if (tri.pts[j].y > bboxHigh[1])
455  bboxHigh[1] = tri.pts[j].y;
456  }
457 
458  /* Convert the bounding box into grid cell coordinates */
459 
460  coordToGridCoord(bboxLow, gridCoord[0]);
461  coordToGridCoord(bboxHigh, gridCoord[1]);
462 
463  /* Every grid cell within bounding box gets
464  tri added to its map entry */
465  std::vector<int> c(2);
466  for (int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
467  {
468  for (int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
469  {
470  c[0] = x;
471  c[1] = y;
472  int cellID = gridCoordToRegion(c);
473  regToTriangles_[cellID].push_back(i);
474  }
475  }
476  }
477 }
478 
479 void ompl::control::TriangularDecomposition::buildLocatorGrid()
480 {
481  locator.buildTriangleMap(triangles_);
482 }
483 
484 bool ompl::control::TriangularDecomposition::triContains(const Triangle &tri, const std::vector<double> &coord)
485 {
486  for (int i = 0; i < 3; ++i)
487  {
488  /* point (coord[0],coord[1]) needs to be to the left of
489  the vector from (ax,ay) to (bx,by) */
490  const double ax = tri.pts[i].x;
491  const double ay = tri.pts[i].y;
492  const double bx = tri.pts[(i + 1) % 3].x;
493  const double by = tri.pts[(i + 1) % 3].y;
494 
495  // return false if the point is instead to the right of the vector
496  if ((coord[0] - ax) * (by - ay) - (bx - ax) * (coord[1] - ay) > 0.)
497  return false;
498  }
499  return true;
500 }
501 
503 ompl::control::TriangularDecomposition::getPointInPoly(const Polygon &poly)
504 {
505  Vertex p;
506  p.x = 0.;
507  p.y = 0.;
508  for (auto pt : poly.pts)
509  {
510  p.x += pt.x;
511  p.y += pt.y;
512  }
513  p.x /= poly.pts.size();
514  p.y /= poly.pts.size();
515  return p;
516 }
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...
std::vector< double > low
Lower bound.
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:68
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:58
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
Definition: Decomposition.h:89
std::vector< double > high
Upper bound.
int getNumRegions() const override
Returns the number of regions in this Decomposition.
Definition of an abstract state.
Definition: State.h:49
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
std::vector< int > intRegInfo_
Maps from triangle ID to index of Polygon in intReg_ that contains the triangle ID. Maps to -1 if the triangle ID is not in a region of interest.
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...
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.
double getVolume() const
Compute the volume of the space enclosed by the bounds.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68