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