37 #include "ompl/control/planners/syclop/GridDecomposition.h"
40 Decomposition(calcNumRegions(len,dim), dim, b), length_(len), cellVolume_(b.getVolume())
42 const double lenInv = 1.0 / len;
43 for (std::size_t i = 0; i < dim; ++i)
44 cellVolume_ *= lenInv;
53 neighbors.push_back(rid-1);
55 neighbors.push_back(rid+1);
57 else if (dimension_ == 2)
59 static const int offset[] = {
69 std::vector<int> coord(2);
70 regionToCoord(rid, coord);
71 std::vector<int> nc(2);
72 for (std::size_t i = 0; i < 16; i += 2)
74 nc[0] = coord[0] + offset[i];
75 nc[1] = coord[1] + offset[i+1];
76 if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_)
77 neighbors.push_back(nc[0]*length_ + nc[1]);
80 else if (dimension_ == 3)
82 static const int offset[] = {
110 std::vector<int> coord(3);
111 regionToCoord(rid, coord);
112 std::vector<int> nc(3);
113 for (std::size_t i = 0; i < 78; i += 3)
115 nc[0] = coord[0] + offset[i];
116 nc[1] = coord[1] + offset[i+1];
117 nc[2] = coord[2] + offset[i+2];
118 if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_ && nc[2] >= 0 && nc[2] < length_)
119 neighbors.push_back(nc[0]*length_*length_ + nc[1]*length_ + nc[2]);
124 computeGridNeighbors (rid, neighbors);
130 std::vector<double> coord(dimension_);
135 for (
int i = dimension_-1; i >= 0; --i)
137 index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
141 if (index >= length_)
144 region += factor*index;
152 std::vector <int> candidate (dimension_, -1);
153 std::vector <int> coord;
154 regionToCoord (rid, coord);
156 computeGridNeighborsSub (coord, neighbors, 0, candidate);
160 std::vector <int> &neighbors,
162 std::vector <int> &candidate)
const
165 if (dim == dimension_)
169 for (
size_t i = 0; i < coord.size () && same; ++i)
170 same = (coord[i] == candidate[i]);
174 neighbors.push_back (coordToRegion (candidate));
180 if (coord[dim] -1 >= 0)
182 candidate[dim] = coord[dim]-1;
183 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
187 candidate[dim] = coord[dim];
188 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
191 if (coord[dim] +1 < length_)
193 candidate[dim] = coord[dim]+1;
194 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
201 coord.resize(dimension_);
202 for (
int i = dimension_-1; i >= 0; --i)
204 int remainder = rid % length_;
205 coord[i] = remainder;
213 for (
size_t i = 0; i < coord.size (); i++)
216 int multiplicand = 1;
217 for (
size_t j = 1; j < coord.size () - i; j++)
218 multiplicand *= length_;
220 region += (coord[i] * multiplicand);
227 if (regToBounds_.count(rid) > 0)
228 return *regToBounds_[rid].
get();
230 std::vector<int> rc(dimension_);
231 regionToCoord(rid, rc);
232 for (std::size_t i = 0; i < dimension_; ++i)
234 const double length = (bounds_.high[i] - bounds_.low[i]) / length_;
235 regionBounds->
low[i] = bounds_.low[i] + length*rc[i];
236 regionBounds->
high[i] = regionBounds->
low[i] + length;
238 regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
239 return *regToBounds_[rid].get();
242 int ompl::control::GridDecomposition::calcNumRegions(
const int len,
const std::size_t dim)
const
245 for (std::size_t i = 0; i < dim; ++i)