Go to the documentation of this file.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 #ifndef PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
00031 #define PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
00032
00033 #include <limits>
00034
00035 #include <planner_cspace/cyclic_vec.h>
00036
00037 template <class T, int DIM, int NONCYCLIC, int BLOCK_WIDTH = 0x20>
00038 class BlockMemGridmap
00039 {
00040 protected:
00041 std::unique_ptr<T[]> c_;
00042 CyclicVecInt<DIM, NONCYCLIC> size_;
00043 CyclicVecInt<DIM, NONCYCLIC> block_size_;
00044 size_t ser_size_;
00045 size_t block_ser_size_;
00046 size_t block_num_;
00047 T dummy_;
00048
00049 void block_addr(
00050 const CyclicVecInt<DIM, NONCYCLIC>& pos, size_t& baddr, size_t& addr) const
00051 {
00052 addr = 0;
00053 baddr = 0;
00054 for (int i = 0; i < NONCYCLIC; i++)
00055 {
00056 addr *= BLOCK_WIDTH;
00057 addr += pos[i] % BLOCK_WIDTH;
00058 baddr *= block_size_[i];
00059 baddr += pos[i] / BLOCK_WIDTH;
00060 }
00061 for (int i = NONCYCLIC; i < DIM; i++)
00062 {
00063 addr *= size_[i];
00064 addr += pos[i];
00065 }
00066 }
00067
00068 public:
00069 std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> getAddressor() const
00070 {
00071 return std::bind(
00072 &BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>::block_addr,
00073 this,
00074 std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
00075 }
00076 const CyclicVecInt<DIM, NONCYCLIC>& size() const
00077 {
00078 return size_;
00079 }
00080 size_t ser_size() const
00081 {
00082 return ser_size_;
00083 }
00084 void clear(const T zero)
00085 {
00086 for (size_t i = 0; i < ser_size_; i++)
00087 {
00088 c_[i] = zero;
00089 }
00090 }
00091 void clear_positive(const T zero)
00092 {
00093 for (size_t i = 0; i < ser_size_; i++)
00094 {
00095 if (c_[i] >= 0)
00096 c_[i] = zero;
00097 }
00098 }
00099 void reset(const CyclicVecInt<DIM, NONCYCLIC>& size)
00100 {
00101 auto size_tmp = size;
00102
00103 for (int i = 0; i < NONCYCLIC; i++)
00104 {
00105 if (size_tmp[i] < BLOCK_WIDTH)
00106 size_tmp[i] = BLOCK_WIDTH;
00107 }
00108
00109 block_ser_size_ = 1;
00110 block_num_ = 1;
00111 for (int i = 0; i < DIM; i++)
00112 {
00113 int width;
00114 if (i < NONCYCLIC)
00115 {
00116 width = BLOCK_WIDTH;
00117 block_size_[i] = (size_tmp[i] + width - 1) / width;
00118 }
00119 else
00120 {
00121 width = size_tmp[i];
00122 block_size_[i] = 1;
00123 }
00124
00125 block_ser_size_ *= width;
00126 block_num_ *= block_size_[i];
00127 }
00128 ser_size_ = block_ser_size_ * block_num_;
00129
00130 c_.reset(new T[ser_size_]);
00131 size_ = size;
00132 }
00133 explicit BlockMemGridmap(const CyclicVecInt<DIM, NONCYCLIC>& size_)
00134 {
00135 reset(size_);
00136 }
00137 BlockMemGridmap()
00138 {
00139 }
00140 T& operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos)
00141 {
00142 size_t baddr, addr;
00143 block_addr(pos, baddr, addr);
00144 if (addr >= block_ser_size_ || baddr >= block_num_)
00145 {
00146 dummy_ = std::numeric_limits<T>::max();
00147 return dummy_;
00148 }
00149 return c_[baddr * block_ser_size_ + addr];
00150 }
00151 const T operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos) const
00152 {
00153 size_t baddr, addr;
00154 block_addr(pos, baddr, addr);
00155 if (addr >= block_ser_size_ || baddr >= block_num_)
00156 return std::numeric_limits<T>::max();
00157 return c_[baddr * block_ser_size_ + addr];
00158 }
00159 bool validate(const CyclicVecInt<DIM, NONCYCLIC>& pos, const int tolerance = 0) const
00160 {
00161 for (int i = 0; i < NONCYCLIC; i++)
00162 {
00163 if (pos[i] < tolerance || size_[i] - tolerance <= pos[i])
00164 return false;
00165 }
00166 for (int i = NONCYCLIC; i < DIM; i++)
00167 {
00168 if (pos[i] < 0 || size_[i] <= pos[i])
00169 return false;
00170 }
00171 return true;
00172 }
00173 const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>& operator=(
00174 const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>& gm)
00175 {
00176 reset(gm.size_);
00177 memcpy(c_.get(), gm.c_.get(), ser_size_);
00178
00179 return *this;
00180 }
00181 };
00182
00183 #endif // PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H