blockmem_gridmap.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014-2017, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
31 #define PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
32 
33 #include <bitset>
34 #include <limits>
35 #include <memory>
36 
38 
39 namespace planner_cspace
40 {
41 template <class T, int DIM, int NONCYCLIC>
43 {
44 public:
45  virtual const CyclicVecInt<DIM, NONCYCLIC>& size() const = 0;
46  virtual size_t ser_size() const = 0;
47  virtual void clear(const T zero) = 0;
48  virtual void clear_partially(
49  const T zero, const CyclicVecInt<DIM, NONCYCLIC>& min, const CyclicVecInt<DIM, NONCYCLIC>& max) = 0;
50  virtual void copy_partially(
52  const CyclicVecInt<DIM, NONCYCLIC>& max) = 0;
53  virtual void reset(const CyclicVecInt<DIM, NONCYCLIC>& size) = 0;
54  virtual T& operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos) = 0;
55  virtual const T operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos) const = 0;
56  virtual std::function<void(CyclicVecInt<DIM, NONCYCLIC>, size_t&, size_t&)> getAddressor() const = 0;
57 };
58 
59 template <class T, int DIM, int NONCYCLIC, int BLOCK_WIDTH = 0x20, bool ENABLE_VALIDATION = false>
60 class BlockMemGridmap : public BlockMemGridmapBase<T, DIM, NONCYCLIC>
61 {
62 private:
63  static constexpr bool isPowOf2(const int v)
64  {
65  return v && ((v & (v - 1)) == 0);
66  }
67  static_assert(isPowOf2(BLOCK_WIDTH), "BLOCK_WIDTH must be power of 2");
68  static_assert(BLOCK_WIDTH > 0, "BLOCK_WIDTH must be >0");
69 
70  static constexpr size_t log2Recursive(const size_t v, const size_t depth = 0)
71  {
72  return v == 1 ? depth : log2Recursive(v >> 1, depth + 1);
73  }
74 
75 protected:
76  constexpr static size_t block_bit_ = log2Recursive(BLOCK_WIDTH);
77  constexpr static size_t block_bit_mask_ = (1 << block_bit_) - 1;
78 
79  std::unique_ptr<T[]> c_;
82  size_t ser_size_;
84  size_t block_num_;
85  T dummy_;
86 
87  inline void block_addr(
88  const CyclicVecInt<DIM, NONCYCLIC>& pos, size_t& baddr, size_t& addr) const
89  {
90  addr = 0;
91  baddr = 0;
92  for (int i = 0; i < NONCYCLIC; i++)
93  {
94  addr = (addr << block_bit_) + (pos[i] & block_bit_mask_);
95  baddr *= block_size_[i];
96  baddr += pos[i] >> block_bit_;
97  }
98  for (int i = NONCYCLIC; i < DIM; i++)
99  {
100  addr *= size_[i];
101  addr += pos[i];
102  }
103  }
104 
105 public:
106  std::function<void(CyclicVecInt<DIM, NONCYCLIC>, size_t&, size_t&)> getAddressor() const
107  {
108  return std::bind(
110  this,
111  std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
112  }
114  {
115  return size_;
116  }
117  size_t ser_size() const
118  {
119  return ser_size_;
120  }
121  void clear(const T zero)
122  {
123  for (size_t i = 0; i < ser_size_; i++)
124  {
125  c_[i] = zero;
126  }
127  }
129  const T zero, const CyclicVecInt<DIM, NONCYCLIC>& min, const CyclicVecInt<DIM, NONCYCLIC>& max)
130  {
132  for (p[0] = min[0]; p[0] < max[0]; ++p[0])
133  {
134  for (p[1] = min[1]; p[1] < max[1]; ++p[1])
135  {
136  for (p[2] = min[2]; p[2] < max[2]; ++p[2])
137  {
138  (*this)[p] = zero;
139  }
140  }
141  }
142  }
145  const CyclicVecInt<DIM, NONCYCLIC>& max)
146  {
148  for (p[0] = min[0]; p[0] < max[0]; ++p[0])
149  {
150  for (p[1] = min[1]; p[1] < max[1]; ++p[1])
151  {
152  for (p[2] = min[2]; p[2] < max[2]; ++p[2])
153  {
154  (*this)[p] = base[p];
155  }
156  }
157  }
158  }
159  void clear_positive(const T zero)
160  {
161  for (size_t i = 0; i < ser_size_; i++)
162  {
163  if (c_[i] >= 0)
164  c_[i] = zero;
165  }
166  }
168  {
170 
171  for (int i = 0; i < NONCYCLIC; i++)
172  {
173  if (size_tmp[i] < BLOCK_WIDTH)
174  size_tmp[i] = BLOCK_WIDTH;
175  }
176 
177  block_ser_size_ = 1;
178  block_num_ = 1;
179  for (int i = 0; i < DIM; i++)
180  {
181  int width;
182  if (i < NONCYCLIC)
183  {
184  width = BLOCK_WIDTH;
185  block_size_[i] = (size_tmp[i] + width - 1) / width;
186  }
187  else
188  {
189  width = size_tmp[i];
190  block_size_[i] = 1;
191  }
192 
193  block_ser_size_ *= width;
194  block_num_ *= block_size_[i];
195  }
196  ser_size_ = block_ser_size_ * block_num_;
197 
198  c_.reset(new T[ser_size_]);
199  size_ = size;
200  }
202  : BlockMemGridmap()
203  {
204  reset(size_);
205  }
207  : dummy_(std::numeric_limits<T>::max())
208  {
209  }
211  {
212  size_t baddr, addr;
213  block_addr(pos, baddr, addr);
214  const size_t a = baddr * block_ser_size_ + addr;
215  if (ENABLE_VALIDATION)
216  {
217  if (a >= ser_size_)
218  return dummy_;
219  }
220  return c_[a];
221  }
222  const T operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos) const
223  {
224  size_t baddr, addr;
225  block_addr(pos, baddr, addr);
226  const size_t a = baddr * block_ser_size_ + addr;
227  if (ENABLE_VALIDATION)
228  {
229  if (a >= ser_size_)
230  return std::numeric_limits<T>::max();
231  }
232  return c_[a];
233  }
234  bool validate(const CyclicVecInt<DIM, NONCYCLIC>& pos, const int tolerance = 0) const
235  {
236  for (int i = 0; i < NONCYCLIC; i++)
237  {
238  if (pos[i] < tolerance || size_[i] - tolerance <= pos[i])
239  return false;
240  }
241  for (int i = NONCYCLIC; i < DIM; i++)
242  {
243  if (pos[i] < 0 || size_[i] <= pos[i])
244  return false;
245  }
246  return true;
247  }
250  {
251  reset(gm.size_);
252  memcpy(c_.get(), gm.c_.get(), ser_size_ * sizeof(T));
253 
254  return *this;
255  }
256 };
257 } // namespace planner_cspace
258 
259 #endif // PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
void reset(const CyclicVecInt< DIM, NONCYCLIC > &size)
BlockMemGridmap(const CyclicVecInt< DIM, NONCYCLIC > &size_)
std::function< void(CyclicVecInt< DIM, NONCYCLIC >, size_t &, size_t &)> getAddressor() const
bool validate(const CyclicVecInt< DIM, NONCYCLIC > &pos, const int tolerance=0) const
virtual void clear_partially(const T zero, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)=0
virtual const CyclicVecInt< DIM, NONCYCLIC > & size() const =0
virtual std::function< void(CyclicVecInt< DIM, NONCYCLIC >, size_t &, size_t &)> getAddressor() const =0
virtual void clear(const T zero)=0
const CyclicVecInt< DIM, NONCYCLIC > & size() const
void clear_partially(const T zero, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)
const BlockMemGridmap< T, DIM, NONCYCLIC, BLOCK_WIDTH, ENABLE_VALIDATION > & operator=(const BlockMemGridmap< T, DIM, NONCYCLIC, BLOCK_WIDTH, ENABLE_VALIDATION > &gm)
virtual void reset(const CyclicVecInt< DIM, NONCYCLIC > &size)=0
void block_addr(const CyclicVecInt< DIM, NONCYCLIC > &pos, size_t &baddr, size_t &addr) const
static constexpr bool isPowOf2(const int v)
void copy_partially(const BlockMemGridmapBase< T, DIM, NONCYCLIC > &base, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)
CyclicVecInt< DIM, NONCYCLIC > size_
virtual T & operator[](const CyclicVecInt< DIM, NONCYCLIC > &pos)=0
T & operator[](const CyclicVecInt< DIM, NONCYCLIC > &pos)
virtual void copy_partially(const BlockMemGridmapBase< T, DIM, NONCYCLIC > &base, const CyclicVecInt< DIM, NONCYCLIC > &min, const CyclicVecInt< DIM, NONCYCLIC > &max)=0
double min(double a, double b)
static constexpr size_t log2Recursive(const size_t v, const size_t depth=0)
CyclicVecInt< DIM, NONCYCLIC > block_size_
virtual size_t ser_size() const =0
double max(double a, double b)
const T operator[](const CyclicVecInt< DIM, NONCYCLIC > &pos) const


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42