All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 <boost/lexical_cast.hpp>
39 
40 extern "C"
41 {
42  #define REAL double
43  #define VOID void
44  #define ANSI_DECLARATORS
45  #include <triangle.h>
46 }
47 
48 ompl::control::TriangularDecomposition::TriangularDecomposition(unsigned int dim, const base::RealVectorBounds& b, const std::vector<Polygon>& holes) :
49  Decomposition(dim, b),
50  holes_(holes),
51  locator(64, this)
52 {
53  unsigned int numTriangles = createTriangles();
54  OMPL_INFORM("Created %u triangles", numTriangles);
55  setNumRegions(numTriangles);
56  buildLocatorGrid();
57 }
58 
60 {
61  Triangle& tri = triangles_[triID];
62  if (tri.volume < 0)
63  {
64  /* This triangle area formula relies on the vertices being
65  * stored in counter-clockwise order. */
66  tri.volume = 0.5*(
67  (tri.pts[0].x-tri.pts[2].x)*(tri.pts[1].y-tri.pts[0].y)
68  - (tri.pts[0].x-tri.pts[1].x)*(tri.pts[2].y-tri.pts[0].y)
69  );
70  }
71  return tri.volume;
72 }
73 
74 void ompl::control::TriangularDecomposition::sampleFromRegion(unsigned int triID, RNG& rng, std::vector<double>& coord) const
75 {
76  /* Uniformly sample a point from within a triangle, using the approach discussed in
77  * http://math.stackexchange.com/questions/18686/uniform-random-point-in-triangle */
78  const Triangle& tri = triangles_[triID];
79  coord.resize(2);
80  const double r1 = sqrt(rng.uniform01());
81  const double r2 = rng.uniform01();
82  coord[0] = (1-r1)*tri.pts[0].x + r1*(1-r2)*tri.pts[1].x + r1*r2*tri.pts[2].x;
83  coord[1] = (1-r1)*tri.pts[0].y + r1*(1-r2)*tri.pts[1].y + r1*r2*tri.pts[2].y;
84 }
85 
86 void ompl::control::TriangularDecomposition::getNeighbors(unsigned int triID, std::vector<unsigned int>& neighbors) const
87 {
88  neighbors = triangles_[triID].neighbors;
89 }
90 
92 {
93  std::vector<double> coord(2);
94  project(s, coord);
95  const std::vector<unsigned int>& gridTriangles = locator.locateTriangles(s);
96  int triangle = -1;
97  for (std::vector<unsigned int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
98  {
99  unsigned int triID = *i;
100  if (triContains(triangles_[triID], coord))
101  {
102  if (triangle >= 0)
103  OMPL_ERROR("Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles.\n", coord[0], coord[1]);
104  triangle = triID;
105  }
106  }
107  return triangle;
108 }
109 
111 {
112  /* create a conforming Delaunay triangulation
113  where each triangle takes up no more than 0.03% of
114  the total area of the decomposition space */
115  const base::RealVectorBounds& bounds = getBounds();
116  const double maxTriangleArea = bounds.getVolume()*0.0003;
117  std::string triswitches = "pDznQ -a" + boost::lexical_cast<std::string>(maxTriangleArea);
118  struct triangulateio in;
119 
120  in.numberofpoints = 4;
121  //each obstacle vertex is also considered a hole
122  for (std::vector<Polygon>::const_iterator i = holes_.begin(); i != holes_.end(); ++i)
123  in.numberofpoints += i->pts.size();
124 
125  in.numberofpointattributes = 0;
126  in.pointlist = (REAL*) malloc(2*in.numberofpoints*sizeof(REAL));
127  in.pointlist[0] = bounds.low[0];
128  in.pointlist[1] = bounds.low[1];
129  in.pointlist[2] = bounds.high[0];
130  in.pointlist[3] = bounds.low[1];
131  in.pointlist[4] = bounds.high[0];
132  in.pointlist[5] = bounds.high[1];
133  in.pointlist[6] = bounds.low[0];
134  in.pointlist[7] = bounds.high[1];
135 
136  in.pointattributelist = NULL;
137  in.pointmarkerlist = NULL;
138 
139  in.numberofsegments = in.numberofpoints;
140  in.segmentlist = (int*) malloc(2*in.numberofsegments*sizeof(int));
141  //first add segments for bounding box
142  for (int i = 0; i < 4; ++i)
143  {
144  in.segmentlist[2*i] = i;
145  in.segmentlist[2*i+1] = (i+1) % 4;
146  }
147  //now add points and segments for each obstacle, if any exist
148  int pointIndex = 4;
149  for (std::vector<Polygon>::const_iterator i = holes_.begin(); i != holes_.end(); ++i)
150  {
151  const Polygon& p = *i;
152  for (unsigned int j = 0; j < p.pts.size(); ++j)
153  {
154  in.pointlist[2*(pointIndex+j)] = p.pts[j].x;
155  in.pointlist[2*(pointIndex+j)+1] = p.pts[j].y;
156  in.segmentlist[2*(pointIndex+j)] = pointIndex + j;
157  in.segmentlist[2*(pointIndex+j)+1] = pointIndex + (j+1)%p.pts.size();
158  }
159  pointIndex += p.pts.size();
160  }
161  in.segmentmarkerlist = (int*) NULL;
162 
163  in.numberofholes = holes_.size();
164  in.holelist = NULL;
165  if (in.numberofholes > 0)
166  {
167  in.holelist = (REAL*) malloc(2*in.numberofholes*sizeof(REAL));
168  for (unsigned int i = 0; i < holes_.size(); ++i)
169  {
170  Vertex v = pointWithinPoly(holes_[i]);
171  in.holelist[2*i] = v.x;
172  in.holelist[2*i+1] = v.y;
173  }
174  }
175  in.numberofregions = 0;
176  in.regionlist = NULL;
177 
178  struct triangulateio out;
179  out.pointlist = (REAL*) NULL;
180  out.pointattributelist = (REAL*) NULL;
181  out.pointmarkerlist = (int*) NULL;
182  out.trianglelist = (int*) NULL;
183  out.triangleattributelist = (REAL*) NULL;
184  out.neighborlist = (int*) NULL;
185  out.segmentlist = (int*) NULL;
186  out.segmentmarkerlist = (int*) NULL;
187  out.edgelist = (int*) NULL;
188  out.edgemarkerlist = (int*) NULL;
189  out.pointlist = (REAL*) NULL;
190  out.pointattributelist = (REAL*) NULL;
191  out.trianglelist = (int*) NULL;
192  out.triangleattributelist = (REAL*) NULL;
193 
194  struct triangulateio* vorout = NULL;
195  triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, vorout);
196 
197  triangles_.resize(out.numberoftriangles);
198  for (int i = 0; i < out.numberoftriangles; ++i)
199  {
200  Triangle& t = triangles_[i];
201  for (int j = 0; j < 3; ++j)
202  {
203  t.pts[j].x = out.pointlist[2*out.trianglelist[3*i+j]];
204  t.pts[j].y = out.pointlist[2*out.trianglelist[3*i+j]+1];
205  if (out.neighborlist[3*i+j] >= 0)
206  t.neighbors.push_back(out.neighborlist[3*i+j]);
207  }
208  t.volume = -1.;
209  }
210 
211  trifree(in.pointlist);
212  trifree(in.segmentlist);
213  if (in.numberofholes > 0)
214  trifree(in.holelist);
215  trifree(out.pointlist);
216  trifree(out.pointmarkerlist);
217  trifree(out.trianglelist);
218  trifree(out.neighborlist);
219  trifree(out.edgelist);
220  trifree(out.edgemarkerlist);
221  trifree(out.segmentlist);
222  trifree(out.segmentmarkerlist);
223 
224  return out.numberoftriangles;
225 }
226 
227 void ompl::control::TriangularDecomposition::print(std::ostream& out) const
228 {
229  //number of triangles
230  out << triangles_.size() << std::endl;
231 
232  //print all triangles; each triplet of lines determines a triangle
233  for (unsigned int i = 0; i < triangles_.size(); ++i)
234  {
235  const Triangle& tri = triangles_[i];
236  // <x> <y>
237  for (unsigned int v = 0; v < 3; ++v)
238  out << tri.pts[v].x << " " << tri.pts[v].y << " ";
239  out << "-1" << std::endl;
240  }
241 }
242 
243 void ompl::control::TriangularDecomposition::buildLocatorGrid()
244 {
245  locator.buildTriangleMap(triangles_);
246 }
247 
248 bool ompl::control::TriangularDecomposition::triContains(const Triangle& tri, const std::vector<double>& coord) const
249 {
250  for (int i = 0; i < 3; ++i)
251  {
252  /* point (coord[0],coord[1]) needs to be to the left of
253  the vector from (ax,ay) to (bx,by) */
254  const double ax = tri.pts[i].x;
255  const double ay = tri.pts[i].y;
256  const double bx = tri.pts[(i+1)%3].x;
257  const double by = tri.pts[(i+1)%3].y;
258 
259  // return false if the point is instead to the right of the vector
260  if ((coord[0]-ax)*(by-ay) - (bx-ax)*(coord[1]-ay) > 0.)
261  return false;
262  }
263  return true;
264 }
265 
266 ompl::control::TriangularDecomposition::Vertex ompl::control::TriangularDecomposition::pointWithinPoly(const Polygon& poly) const
267 {
268  Vertex p;
269  p.x = 0.;
270  p.y = 0.;
271  for (std::vector<Vertex>::const_iterator i = poly.pts.begin(); i != poly.pts.end(); ++i)
272  {
273  p.x += i->x;
274  p.y += i->y;
275  }
276  p.x /= poly.pts.size();
277  p.y /= poly.pts.size();
278  return p;
279 }
280 
281 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(const std::vector<Triangle>& triangles)
282 {
283  regToTriangles_.resize(getNumRegions());
284  std::vector<double> bboxLow(2);
285  std::vector<double> bboxHigh(2);
286  std::vector<unsigned int> gridCoord[2];
287  for (unsigned int i = 0; i < triangles.size(); ++i)
288  {
289  /* for Triangle tri, compute the smallest rectangular
290  * bounding box that contains tri. */
291  const Triangle& tri = triangles[i];
292  bboxLow[0] = tri.pts[0].x;
293  bboxLow[1] = tri.pts[0].y;
294  bboxHigh[0] = bboxLow[0];
295  bboxHigh[1] = bboxLow[1];
296 
297  for (unsigned int j = 1; j < 3; ++j)
298  {
299  if (tri.pts[j].x < bboxLow[0])
300  bboxLow[0] = tri.pts[j].x;
301  else if (tri.pts[j].x > bboxHigh[0])
302  bboxHigh[0] = tri.pts[j].x;
303  if (tri.pts[j].y < bboxLow[1])
304  bboxLow[1] = tri.pts[j].y;
305  else if (tri.pts[j].y > bboxHigh[1])
306  bboxHigh[1] = tri.pts[j].y;
307  }
308 
309  /* Convert the bounding box into grid cell coordinates */
310 
311  coordToGridCoord(bboxLow, gridCoord[0]);
312  coordToGridCoord(bboxHigh, gridCoord[1]);
313 
314  /* Every grid cell within bounding box gets
315  tri added to its map entry */
316  std::vector<unsigned int> c(2);
317  for (unsigned int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
318  {
319  for (unsigned int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
320  {
321  c[0] = x;
322  c[1] = y;
323  unsigned int cellID = gridCoordToRegion(c);
324  regToTriangles_[cellID].push_back(i);
325  }
326  }
327  }
328 }
std::vector< double > low
Lower bound.
virtual void getNeighbors(unsigned int triID, std::vector< unsigned int > &neighbors) const
Stores a given region'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...
TriangularDecomposition(unsigned int dim, const base::RealVectorBounds &b, const std::vector< Polygon > &holes=std::vector< Polygon >())
Constructor. Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional...
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
Definition: Decomposition.h:62
virtual void sampleFromRegion(unsigned int triID, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
double getVolume(void) const
Compute the volume of the space enclosed by the bounds.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
virtual unsigned int createTriangles()
Helper method to triangulate the space and return the number of triangles.
std::vector< double > high
Upper bound.
virtual double getRegionVolume(unsigned int triID)
Returns the volume of a given region in this Decomposition.
Definition of an abstract state.
Definition: State.h:50
The lower and upper bounds for an Rn space.
double uniform01(void)
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68