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
00037 #ifndef DF_VOXEL_GRID_H_
00038 #define DF_VOXEL_GRID_H_
00039
00040 #include <algorithm>
00041 #include <math.h>
00042
00043 namespace distance_field
00044 {
00045
00049 template <typename T>
00050 class VoxelGrid
00051 {
00052 public:
00065 VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00066 double origin_x, double origin_y, double origin_z, T default_object);
00067 virtual ~VoxelGrid();
00068
00072 const T& operator()(double x, double y, double z) const;
00073
00077 T& getCell(int x, int y, int z);
00078
00082 void setCell(int x, int y, int z, T& obj);
00083
00087 const T& getCell(int x, int y, int z) const;
00088
00092 void reset(T initial);
00093
00094 enum Dimension
00095 {
00096 DIM_X = 0,
00097 DIM_Y = 1,
00098 DIM_Z = 2
00099 };
00100
00104 double getSize(Dimension dim) const;
00105
00109 double getResolution(Dimension dim) const;
00110
00114 double getOrigin(Dimension dim) const;
00115
00119 int getNumCells(Dimension dim) const;
00120
00124 bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00125
00129 bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00130
00131 protected:
00132 T* data_;
00133 T default_object_;
00134 T*** data_ptrs_;
00135 double size_[3];
00136 double resolution_[3];
00137 double origin_[3];
00138 int num_cells_[3];
00139 int num_cells_total_;
00140 int stride1_;
00141 int stride2_;
00142
00146 int ref(int x, int y, int z) const;
00147
00151 int getCellFromLocation(Dimension dim, double loc) const;
00152
00156 double getLocationFromCell(Dimension dim, int cell) const;
00157
00161 bool isCellValid(int x, int y, int z) const;
00162
00166 bool isCellValid(Dimension dim, int cell) const;
00167 };
00168
00170
00171 template<typename T>
00172 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00173 double origin_x, double origin_y, double origin_z, T default_object)
00174 {
00175 size_[DIM_X] = size_x;
00176 size_[DIM_Y] = size_y;
00177 size_[DIM_Z] = size_z;
00178 origin_[DIM_X] = origin_x;
00179 origin_[DIM_Y] = origin_y;
00180 origin_[DIM_Z] = origin_z;
00181 num_cells_total_ = 1;
00182 for (int i=DIM_X; i<=DIM_Z; ++i)
00183 {
00184 resolution_[i] = resolution;
00185 num_cells_[i] = size_[i] / resolution_[i];
00186 num_cells_total_ *= num_cells_[i];
00187 }
00188 default_object_ = default_object;
00189
00190 stride1_ = num_cells_[DIM_Y]*num_cells_[DIM_Z];
00191 stride2_ = num_cells_[DIM_Z];
00192
00193
00194 data_ = new T[num_cells_total_];
00195
00196 }
00197
00198 template<typename T>
00199 VoxelGrid<T>::~VoxelGrid()
00200 {
00201 delete[] data_;
00202 }
00203
00204 template<typename T>
00205 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00206 {
00207 return (
00208 x>=0 && x<num_cells_[DIM_X] &&
00209 y>=0 && y<num_cells_[DIM_Y] &&
00210 z>=0 && z<num_cells_[DIM_Z]);
00211 }
00212
00213 template<typename T>
00214 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00215 {
00216 return cell>=0 && cell<num_cells_[dim];
00217 }
00218
00219 template<typename T>
00220 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00221 {
00222 return x*stride1_ + y*stride2_ + z;
00223 }
00224
00225 template<typename T>
00226 inline double VoxelGrid<T>::getSize(Dimension dim) const
00227 {
00228 return size_[dim];
00229 }
00230
00231 template<typename T>
00232 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00233 {
00234 return resolution_[dim];
00235 }
00236
00237 template<typename T>
00238 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00239 {
00240 return origin_[dim];
00241 }
00242
00243 template<typename T>
00244 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00245 {
00246 return num_cells_[dim];
00247 }
00248
00249 template<typename T>
00250 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00251 {
00252 int cellX = getCellFromLocation(DIM_X, x);
00253 int cellY = getCellFromLocation(DIM_Y, y);
00254 int cellZ = getCellFromLocation(DIM_Z, z);
00255 if (!isCellValid(cellX, cellY, cellZ))
00256 return default_object_;
00257 return getCell(cellX, cellY, cellZ);
00258 }
00259
00260 template<typename T>
00261 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00262 {
00263 return data_[ref(x,y,z)];
00264 }
00265
00266 template<typename T>
00267 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00268 {
00269 return data_[ref(x,y,z)];
00270 }
00271
00272 template<typename T>
00273 inline void VoxelGrid<T>::setCell(int x, int y, int z, T& obj)
00274 {
00275 data_[ref(x,y,z)] = obj;
00276 }
00277
00278 template<typename T>
00279 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00280 {
00281 double res = (loc-origin_[dim])/resolution_[dim];
00282 if (res > 0)
00283 return floor(res + 0.5);
00284 else
00285 return ceil(res - 0.5);
00286 }
00287
00288 template<typename T>
00289 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00290 {
00291 return origin_[dim] + resolution_[dim]*(double(cell));
00292 }
00293
00294
00295 template<typename T>
00296 inline void VoxelGrid<T>::reset(T initial)
00297 {
00298 std::fill(data_, data_+num_cells_total_, initial);
00299 }
00300
00301 template<typename T>
00302 inline bool VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00303 {
00304 world_x = getLocationFromCell(DIM_X, x);
00305 world_y = getLocationFromCell(DIM_Y, y);
00306 world_z = getLocationFromCell(DIM_Z, z);
00307 return true;
00308 }
00309
00310 template<typename T>
00311 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00312 {
00313 x = getCellFromLocation(DIM_X, world_x);
00314 y = getCellFromLocation(DIM_Y, world_y);
00315 z = getCellFromLocation(DIM_Z, world_z);
00316 return isCellValid(x,y,z);
00317 }
00318
00319 }
00320 #endif