All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
GridDecomposition.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #include "ompl/control/planners/syclop/GridDecomposition.h"
38 
39 ompl::control::GridDecomposition::GridDecomposition(const int len, const std::size_t dim, const base::RealVectorBounds& b) :
40  Decomposition(calcNumRegions(len,dim), dim, b), length_(len), cellVolume_(b.getVolume())
41 {
42  const double lenInv = 1.0 / len;
43  for (std::size_t i = 0; i < dim; ++i)
44  cellVolume_ *= lenInv;
45 }
46 
47 void ompl::control::GridDecomposition::getNeighbors(const int rid, std::vector<int>& neighbors) const
48 {
49  //We efficiently compute neighbors for dim = 1, 2, or 3; for higher dimensions we use a general approach.
50  if (dimension_ == 1)
51  {
52  if (rid > 0)
53  neighbors.push_back(rid-1);
54  if (rid < length_-1)
55  neighbors.push_back(rid+1);
56  }
57  else if (dimension_ == 2)
58  {
59  static const int offset[] = {
60  -1, -1,
61  0, -1,
62  +1, -1,
63  -1, 0,
64  +1, 0,
65  -1, +1,
66  0, +1,
67  +1, +1
68  };
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)
73  {
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]);
78  }
79  }
80  else if (dimension_ == 3)
81  {
82  static const int offset[] = {
83  -1, 0, 0,
84  +1, 0, 0,
85  0, -1, 0,
86  0, +1, 0,
87  -1, -1, 0,
88  -1, +1, 0,
89  +1, -1, 0,
90  +1, +1, 0,
91  -1, 0, -1,
92  +1, 0, -1,
93  0, -1, -1,
94  0, +1, -1,
95  -1, -1, -1,
96  -1, +1, -1,
97  +1, -1, -1,
98  +1, +1, -1,
99  -1, 0, +1,
100  +1, 0, +1,
101  0, -1, +1,
102  0, +1, +1,
103  -1, -1, +1,
104  -1, +1, +1,
105  +1, -1, +1,
106  +1, +1, +1,
107  0, 0, -1,
108  0, 0, +1
109  };
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)
114  {
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]);
120  }
121  }
122  else
123  {
124  computeGridNeighbors (rid, neighbors);
125  }
126 }
127 
129 {
130  std::vector<double> coord(dimension_);
131  project(s, coord);
132  int region = 0;
133  int factor = 1;
134  int index;
135  for (int i = dimension_-1; i >= 0; --i)
136  {
137  index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
138 
139  // There is an edge case when the coordinate lies exactly on the upper bound where
140  // the region index will be out of bounds. Ensure index lies within [0, length_)
141  if (index >= length_)
142  index = length_-1;
143 
144  region += factor*index;
145  factor *= length_;
146  }
147  return region;
148 }
149 
150 void ompl::control::GridDecomposition::computeGridNeighbors (int rid, std::vector <int> &neighbors) const
151 {
152  std::vector <int> candidate (dimension_, -1);
153  std::vector <int> coord;
154  regionToCoord (rid, coord);
155 
156  computeGridNeighborsSub (coord, neighbors, 0, candidate);
157 }
158 
160  std::vector <int> &neighbors,
161  unsigned int dim,
162  std::vector <int> &candidate) const
163 {
164  // Stopping condition for recursive method.
165  if (dim == dimension_)
166  {
167  // Make sure we don't push back ourselves as a neighbor
168  bool same = true;
169  for (size_t i = 0; i < coord.size () && same; ++i)
170  same = (coord[i] == candidate[i]);
171 
172  if (!same)
173  {
174  neighbors.push_back (coordToRegion (candidate));
175  }
176  }
177  else
178  {
179  // Check neighbor in the cell preceding this one in this dimension
180  if (coord[dim] -1 >= 0)
181  {
182  candidate[dim] = coord[dim]-1;
183  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
184  }
185 
186  // Make sure to include the same coordinate, for neighbors "above", "below", "in front of", "behind", etcetera.
187  candidate[dim] = coord[dim];
188  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
189 
190  // Check neighbor in the cell after this one in this dimension
191  if (coord[dim] +1 < length_)
192  {
193  candidate[dim] = coord[dim]+1;
194  computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
195  }
196  }
197 }
198 
199 void ompl::control::GridDecomposition::regionToCoord(int rid, std::vector<int>& coord) const
200 {
201  coord.resize(dimension_);
202  for (int i = dimension_-1; i >= 0; --i)
203  {
204  int remainder = rid % length_;
205  coord[i] = remainder;
206  rid /= length_;
207  }
208 }
209 
210 int ompl::control::GridDecomposition::coordToRegion (const std::vector <int> &coord) const
211 {
212  int region = 0;
213  for (size_t i = 0; i < coord.size (); i++)
214  {
215  // Computing length_^(dimension of coord -1)
216  int multiplicand = 1;
217  for (size_t j = 1; j < coord.size () - i; j++)
218  multiplicand *= length_;
219 
220  region += (coord[i] * multiplicand);
221  }
222  return region;
223 }
224 
226 {
227  if (regToBounds_.count(rid) > 0)
228  return *regToBounds_[rid].get();
229  ompl::base::RealVectorBounds* regionBounds = new ompl::base::RealVectorBounds(dimension_);
230  std::vector<int> rc(dimension_);
231  regionToCoord(rid, rc);
232  for (std::size_t i = 0; i < dimension_; ++i)
233  {
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;
237  }
238  regToBounds_[rid] = boost::shared_ptr<ompl::base::RealVectorBounds>(regionBounds);
239  return *regToBounds_[rid].get();
240 }
241 
242 int ompl::control::GridDecomposition::calcNumRegions(const int len, const std::size_t dim) const
243 {
244  int numRegions = 1;
245  for (std::size_t i = 0; i < dim; ++i)
246  numRegions *= len;
247  return numRegions;
248 }