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"
45 #include "ompl/util/String.h"
51 #include <unordered_map>
57 #define ANSI_DECLARATORS
64 struct hash<
ompl::control::TriangularDecomposition::Vertex>
68 std::size_t hash = std::hash<double>()(v.x);
69 ompl::hash_combine(hash, v.y);
76 std::vector<Polygon> holes,
77 std::vector<Polygon> intRegs)
79 , holes_(std::move(holes))
80 , intRegs_(std::move(intRegs))
88 ompl::control::TriangularDecomposition::~TriangularDecomposition() =
default;
90 void ompl::control::TriangularDecomposition::setup()
92 int numTriangles = createTriangles();
97 void ompl::control::TriangularDecomposition::addHole(
const Polygon &hole)
99 holes_.push_back(hole);
102 void ompl::control::TriangularDecomposition::addRegionOfInterest(
const Polygon ®ion)
104 intRegs_.push_back(region);
107 int ompl::control::TriangularDecomposition::getNumHoles()
const
109 return holes_.size();
112 int ompl::control::TriangularDecomposition::getNumRegionsOfInterest()
const
114 return intRegs_.size();
117 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
118 ompl::control::TriangularDecomposition::getHoles()
const
123 const std::vector<ompl::control::TriangularDecomposition::Polygon> &
124 ompl::control::TriangularDecomposition::getAreasOfInterest()
const
131 return intRegInfo_[triID];
141 tri.volume = 0.5 * ((tri.pts[0].x - tri.pts[2].x) * (tri.pts[1].y - tri.pts[0].y) -
142 (tri.pts[0].x - tri.pts[1].x) * (tri.pts[2].y - tri.pts[0].y));
149 neighbors = triangles_[triID].neighbors;
154 std::vector<double> coord(2);
156 const std::vector<int> &gridTriangles = locator.locateTriangles(s);
158 for (
int triID : gridTriangles)
160 if (triContains(triangles_[triID], coord))
163 OMPL_WARN(
"Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles. \
164 This can happen if the coordinate is located exactly on a triangle segment.\n",
176 const Triangle &tri = triangles_[triID];
180 coord[0] = (1 - r1) * tri.pts[0].x + r1 * (1 - r2) * tri.pts[1].x + r1 * r2 * tri.pts[2].x;
181 coord[1] = (1 - r1) * tri.pts[0].y + r1 * (1 - r2) * tri.pts[1].y + r1 * r2 * tri.pts[2].y;
184 void ompl::control::TriangularDecomposition::print(std::ostream &out)
const
191 for (
unsigned int i = 0; i < triangles_.size(); ++i)
194 const Triangle &tri = triangles_[i];
195 for (
int v = 0; v < 3; ++v)
196 out << tri.pts[v].x <<
" " << tri.pts[v].y <<
" ";
197 if (intRegInfo_[i] > -1)
198 out << intRegInfo_[i] <<
" ";
199 out <<
"-1" << std::endl;
203 ompl::control::TriangularDecomposition::Vertex::Vertex(
double vx,
double vy) : x(vx), y(vy)
207 bool ompl::control::TriangularDecomposition::Vertex::operator==(
const Vertex &v)
const
209 return x == v.x && y == v.y;
218 const double maxTriangleArea = bounds.getVolume() * triAreaPct_;
219 std::string triswitches =
"pDznQA -a" +
ompl::toString(maxTriangleArea);
220 struct triangulateio in;
228 std::unordered_map<Vertex, int> pointIndex;
231 pointIndex[
Vertex(bounds.low[0], bounds.low[1])] = 0;
232 pointIndex[
Vertex(bounds.high[0], bounds.low[1])] = 1;
233 pointIndex[
Vertex(bounds.high[0], bounds.high[1])] = 2;
234 pointIndex[
Vertex(bounds.low[0], bounds.high[1])] = 3;
239 in.numberofpoints = 4;
240 in.numberofsegments = 4;
243 for (
auto &p : holes_)
245 for (
auto &pt : p.pts)
247 ++in.numberofsegments;
250 if (pointIndex.find(pt) == pointIndex.end())
251 pointIndex[pt] = in.numberofpoints++;
257 for (
auto &p : intRegs_)
259 for (
auto &pt : p.pts)
261 ++in.numberofsegments;
262 if (pointIndex.find(pt) == pointIndex.end())
263 pointIndex[pt] = in.numberofpoints++;
268 in.pointlist = (REAL *)malloc(2 * in.numberofpoints *
sizeof(REAL));
271 for (
const auto &i : pointIndex)
273 const Vertex &v = i.first;
274 int index = i.second;
275 in.pointlist[2 * index] = v.x;
276 in.pointlist[2 * index + 1] = v.y;
281 in.segmentlist = (
int *)malloc(2 * in.numberofsegments *
sizeof(
int));
284 for (
int i = 0; i < 4; ++i)
286 in.segmentlist[2 * i] = i;
287 in.segmentlist[2 * i + 1] = (i + 1) % 4;
296 for (
auto &p : holes_)
298 for (
unsigned int j = 0; j < p.pts.size(); ++j)
300 in.segmentlist[2 * segIndex] = pointIndex[p.pts[j]];
301 in.segmentlist[2 * segIndex + 1] = pointIndex[p.pts[(j + 1) % p.pts.size()]];
308 for (
auto &p : intRegs_)
310 for (
unsigned int j = 0; j < p.pts.size(); ++j)
312 in.segmentlist[2 * segIndex] = pointIndex[p.pts[j]];
313 in.segmentlist[2 * segIndex + 1] = pointIndex[p.pts[(j + 1) % p.pts.size()]];
321 in.numberofholes = holes_.size();
322 in.holelist =
nullptr;
323 if (in.numberofholes > 0)
327 in.holelist = (REAL *)malloc(2 * in.numberofholes *
sizeof(REAL));
328 for (
int i = 0; i < in.numberofholes; ++i)
330 Vertex v = getPointInPoly(holes_[i]);
331 in.holelist[2 * i] = v.x;
332 in.holelist[2 * i + 1] = v.y;
339 in.numberofregions = intRegs_.size();
340 in.regionlist =
nullptr;
341 if (in.numberofregions > 0)
347 in.regionlist = (REAL *)malloc(4 * in.numberofregions *
sizeof(REAL));
348 for (
unsigned int i = 0; i < intRegs_.size(); ++i)
350 Vertex v = getPointInPoly(intRegs_[i]);
351 in.regionlist[4 * i] = v.x;
352 in.regionlist[4 * i + 1] = v.y;
355 in.regionlist[4 * i + 2] = (REAL)(i + 1);
356 in.regionlist[4 * i + 3] = -1.;
361 in.segmentmarkerlist = (
int *)
nullptr;
362 in.numberofpointattributes = 0;
363 in.pointattributelist =
nullptr;
364 in.pointmarkerlist =
nullptr;
367 struct triangulateio out;
368 out.pointlist = (REAL *)
nullptr;
369 out.pointattributelist = (REAL *)
nullptr;
370 out.pointmarkerlist = (
int *)
nullptr;
371 out.trianglelist = (
int *)
nullptr;
372 out.triangleattributelist = (REAL *)
nullptr;
373 out.neighborlist = (
int *)
nullptr;
374 out.segmentlist = (
int *)
nullptr;
375 out.segmentmarkerlist = (
int *)
nullptr;
376 out.edgelist = (
int *)
nullptr;
377 out.edgemarkerlist = (
int *)
nullptr;
378 out.pointlist = (REAL *)
nullptr;
379 out.pointattributelist = (REAL *)
nullptr;
380 out.trianglelist = (
int *)
nullptr;
381 out.triangleattributelist = (REAL *)
nullptr;
384 triangulate(
const_cast<char *
>(triswitches.c_str()), &in, &out,
nullptr);
386 triangles_.resize(out.numberoftriangles);
387 intRegInfo_.resize(out.numberoftriangles);
388 for (
int i = 0; i < out.numberoftriangles; ++i)
391 for (
int j = 0; j < 3; ++j)
393 t.pts[j].x = out.pointlist[2 * out.trianglelist[3 * i + j]];
394 t.pts[j].y = out.pointlist[2 * out.trianglelist[3 * i + j] + 1];
395 if (out.neighborlist[3 * i + j] >= 0)
396 t.neighbors.push_back(out.neighborlist[3 * i + j]);
400 if (in.numberofregions > 0)
402 auto attribute = (int)out.triangleattributelist[i];
404 intRegInfo_[i] = (attribute > 0 ? attribute - 1 : -1);
408 trifree(in.pointlist);
409 trifree(in.segmentlist);
410 if (in.numberofholes > 0)
411 trifree(in.holelist);
412 if (in.numberofregions > 0)
413 trifree(in.regionlist);
414 trifree(out.pointlist);
415 trifree(out.pointattributelist);
416 trifree(out.pointmarkerlist);
417 trifree(out.trianglelist);
418 trifree(out.triangleattributelist);
419 trifree(out.neighborlist);
420 trifree(out.edgelist);
421 trifree(out.edgemarkerlist);
422 trifree(out.segmentlist);
423 trifree(out.segmentmarkerlist);
425 return out.numberoftriangles;
428 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(
const std::vector<Triangle> &triangles)
430 regToTriangles_.resize(getNumRegions());
431 std::vector<double> bboxLow(2);
432 std::vector<double> bboxHigh(2);
433 std::vector<int> gridCoord[2];
434 for (
unsigned int i = 0; i < triangles.size(); ++i)
438 const Triangle &tri = triangles[i];
439 bboxLow[0] = tri.pts[0].x;
440 bboxLow[1] = tri.pts[0].y;
441 bboxHigh[0] = bboxLow[0];
442 bboxHigh[1] = bboxLow[1];
444 for (
int j = 1; j < 3; ++j)
446 if (tri.pts[j].x < bboxLow[0])
447 bboxLow[0] = tri.pts[j].x;
448 else if (tri.pts[j].x > bboxHigh[0])
449 bboxHigh[0] = tri.pts[j].x;
450 if (tri.pts[j].y < bboxLow[1])
451 bboxLow[1] = tri.pts[j].y;
452 else if (tri.pts[j].y > bboxHigh[1])
453 bboxHigh[1] = tri.pts[j].y;
458 coordToGridCoord(bboxLow, gridCoord[0]);
459 coordToGridCoord(bboxHigh, gridCoord[1]);
463 std::vector<int> c(2);
464 for (
int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
466 for (
int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
470 int cellID = gridCoordToRegion(c);
471 regToTriangles_[cellID].push_back(i);
477 void ompl::control::TriangularDecomposition::buildLocatorGrid()
479 locator.buildTriangleMap(triangles_);
482 bool ompl::control::TriangularDecomposition::triContains(
const Triangle &tri,
const std::vector<double> &coord)
484 for (
int i = 0; i < 3; ++i)
488 const double ax = tri.pts[i].x;
489 const double ay = tri.pts[i].y;
490 const double bx = tri.pts[(i + 1) % 3].x;
491 const double by = tri.pts[(i + 1) % 3].y;
494 if ((coord[0] - ax) * (by - ay) - (bx - ax) * (coord[1] - ay) > 0.)
501 ompl::control::TriangularDecomposition::getPointInPoly(
const Polygon &poly)
506 for (
auto pt : poly.pts)
511 p.x /= poly.pts.size();
512 p.y /= poly.pts.size();