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)
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 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 <<
" ";
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;
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);
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)
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)
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();
std::vector< double > low
Lower bound.
virtual void getNeighbors(int triID, std::vector< 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...
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 ...
double uniform01()
Generate a random real between 0 and 1.
Main namespace. Contains everything in this library.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
virtual const base::RealVectorBounds & getBounds() const
Returns the bounds of this Decomposition.
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.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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.