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"
48 #include <boost/functional/hash.hpp>
49 #include <boost/lexical_cast.hpp>
50 #include <boost/unordered_map.hpp>
57 #define ANSI_DECLARATORS
62 const std::vector<Polygon> &holes,
const std::vector<Polygon> &intRegs) :
73 ompl::control::TriangularDecomposition::~TriangularDecomposition(
void)
77 void ompl::control::TriangularDecomposition::setup(
void)
79 int numTriangles = createTriangles();
84 void ompl::control::TriangularDecomposition::addHole(
const Polygon& hole)
86 holes_.push_back(hole);
89 void ompl::control::TriangularDecomposition::addRegionOfInterest(
const Polygon& region)
91 intRegs_.push_back(region);
94 int ompl::control::TriangularDecomposition::getNumHoles(
void)
const
99 int ompl::control::TriangularDecomposition::getNumRegionsOfInterest(
void)
const
101 return intRegs_.size();
104 const std::vector<ompl::control::TriangularDecomposition::Polygon>&
105 ompl::control::TriangularDecomposition::getHoles(
void)
const
110 const std::vector<ompl::control::TriangularDecomposition::Polygon>&
111 ompl::control::TriangularDecomposition::getAreasOfInterest(
void)
const
118 return intRegInfo_[triID];
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)
138 neighbors = triangles_[triID].neighbors;
143 std::vector<double> coord(2);
145 const std::vector<int>& gridTriangles = locator.locateTriangles(s);
147 for (std::vector<int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
150 if (triContains(triangles_[triID], coord))
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",
166 const Triangle& tri = triangles_[triID];
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;
174 void ompl::control::TriangularDecomposition::print(std::ostream& out)
const
181 for (
unsigned int i = 0; i < triangles_.size(); ++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;
192 ompl::control::TriangularDecomposition::Vertex::Vertex(
double vx,
double vy) : x(vx), y(vy)
196 bool ompl::control::TriangularDecomposition::Vertex::operator==(
const Vertex &v)
const
198 return x == v.x && y == v.y;
205 std::size_t hash_value(
const TriangularDecomposition::Vertex &v)
207 std::size_t hash = 0;
208 boost::hash_combine(hash, v.x);
209 boost::hash_combine(hash, v.y);
221 const double maxTriangleArea = bounds.
getVolume() * triAreaPct_;
222 std::string triswitches =
"pDznQA -a" + boost::lexical_cast<std::string>(maxTriangleArea);
223 struct triangulateio in;
231 boost::unordered_map<Vertex, int> pointIndex;
242 in.numberofpoints = 4;
243 in.numberofsegments = 4;
245 typedef std::vector<Polygon>::const_iterator PolyIter;
246 typedef std::vector<Vertex>::const_iterator VertexIter;
249 for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
251 for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
253 ++in.numberofsegments;
256 if (pointIndex.find(*v) == pointIndex.end())
257 pointIndex[*v] = in.numberofpoints++;
263 for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
265 for (VertexIter v = p->pts.begin(); v != p->pts.end(); ++v)
267 ++in.numberofsegments;
268 if (pointIndex.find(*v) == pointIndex.end())
269 pointIndex[*v] = in.numberofpoints++;
274 in.pointlist = (REAL*) malloc(2*in.numberofpoints*
sizeof(REAL));
277 typedef boost::unordered_map<Vertex, int>::const_iterator IndexIter;
278 for (IndexIter i = pointIndex.begin(); i != pointIndex.end(); ++i)
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;
288 in.segmentlist = (
int*) malloc(2*in.numberofsegments*
sizeof(
int));
291 for (
int i = 0; i < 4; ++i)
293 in.segmentlist[2*i] = i;
294 in.segmentlist[2*i+1] = (i+1) % 4;
303 for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
305 for (
unsigned int j = 0; j < p->pts.size(); ++j)
307 in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
308 in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
315 for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
317 for (
unsigned int j = 0; j < p->pts.size(); ++j)
319 in.segmentlist[2*segIndex] = pointIndex[p->pts[j]];
320 in.segmentlist[2*segIndex+1] = pointIndex[p->pts[(j+1)%p->pts.size()]];
328 in.numberofholes = holes_.size();
330 if (in.numberofholes > 0)
334 in.holelist = (REAL*) malloc(2*in.numberofholes*
sizeof(REAL));
335 for (
int i = 0; i < in.numberofholes; ++i)
337 Vertex v = getPointInPoly(holes_[i]);
338 in.holelist[2*i] = v.x;
339 in.holelist[2*i+1] = v.y;
346 in.numberofregions = intRegs_.size();
347 in.regionlist = NULL;
348 if (in.numberofregions > 0)
354 in.regionlist = (REAL*) malloc(4*in.numberofregions*
sizeof(REAL));
355 for (
unsigned int i = 0; i < intRegs_.size(); ++i)
357 Vertex v = getPointInPoly(intRegs_[i]);
358 in.regionlist[4*i] = v.x;
359 in.regionlist[4*i+1] = v.y;
362 in.regionlist[4*i+2] = (REAL) (i+1);
363 in.regionlist[4*i+3] = -1.;
368 in.segmentmarkerlist = (
int*) NULL;
369 in.numberofpointattributes = 0;
370 in.pointattributelist = NULL;
371 in.pointmarkerlist = NULL;
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;
391 triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, NULL);
393 triangles_.resize(out.numberoftriangles);
394 intRegInfo_.resize(out.numberoftriangles);
395 for (
int i = 0; i < out.numberoftriangles; ++i)
398 for (
int j = 0; j < 3; ++j)
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]);
407 if (in.numberofregions > 0)
409 int attribute = (int) out.triangleattributelist[i];
411 intRegInfo_[i] = (attribute > 0 ? attribute-1 : -1);
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);
432 return out.numberoftriangles;
435 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(
const std::vector<Triangle>& triangles)
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)
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];
451 for (
int j = 1; j < 3; ++j)
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;
465 coordToGridCoord(bboxLow, gridCoord[0]);
466 coordToGridCoord(bboxHigh, gridCoord[1]);
470 std::vector<int> c(2);
471 for (
int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
473 for (
int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
477 int cellID = gridCoordToRegion(c);
478 regToTriangles_[cellID].push_back(i);
484 void ompl::control::TriangularDecomposition::buildLocatorGrid()
486 locator.buildTriangleMap(triangles_);
489 bool ompl::control::TriangularDecomposition::triContains(
const Triangle& tri,
const std::vector<double>& coord)
491 for (
int i = 0; i < 3; ++i)
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;
501 if ((coord[0]-ax)*(by-ay) - (bx-ax)*(coord[1]-ay) > 0.)
512 for (std::vector<Vertex>::const_iterator i = poly.pts.begin(); i != poly.pts.end(); ++i)
517 p.x /= poly.pts.size();
518 p.y /= poly.pts.size();
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 ...
double uniform01()
Generate a random real between 0 and 1.
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.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
std::vector< double > high
Upper bound.
Definition of an abstract state.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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.