37 #include "ompl/extensions/triangle/TriangularDecomposition.h"
38 #include <boost/lexical_cast.hpp>
44 #define ANSI_DECLARATORS
55 setNumRegions(numTriangles);
67 (tri.pts[0].x-tri.pts[2].x)*(tri.pts[1].y-tri.pts[0].y)
68 - (tri.pts[0].x-tri.pts[1].x)*(tri.pts[2].y-tri.pts[0].y)
78 const Triangle& tri = triangles_[triID];
82 coord[0] = (1-r1)*tri.pts[0].x + r1*(1-r2)*tri.pts[1].x + r1*r2*tri.pts[2].x;
83 coord[1] = (1-r1)*tri.pts[0].y + r1*(1-r2)*tri.pts[1].y + r1*r2*tri.pts[2].y;
88 neighbors = triangles_[triID].neighbors;
93 std::vector<double> coord(2);
95 const std::vector<unsigned int>& gridTriangles = locator.locateTriangles(s);
97 for (std::vector<unsigned int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
99 unsigned int triID = *i;
100 if (triContains(triangles_[triID], coord))
103 OMPL_ERROR(
"Decomposition space coordinate (%f,%f) is somehow contained by multiple triangles.\n", coord[0], coord[1]);
116 const double maxTriangleArea = bounds.
getVolume()*0.0003;
117 std::string triswitches =
"pDznQ -a" + boost::lexical_cast<std::string>(maxTriangleArea);
118 struct triangulateio in;
120 in.numberofpoints = 4;
122 for (std::vector<Polygon>::const_iterator i = holes_.begin(); i != holes_.end(); ++i)
123 in.numberofpoints += i->pts.size();
125 in.numberofpointattributes = 0;
126 in.pointlist = (REAL*) malloc(2*in.numberofpoints*
sizeof(REAL));
127 in.pointlist[0] = bounds.
low[0];
128 in.pointlist[1] = bounds.
low[1];
129 in.pointlist[2] = bounds.
high[0];
130 in.pointlist[3] = bounds.
low[1];
131 in.pointlist[4] = bounds.
high[0];
132 in.pointlist[5] = bounds.
high[1];
133 in.pointlist[6] = bounds.
low[0];
134 in.pointlist[7] = bounds.
high[1];
136 in.pointattributelist = NULL;
137 in.pointmarkerlist = NULL;
139 in.numberofsegments = in.numberofpoints;
140 in.segmentlist = (
int*) malloc(2*in.numberofsegments*
sizeof(
int));
142 for (
int i = 0; i < 4; ++i)
144 in.segmentlist[2*i] = i;
145 in.segmentlist[2*i+1] = (i+1) % 4;
149 for (std::vector<Polygon>::const_iterator i = holes_.begin(); i != holes_.end(); ++i)
152 for (
unsigned int j = 0; j < p.pts.size(); ++j)
154 in.pointlist[2*(pointIndex+j)] = p.pts[j].x;
155 in.pointlist[2*(pointIndex+j)+1] = p.pts[j].y;
156 in.segmentlist[2*(pointIndex+j)] = pointIndex + j;
157 in.segmentlist[2*(pointIndex+j)+1] = pointIndex + (j+1)%p.pts.size();
159 pointIndex += p.pts.size();
161 in.segmentmarkerlist = (
int*) NULL;
163 in.numberofholes = holes_.size();
165 if (in.numberofholes > 0)
167 in.holelist = (REAL*) malloc(2*in.numberofholes*
sizeof(REAL));
168 for (
unsigned int i = 0; i < holes_.size(); ++i)
170 Vertex v = pointWithinPoly(holes_[i]);
171 in.holelist[2*i] = v.x;
172 in.holelist[2*i+1] = v.y;
175 in.numberofregions = 0;
176 in.regionlist = NULL;
178 struct triangulateio out;
179 out.pointlist = (REAL*) NULL;
180 out.pointattributelist = (REAL*) NULL;
181 out.pointmarkerlist = (
int*) NULL;
182 out.trianglelist = (
int*) NULL;
183 out.triangleattributelist = (REAL*) NULL;
184 out.neighborlist = (
int*) NULL;
185 out.segmentlist = (
int*) NULL;
186 out.segmentmarkerlist = (
int*) NULL;
187 out.edgelist = (
int*) NULL;
188 out.edgemarkerlist = (
int*) NULL;
189 out.pointlist = (REAL*) NULL;
190 out.pointattributelist = (REAL*) NULL;
191 out.trianglelist = (
int*) NULL;
192 out.triangleattributelist = (REAL*) NULL;
194 struct triangulateio* vorout = NULL;
195 triangulate(const_cast<char*>(triswitches.c_str()), &in, &out, vorout);
197 triangles_.resize(out.numberoftriangles);
198 for (
int i = 0; i < out.numberoftriangles; ++i)
201 for (
int j = 0; j < 3; ++j)
203 t.pts[j].x = out.pointlist[2*out.trianglelist[3*i+j]];
204 t.pts[j].y = out.pointlist[2*out.trianglelist[3*i+j]+1];
205 if (out.neighborlist[3*i+j] >= 0)
206 t.neighbors.push_back(out.neighborlist[3*i+j]);
211 trifree(in.pointlist);
212 trifree(in.segmentlist);
213 if (in.numberofholes > 0)
214 trifree(in.holelist);
215 trifree(out.pointlist);
216 trifree(out.pointmarkerlist);
217 trifree(out.trianglelist);
218 trifree(out.neighborlist);
219 trifree(out.edgelist);
220 trifree(out.edgemarkerlist);
221 trifree(out.segmentlist);
222 trifree(out.segmentmarkerlist);
224 return out.numberoftriangles;
227 void ompl::control::TriangularDecomposition::print(std::ostream& out)
const
230 out << triangles_.size() << std::endl;
233 for (
unsigned int i = 0; i < triangles_.size(); ++i)
235 const Triangle& tri = triangles_[i];
237 for (
unsigned int v = 0; v < 3; ++v)
238 out << tri.pts[v].x <<
" " << tri.pts[v].y <<
" ";
239 out <<
"-1" << std::endl;
243 void ompl::control::TriangularDecomposition::buildLocatorGrid()
245 locator.buildTriangleMap(triangles_);
248 bool ompl::control::TriangularDecomposition::triContains(
const Triangle& tri,
const std::vector<double>& coord)
const
250 for (
int i = 0; i < 3; ++i)
254 const double ax = tri.pts[i].x;
255 const double ay = tri.pts[i].y;
256 const double bx = tri.pts[(i+1)%3].x;
257 const double by = tri.pts[(i+1)%3].y;
260 if ((coord[0]-ax)*(by-ay) - (bx-ax)*(coord[1]-ay) > 0.)
271 for (std::vector<Vertex>::const_iterator i = poly.pts.begin(); i != poly.pts.end(); ++i)
276 p.x /= poly.pts.size();
277 p.y /= poly.pts.size();
281 void ompl::control::TriangularDecomposition::LocatorGrid::buildTriangleMap(
const std::vector<Triangle>& triangles)
283 regToTriangles_.resize(getNumRegions());
284 std::vector<double> bboxLow(2);
285 std::vector<double> bboxHigh(2);
286 std::vector<unsigned int> gridCoord[2];
287 for (
unsigned int i = 0; i < triangles.size(); ++i)
291 const Triangle& tri = triangles[i];
292 bboxLow[0] = tri.pts[0].x;
293 bboxLow[1] = tri.pts[0].y;
294 bboxHigh[0] = bboxLow[0];
295 bboxHigh[1] = bboxLow[1];
297 for (
unsigned int j = 1; j < 3; ++j)
299 if (tri.pts[j].x < bboxLow[0])
300 bboxLow[0] = tri.pts[j].x;
301 else if (tri.pts[j].x > bboxHigh[0])
302 bboxHigh[0] = tri.pts[j].x;
303 if (tri.pts[j].y < bboxLow[1])
304 bboxLow[1] = tri.pts[j].y;
305 else if (tri.pts[j].y > bboxHigh[1])
306 bboxHigh[1] = tri.pts[j].y;
311 coordToGridCoord(bboxLow, gridCoord[0]);
312 coordToGridCoord(bboxHigh, gridCoord[1]);
316 std::vector<unsigned int> c(2);
317 for (
unsigned int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
319 for (
unsigned int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
323 unsigned int cellID = gridCoordToRegion(c);
324 regToTriangles_[cellID].push_back(i);
std::vector< double > low
Lower bound.
virtual void getNeighbors(unsigned int triID, std::vector< unsigned 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...
TriangularDecomposition(unsigned int dim, const base::RealVectorBounds &b, const std::vector< Polygon > &holes=std::vector< Polygon >())
Constructor. Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional...
A Decomposition is a partition of a bounded Euclidean space into a fixed number of regions which are ...
virtual void sampleFromRegion(unsigned int triID, RNG &rng, std::vector< double > &coord) const
Samples a projected coordinate from a given region.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double getVolume(void) const
Compute the volume of the space enclosed by the bounds.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
virtual unsigned int createTriangles()
Helper method to triangulate the space and return the number of triangles.
std::vector< double > high
Upper bound.
virtual double getRegionVolume(unsigned int triID)
Returns the volume of a given region in this Decomposition.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
double uniform01(void)
Generate a random real between 0 and 1.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.