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 "ompl/util/Hash.h" 50 #include <unordered_map> 56 #define ANSI_DECLARATORS 63 struct hash<
ompl::control::TriangularDecomposition::Vertex>
67 std::size_t hash = std::hash<double>()(v.x);
68 ompl::hash_combine(hash, v.y);
75 std::vector<Polygon> holes,
76 std::vector<Polygon> intRegs)
78 , holes_(
std::move(holes))
79 , intRegs_(
std::move(intRegs))
87 ompl::control::TriangularDecomposition::~TriangularDecomposition() =
default;
89 void ompl::control::TriangularDecomposition::setup()
91 int numTriangles = createTriangles();
96 void ompl::control::TriangularDecomposition::addHole(
const Polygon &hole)
98 holes_.push_back(hole);
101 void ompl::control::TriangularDecomposition::addRegionOfInterest(
const Polygon ®ion)
103 intRegs_.push_back(region);
106 int ompl::control::TriangularDecomposition::getNumHoles()
const 108 return holes_.size();
111 int ompl::control::TriangularDecomposition::getNumRegionsOfInterest()
const 113 return intRegs_.size();
116 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
117 ompl::control::TriangularDecomposition::getHoles()
const 122 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
123 ompl::control::TriangularDecomposition::getAreasOfInterest()
const 130 return intRegInfo_[triID];
140 tri.volume = 0.5 * ((tri.pts[0].x - tri.pts[2].x) * (tri.pts[1].y - tri.pts[0].y) -
141 (tri.pts[0].x - tri.pts[1].x) * (tri.pts[2].y - tri.pts[0].y));
148 neighbors = triangles_[triID].neighbors;
153 std::vector<double> coord(2);
155 const std::vector<int> &gridTriangles = locator.locateTriangles(s);
157 for (
int triID : gridTriangles)
159 if (triContains(triangles_[triID], coord))
162 OMPL_WARN(
"Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles. \ 163 This can happen if the coordinate is located exactly on a triangle segment.\n",
175 const Triangle &tri = triangles_[triID];
179 coord[0] = (1 - r1) * tri.pts[0].x + r1 * (1 - r2) * tri.pts[1].x + r1 * r2 * tri.pts[2].x;
180 coord[1] = (1 - r1) * tri.pts[0].y + r1 * (1 - r2) * tri.pts[1].y + r1 * r2 * tri.pts[2].y;
183 void ompl::control::TriangularDecomposition::print(std::ostream &out)
const 190 for (
unsigned int i = 0; i < triangles_.size(); ++i)
193 const Triangle &tri = triangles_[i];
194 for (
int v = 0; v < 3; ++v)
195 out << tri.pts[v].x <<
" " << tri.pts[v].y <<
" ";
196 if (intRegInfo_[i] > -1)
197 out << intRegInfo_[i] <<
" ";
198 out <<
"-1" << std::endl;
202 ompl::control::TriangularDecomposition::Vertex::Vertex(
double vx,
double vy) : x(vx), y(vy)
206 bool ompl::control::TriangularDecomposition::Vertex::operator==(
const Vertex &v)
const 208 return x == v.x && y == v.y;
217 const double maxTriangleArea = bounds.
getVolume() * triAreaPct_;
218 std::string triswitches =
"pDznQA -a" + std::to_string(maxTriangleArea);
219 struct triangulateio in;
227 std::unordered_map<Vertex, int> pointIndex;
238 in.numberofpoints = 4;
239 in.numberofsegments = 4;
241 using PolyIter = std::vector<Polygon>::const_iterator;
242 using VertexIter = std::vector<Vertex>::const_iterator;
245 for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
247 for (
auto pt : p->pts)
249 ++in.numberofsegments;
252 if (pointIndex.find(pt) == pointIndex.end())
253 pointIndex[pt] = in.numberofpoints++;
259 for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
261 for (
auto pt : p->pts)
263 ++in.numberofsegments;
264 if (pointIndex.find(pt) == pointIndex.end())
265 pointIndex[pt] = in.numberofpoints++;
270 in.pointlist = (REAL *)malloc(2 * in.numberofpoints *
sizeof(REAL));
273 for (
const auto &i : pointIndex)
275 const Vertex &v = i.first;
276 int index = i.second;
277 in.pointlist[2 * index] = v.x;
278 in.pointlist[2 * index + 1] = v.y;
283 in.segmentlist = (
int *)malloc(2 * in.numberofsegments *
sizeof(
int));
286 for (
int i = 0; i < 4; ++i)
288 in.segmentlist[2 * i] = i;
289 in.segmentlist[2 * i + 1] = (i + 1) % 4;
298 for (PolyIter p = holes_.begin(); p != holes_.end(); ++p)
300 for (
unsigned int j = 0; j < p->pts.size(); ++j)
302 in.segmentlist[2 * segIndex] = pointIndex[p->pts[j]];
303 in.segmentlist[2 * segIndex + 1] = pointIndex[p->pts[(j + 1) % p->pts.size()]];
310 for (PolyIter p = intRegs_.begin(); p != intRegs_.end(); ++p)
312 for (
unsigned int j = 0; j < p->pts.size(); ++j)
314 in.segmentlist[2 * segIndex] = pointIndex[p->pts[j]];
315 in.segmentlist[2 * segIndex + 1] = pointIndex[p->pts[(j + 1) % p->pts.size()]];
323 in.numberofholes = holes_.size();
324 in.holelist =
nullptr;
325 if (in.numberofholes > 0)
329 in.holelist = (REAL *)malloc(2 * in.numberofholes *
sizeof(REAL));
330 for (
int i = 0; i < in.numberofholes; ++i)
332 Vertex v = getPointInPoly(holes_[i]);
333 in.holelist[2 * i] = v.x;
334 in.holelist[2 * i + 1] = v.y;
341 in.numberofregions = intRegs_.size();
342 in.regionlist =
nullptr;
343 if (in.numberofregions > 0)
349 in.regionlist = (REAL *)malloc(4 * in.numberofregions *
sizeof(REAL));
350 for (
unsigned int i = 0; i < intRegs_.size(); ++i)
352 Vertex v = getPointInPoly(intRegs_[i]);
353 in.regionlist[4 * i] = v.x;
354 in.regionlist[4 * i + 1] = v.y;
357 in.regionlist[4 * i + 2] = (REAL)(i + 1);
358 in.regionlist[4 * i + 3] = -1.;
363 in.segmentmarkerlist = (
int *)
nullptr;
364 in.numberofpointattributes = 0;
365 in.pointattributelist =
nullptr;
366 in.pointmarkerlist =
nullptr;
369 struct triangulateio out;
370 out.pointlist = (REAL *)
nullptr;
371 out.pointattributelist = (REAL *)
nullptr;
372 out.pointmarkerlist = (
int *)
nullptr;
373 out.trianglelist = (
int *)
nullptr;
374 out.triangleattributelist = (REAL *)
nullptr;
375 out.neighborlist = (
int *)
nullptr;
376 out.segmentlist = (
int *)
nullptr;
377 out.segmentmarkerlist = (
int *)
nullptr;
378 out.edgelist = (
int *)
nullptr;
379 out.edgemarkerlist = (
int *)
nullptr;
380 out.pointlist = (REAL *)
nullptr;
381 out.pointattributelist = (REAL *)
nullptr;
382 out.trianglelist = (
int *)
nullptr;
383 out.triangleattributelist = (REAL *)
nullptr;
386 triangulate(const_cast<char *>(triswitches.c_str()), &in, &out,
nullptr);
388 triangles_.resize(out.numberoftriangles);
389 intRegInfo_.resize(out.numberoftriangles);
390 for (
int i = 0; i < out.numberoftriangles; ++i)
393 for (
int j = 0; j < 3; ++j)
395 t.pts[j].x = out.pointlist[2 * out.trianglelist[3 * i + j]];
396 t.pts[j].y = out.pointlist[2 * out.trianglelist[3 * i + j] + 1];
397 if (out.neighborlist[3 * i + j] >= 0)
398 t.neighbors.push_back(out.neighborlist[3 * i + j]);
402 if (in.numberofregions > 0)
404 auto attribute = (int)out.triangleattributelist[i];
406 intRegInfo_[i] = (attribute > 0 ? attribute - 1 : -1);
410 trifree(in.pointlist);
411 trifree(in.segmentlist);
412 if (in.numberofholes > 0)
413 trifree(in.holelist);
414 if (in.numberofregions > 0)
415 trifree(in.regionlist);
416 trifree(out.pointlist);
417 trifree(out.pointattributelist);
418 trifree(out.pointmarkerlist);
419 trifree(out.trianglelist);
420 trifree(out.triangleattributelist);
421 trifree(out.neighborlist);
422 trifree(out.edgelist);
423 trifree(out.edgemarkerlist);
424 trifree(out.segmentlist);
425 trifree(out.segmentmarkerlist);
427 return out.numberoftriangles;
430 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(
const std::vector<Triangle> &triangles)
432 regToTriangles_.resize(getNumRegions());
433 std::vector<double> bboxLow(2);
434 std::vector<double> bboxHigh(2);
435 std::vector<int> gridCoord[2];
436 for (
unsigned int i = 0; i < triangles.size(); ++i)
440 const Triangle &tri = triangles[i];
441 bboxLow[0] = tri.pts[0].x;
442 bboxLow[1] = tri.pts[0].y;
443 bboxHigh[0] = bboxLow[0];
444 bboxHigh[1] = bboxLow[1];
446 for (
int j = 1; j < 3; ++j)
448 if (tri.pts[j].x < bboxLow[0])
449 bboxLow[0] = tri.pts[j].x;
450 else if (tri.pts[j].x > bboxHigh[0])
451 bboxHigh[0] = tri.pts[j].x;
452 if (tri.pts[j].y < bboxLow[1])
453 bboxLow[1] = tri.pts[j].y;
454 else if (tri.pts[j].y > bboxHigh[1])
455 bboxHigh[1] = tri.pts[j].y;
460 coordToGridCoord(bboxLow, gridCoord[0]);
461 coordToGridCoord(bboxHigh, gridCoord[1]);
465 std::vector<int> c(2);
466 for (
int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
468 for (
int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
472 int cellID = gridCoordToRegion(c);
473 regToTriangles_[cellID].push_back(i);
479 void ompl::control::TriangularDecomposition::buildLocatorGrid()
481 locator.buildTriangleMap(triangles_);
484 bool ompl::control::TriangularDecomposition::triContains(
const Triangle &tri,
const std::vector<double> &coord)
486 for (
int i = 0; i < 3; ++i)
490 const double ax = tri.pts[i].x;
491 const double ay = tri.pts[i].y;
492 const double bx = tri.pts[(i + 1) % 3].x;
493 const double by = tri.pts[(i + 1) % 3].y;
496 if ((coord[0] - ax) * (by - ay) - (bx - ax) * (coord[1] - ay) > 0.)
503 ompl::control::TriangularDecomposition::getPointInPoly(
const Polygon &poly)
508 for (
auto pt : poly.pts)
513 p.x /= poly.pts.size();
514 p.y /= poly.pts.size();
int locateRegion(const base::State *s) const override
Returns the index of the region containing a given State. Most often, this is obtained by first calli...
std::vector< double > low
Lower bound.
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
TriangularDecomposition(const base::RealVectorBounds &bounds, std::vector< Polygon > holes=std::vector< Polygon >(), 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.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
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...
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.
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
double getVolume() const
Compute the volume of the space enclosed by the bounds.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.