00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/planners/syclop/GridDecomposition.h"
00038
00039 ompl::control::GridDecomposition::GridDecomposition(const int len, const std::size_t dim, const base::RealVectorBounds& b) :
00040 Decomposition(calcNumRegions(len,dim), dim, b), length_(len), cellVolume_(b.getVolume())
00041 {
00042 const double lenInv = 1.0 / len;
00043 for (std::size_t i = 0; i < dim; ++i)
00044 cellVolume_ *= lenInv;
00045 }
00046
00047 void ompl::control::GridDecomposition::getNeighbors(const int rid, std::vector<int>& neighbors) const
00048 {
00049
00050 if (dimension_ == 1)
00051 {
00052 if (rid > 0)
00053 neighbors.push_back(rid-1);
00054 if (rid < length_-1)
00055 neighbors.push_back(rid+1);
00056 }
00057 else if (dimension_ == 2)
00058 {
00059 static const int offset[] = {
00060 -1, -1,
00061 0, -1,
00062 +1, -1,
00063 -1, 0,
00064 +1, 0,
00065 -1, +1,
00066 0, +1,
00067 +1, +1
00068 };
00069 std::vector<int> coord(2);
00070 regionToCoord(rid, coord);
00071 std::vector<int> nc(2);
00072 for (std::size_t i = 0; i < 16; i += 2)
00073 {
00074 nc[0] = coord[0] + offset[i];
00075 nc[1] = coord[1] + offset[i+1];
00076 if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_)
00077 neighbors.push_back(nc[0]*length_ + nc[1]);
00078 }
00079 }
00080 else if (dimension_ == 3)
00081 {
00082 static const int offset[] = {
00083 -1, 0, 0,
00084 +1, 0, 0,
00085 0, -1, 0,
00086 0, +1, 0,
00087 -1, -1, 0,
00088 -1, +1, 0,
00089 +1, -1, 0,
00090 +1, +1, 0,
00091 -1, 0, -1,
00092 +1, 0, -1,
00093 0, -1, -1,
00094 0, +1, -1,
00095 -1, -1, -1,
00096 -1, +1, -1,
00097 +1, -1, -1,
00098 +1, +1, -1,
00099 -1, 0, +1,
00100 +1, 0, +1,
00101 0, -1, +1,
00102 0, +1, +1,
00103 -1, -1, +1,
00104 -1, +1, +1,
00105 +1, -1, +1,
00106 +1, +1, +1,
00107 0, 0, -1,
00108 0, 0, +1
00109 };
00110 std::vector<int> coord(3);
00111 regionToCoord(rid, coord);
00112 std::vector<int> nc(3);
00113 for (std::size_t i = 0; i < 78; i += 3)
00114 {
00115 nc[0] = coord[0] + offset[i];
00116 nc[1] = coord[1] + offset[i+1];
00117 nc[2] = coord[2] + offset[i+2];
00118 if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_ && nc[2] >= 0 && nc[2] < length_)
00119 neighbors.push_back(nc[0]*length_*length_ + nc[1]*length_ + nc[2]);
00120 }
00121 }
00122 else
00123 {
00124 computeGridNeighbors (rid, neighbors);
00125 }
00126 }
00127
00128 int ompl::control::GridDecomposition::locateRegion(const base::State* s) const
00129 {
00130 std::vector<double> coord(dimension_);
00131 project(s, coord);
00132 int region = 0;
00133 int factor = 1;
00134 int index;
00135 for (int i = dimension_-1; i >= 0; --i)
00136 {
00137 index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
00138
00139
00140
00141 if (index >= length_)
00142 index = length_-1;
00143
00144 region += factor*index;
00145 factor *= length_;
00146 }
00147 return region;
00148 }
00149
00150 void ompl::control::GridDecomposition::computeGridNeighbors (int rid, std::vector <int> &neighbors) const
00151 {
00152 std::vector <int> candidate (dimension_, -1);
00153 std::vector <int> coord;
00154 regionToCoord (rid, coord);
00155
00156 computeGridNeighborsSub (coord, neighbors, 0, candidate);
00157 }
00158
00159 void ompl::control::GridDecomposition::computeGridNeighborsSub (const std::vector <int> &coord,
00160 std::vector <int> &neighbors,
00161 unsigned int dim,
00162 std::vector <int> &candidate) const
00163 {
00164
00165 if (dim == dimension_)
00166 {
00167
00168 bool same = true;
00169 for (size_t i = 0; i < coord.size () && same; ++i)
00170 same = (coord[i] == candidate[i]);
00171
00172 if (!same)
00173 {
00174 neighbors.push_back (coordToRegion (candidate));
00175 }
00176 }
00177 else
00178 {
00179
00180 if (coord[dim] -1 >= 0)
00181 {
00182 candidate[dim] = coord[dim]-1;
00183 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
00184 }
00185
00186
00187 candidate[dim] = coord[dim];
00188 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
00189
00190
00191 if (coord[dim] +1 < length_)
00192 {
00193 candidate[dim] = coord[dim]+1;
00194 computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
00195 }
00196 }
00197 }
00198
00199 void ompl::control::GridDecomposition::regionToCoord(int rid, std::vector<int>& coord) const
00200 {
00201 coord.resize(dimension_);
00202 for (int i = dimension_-1; i >= 0; --i)
00203 {
00204 int remainder = rid % length_;
00205 coord[i] = remainder;
00206 rid /= length_;
00207 }
00208 }
00209
00210 int ompl::control::GridDecomposition::coordToRegion (const std::vector <int> &coord) const
00211 {
00212 int region = 0;
00213 for (size_t i = 0; i < coord.size (); i++)
00214 {
00215
00216 int multiplicand = 1;
00217 for (size_t j = 1; j < coord.size () - i; j++)
00218 multiplicand *= length_;
00219
00220 region += (coord[i] * multiplicand);
00221 }
00222 return region;
00223 }
00224
00225 const ompl::base::RealVectorBounds& ompl::control::GridDecomposition::getRegionBounds(const int rid)
00226 {
00227 if (regToBounds_.count(rid) > 0)
00228 return *regToBounds_[rid].get();
00229 ompl::base::RealVectorBounds* regionBounds = new ompl::base::RealVectorBounds(dimension_);
00230 std::vector<int> rc(dimension_);
00231 regionToCoord(rid, rc);
00232 for (std::size_t i = 0; i < dimension_; ++i)
00233 {
00234 const double length = (bounds_.high[i] - bounds_.low[i]) / length_;
00235 regionBounds->low[i] = bounds_.low[i] + length*rc[i];
00236 regionBounds->high[i] = regionBounds->low[i] + length;
00237 }
00238 regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
00239 return *regToBounds_[rid].get();
00240 }
00241
00242 int ompl::control::GridDecomposition::calcNumRegions(const int len, const std::size_t dim) const
00243 {
00244 int numRegions = 1;
00245 for (std::size_t i = 0; i < dim; ++i)
00246 numRegions *= len;
00247 return numRegions;
00248 }