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();