All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
GridDecomposition.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Matt Maly */
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     //We efficiently compute neighbors for dim = 1, 2, or 3; for higher dimensions we use a general approach.
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         // There is an edge case when the coordinate lies exactly on the upper bound where
00140         // the region index will be out of bounds.  Ensure index lies within [0, length_)
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     // Stopping condition for recursive method.
00165     if (dim == dimension_)
00166     {
00167         // Make sure we don't push back ourselves as a neighbor
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         // Check neighbor in the cell preceding this one in this dimension
00180         if (coord[dim] -1 >= 0)
00181         {
00182             candidate[dim] = coord[dim]-1;
00183             computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
00184         }
00185 
00186         // Make sure to include the same coordinate, for neighbors "above", "below", "in front of", "behind", etcetera.
00187         candidate[dim] = coord[dim];
00188         computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
00189 
00190         // Check neighbor in the cell after this one in this dimension
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         // Computing length_^(dimension of coord -1)
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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends