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();
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double uniform01()
Generate a random real between 0 and 1.
The lower and upper bounds for an Rn space.
Definition of an abstract state.
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
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....
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...
virtual int createTriangles()
Helper method to triangulate the space and return the number of triangles.
int getRegionOfInterestAt(int triID) const
Returns the region of interest that contains the given triangle ID. Returns -1 if the triangle ID is ...
void sampleFromRegion(int triID, RNG &rng, std::vector< double > &coord) const override
Samples a projected coordinate from a given region.
void getNeighbors(int triID, std::vector< int > &neighbors) const override
Stores a given region's neighbors into a given vector.
double getRegionVolume(int triID) override
Returns the volume of a given region in this Decomposition.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics