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
00035
00036
00037 #ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00038 #define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00039
00040 #include <algorithm>
00041 #include <cmath>
00042
00043 namespace distance_field
00044 {
00045
00047 enum Dimension {
00048 DIM_X = 0,
00049 DIM_Y = 1,
00050 DIM_Z = 2
00051 };
00052
00059 template <typename T>
00060 class VoxelGrid
00061 {
00062 public:
00088 VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00089 double origin_x, double origin_y, double origin_z, T default_object);
00090 virtual ~VoxelGrid();
00091
00098 VoxelGrid();
00099
00117 void resize(double size_x, double size_y, double size_z, double resolution,
00118 double origin_x, double origin_y, double origin_z, T default_object);
00119
00133 const T& operator()(double x, double y, double z) const;
00134
00152 T& getCell(int x, int y, int z);
00153
00172 void setCell(int x, int y, int z, const T& obj);
00173
00177 const T& getCell(int x, int y, int z) const;
00178
00184 void reset(const T& initial);
00185
00193 double getSize(Dimension dim) const;
00194
00200 double getResolution() const;
00201
00203 double getResolution(Dimension dim) const;
00204
00212 double getOrigin(Dimension dim) const;
00213
00221 int getNumCells(Dimension dim) const;
00222
00241 bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00242
00259 bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00260
00270 bool isCellValid(int x, int y, int z) const;
00271
00280 bool isCellValid(Dimension dim, int cell) const;
00281
00282 protected:
00283 T* data_;
00284 T default_object_;
00285 T*** data_ptrs_;
00286 double size_[3];
00287 double resolution_;
00288 double oo_resolution_;
00289 double origin_[3];
00290 double origin_minus_[3];
00291 int num_cells_[3];
00292 int num_cells_total_;
00293 int stride1_;
00294 int stride2_;
00305 int ref(int x, int y, int z) const;
00306
00319 int getCellFromLocation(Dimension dim, double loc) const;
00320
00330 double getLocationFromCell(Dimension dim, int cell) const;
00331
00332 };
00333
00335
00336 template<typename T>
00337 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00338 double origin_x, double origin_y, double origin_z, T default_object)
00339 : data_(NULL)
00340 {
00341 resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
00342 }
00343
00344 template<typename T>
00345 VoxelGrid<T>::VoxelGrid()
00346 : data_(NULL)
00347 {
00348 for (int i=DIM_X; i<=DIM_Z; ++i)
00349 {
00350 size_[i] = 0;
00351 origin_[i] = 0;
00352 origin_minus_[i] = 0;
00353 num_cells_[i] = 0;
00354 }
00355 resolution_ = 1.0;
00356 oo_resolution_ = 1.0 / resolution_;
00357 num_cells_total_ = 0;
00358 stride1_ = 0;
00359 stride2_ = 0;
00360 }
00361
00362 template<typename T>
00363 void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution,
00364 double origin_x, double origin_y, double origin_z, T default_object)
00365 {
00366 delete[] data_;
00367 data_ = NULL;
00368
00369 size_[DIM_X] = size_x;
00370 size_[DIM_Y] = size_y;
00371 size_[DIM_Z] = size_z;
00372 origin_[DIM_X] = origin_x;
00373 origin_[DIM_Y] = origin_y;
00374 origin_[DIM_Z] = origin_z;
00375 origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
00376 origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
00377 origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
00378 num_cells_total_ = 1;
00379 resolution_ = resolution;
00380 oo_resolution_ = 1.0 / resolution_;
00381 for (int i=DIM_X; i<=DIM_Z; ++i)
00382 {
00383 num_cells_[i] = size_[i] * oo_resolution_;
00384 num_cells_total_ *= num_cells_[i];
00385 }
00386
00387 default_object_ = default_object;
00388
00389 stride1_ = num_cells_[DIM_Y]*num_cells_[DIM_Z];
00390 stride2_ = num_cells_[DIM_Z];
00391
00392
00393 if (num_cells_total_ > 0)
00394 data_ = new T[num_cells_total_];
00395 }
00396
00397 template<typename T>
00398 VoxelGrid<T>::~VoxelGrid()
00399 {
00400 delete[] data_;
00401 }
00402
00403 template<typename T>
00404 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00405 {
00406 return (
00407 x>=0 && x<num_cells_[DIM_X] &&
00408 y>=0 && y<num_cells_[DIM_Y] &&
00409 z>=0 && z<num_cells_[DIM_Z]);
00410 }
00411
00412 template<typename T>
00413 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00414 {
00415 return cell>=0 && cell<num_cells_[dim];
00416 }
00417
00418 template<typename T>
00419 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00420 {
00421 return x*stride1_ + y*stride2_ + z;
00422 }
00423
00424 template<typename T>
00425 inline double VoxelGrid<T>::getSize(Dimension dim) const
00426 {
00427 return size_[dim];
00428 }
00429
00430 template<typename T>
00431 inline double VoxelGrid<T>::getResolution() const
00432 {
00433 return resolution_;
00434 }
00435
00436 template<typename T>
00437 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00438 {
00439 return resolution_;
00440 }
00441
00442 template<typename T>
00443 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00444 {
00445 return origin_[dim];
00446 }
00447
00448 template<typename T>
00449 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00450 {
00451 return num_cells_[dim];
00452 }
00453
00454 template<typename T>
00455 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00456 {
00457 int cellX = getCellFromLocation(DIM_X, x);
00458 int cellY = getCellFromLocation(DIM_Y, y);
00459 int cellZ = getCellFromLocation(DIM_Z, z);
00460 if (!isCellValid(cellX, cellY, cellZ))
00461 return default_object_;
00462 return getCell(cellX, cellY, cellZ);
00463 }
00464
00465 template<typename T>
00466 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00467 {
00468 return data_[ref(x,y,z)];
00469 }
00470
00471 template<typename T>
00472 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00473 {
00474 return data_[ref(x,y,z)];
00475 }
00476
00477 template<typename T>
00478 inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
00479 {
00480 data_[ref(x,y,z)] = obj;
00481 }
00482
00483 template<typename T>
00484 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00485 {
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500 return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
00501 }
00502
00503 template<typename T>
00504 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00505 {
00506 return origin_[dim] + resolution_ * (double(cell));
00507 }
00508
00509
00510 template<typename T>
00511 inline void VoxelGrid<T>::reset(const T& initial)
00512 {
00513 std::fill(data_, data_ + num_cells_total_, initial);
00514 }
00515
00516 template<typename T>
00517 inline bool VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00518 {
00519 world_x = getLocationFromCell(DIM_X, x);
00520 world_y = getLocationFromCell(DIM_Y, y);
00521 world_z = getLocationFromCell(DIM_Z, z);
00522 return true;
00523 }
00524
00525 template<typename T>
00526 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00527 {
00528 x = getCellFromLocation(DIM_X, world_x);
00529 y = getCellFromLocation(DIM_Y, world_y);
00530 z = getCellFromLocation(DIM_Z, world_z);
00531 return isCellValid(x,y,z);
00532 }
00533
00534 }
00535 #endif