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 }
std::vector< double > low
Lower bound.
virtual void getNeighbors(int triID, std::vector< int > &neighbors) const
Stores a given region&#39;s neighbors into a given vector.
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...
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
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.
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
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
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
Definition: Decomposition.h:91
virtual void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
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
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.
virtual int getNumRegions(void) const
Returns the number of regions in this Decomposition.
double getVolume() const
Compute the volume of the space enclosed by the bounds.
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