distance_field: voxel_grid.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #ifndef DF_VOXEL_GRID_H_
00038 #define DF_VOXEL_GRID_H_
00039 
00040 #include <algorithm>
00041 
00042 namespace distance_field
00043 {
00044 
00048 template <typename T>
00049 class VoxelGrid
00050 {
00051 public:
00064   VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00065       double origin_x, double origin_y, double origin_z, T default_object);
00066   virtual ~VoxelGrid();
00067 
00071   const T& operator()(double x, double y, double z) const;
00072 
00076   T& getCell(int x, int y, int z);
00077 
00081   void setCell(int x, int y, int z, T& obj);
00082 
00086   const T& getCell(int x, int y, int z) const;
00087 
00091   void reset(T initial);
00092 
00093   enum Dimension
00094   {
00095     DIM_X = 0,
00096     DIM_Y = 1,
00097     DIM_Z = 2
00098   };
00099 
00103   double getSize(Dimension dim) const;
00104 
00108   double getResolution(Dimension dim) const;
00109 
00113   double getOrigin(Dimension dim) const;
00114 
00118   int getNumCells(Dimension dim) const;
00119 
00123   bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00124 
00128   bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00129 
00130 protected:
00131   T* data_;                     
00132   T default_object_;            
00133   T*** data_ptrs_;
00134   double size_[3];
00135   double resolution_[3];
00136   double origin_[3];
00137   int num_cells_[3];
00138   int num_cells_total_;
00139   int stride1_;
00140   int stride2_;
00141 
00145   int ref(int x, int y, int z) const;
00146 
00150   int getCellFromLocation(Dimension dim, double loc) const;
00151 
00155   double getLocationFromCell(Dimension dim, int cell) const;
00156 
00160   bool isCellValid(int x, int y, int z) const;
00161 
00165   bool isCellValid(Dimension dim, int cell) const;
00166 };
00167 
00169 
00170 template<typename T>
00171 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00172     double origin_x, double origin_y, double origin_z, T default_object)
00173 {
00174   size_[DIM_X] = size_x;
00175   size_[DIM_Y] = size_y;
00176   size_[DIM_Z] = size_z;
00177   origin_[DIM_X] = origin_x;
00178   origin_[DIM_Y] = origin_y;
00179   origin_[DIM_Z] = origin_z;
00180   num_cells_total_ = 1;
00181   for (int i=DIM_X; i<=DIM_Z; ++i)
00182   {
00183     resolution_[i] = resolution;
00184     num_cells_[i] = size_[i] / resolution_[i];
00185     num_cells_total_ *= num_cells_[i];
00186   }
00187   default_object_ = default_object;
00188 
00189   stride1_ = num_cells_[DIM_Y]*num_cells_[DIM_Z];
00190   stride2_ = num_cells_[DIM_Z];
00191 
00192   // initialize the data:
00193   data_ = new T[num_cells_total_];
00194 
00195 }
00196 
00197 template<typename T>
00198 VoxelGrid<T>::~VoxelGrid()
00199 {
00200   delete[] data_;
00201 }
00202 
00203 template<typename T>
00204 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00205 {
00206   return (
00207       x>=0 && x<num_cells_[DIM_X] &&
00208       y>=0 && y<num_cells_[DIM_Y] &&
00209       z>=0 && z<num_cells_[DIM_Z]);
00210 }
00211 
00212 template<typename T>
00213 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00214 {
00215   return cell>=0 && cell<num_cells_[dim];
00216 }
00217 
00218 template<typename T>
00219 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00220 {
00221   return x*stride1_ + y*stride2_ + z;
00222 }
00223 
00224 template<typename T>
00225 inline double VoxelGrid<T>::getSize(Dimension dim) const
00226 {
00227   return size_[dim];
00228 }
00229 
00230 template<typename T>
00231 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00232 {
00233   return resolution_[dim];
00234 }
00235 
00236 template<typename T>
00237 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00238 {
00239   return origin_[dim];
00240 }
00241 
00242 template<typename T>
00243 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00244 {
00245   return num_cells_[dim];
00246 }
00247 
00248 template<typename T>
00249 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00250 {
00251   int cellX = getCellFromLocation(DIM_X, x);
00252   int cellY = getCellFromLocation(DIM_Y, y);
00253   int cellZ = getCellFromLocation(DIM_Z, z);
00254   if (!isCellValid(cellX, cellY, cellZ))
00255     return default_object_;
00256   return getCell(cellX, cellY, cellZ);
00257 }
00258 
00259 template<typename T>
00260 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00261 {
00262   return data_[ref(x,y,z)];
00263 }
00264 
00265 template<typename T>
00266 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00267 {
00268   return data_[ref(x,y,z)];
00269 }
00270 
00271 template<typename T>
00272 inline void VoxelGrid<T>::setCell(int x, int y, int z, T& obj)
00273 {
00274   data_[ref(x,y,z)] = obj;
00275 }
00276 
00277 template<typename T>
00278 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00279 {
00280   return int(round((loc-origin_[dim])/resolution_[dim]));
00281 }
00282 
00283 template<typename T>
00284 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00285 {
00286   return origin_[dim] + resolution_[dim]*(double(cell));
00287 }
00288 
00289 
00290 template<typename T>
00291 inline void VoxelGrid<T>::reset(T initial)
00292 {
00293   std::fill(data_, data_+num_cells_total_, initial);
00294 }
00295 
00296 template<typename T>
00297 inline bool VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00298 {
00299   world_x = getLocationFromCell(DIM_X, x);
00300   world_y = getLocationFromCell(DIM_Y, y);
00301   world_z = getLocationFromCell(DIM_Z, z);
00302   return true;
00303 }
00304 
00305 template<typename T>
00306 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00307 {
00308   x = getCellFromLocation(DIM_X, world_x);
00309   y = getCellFromLocation(DIM_Y, world_y);
00310   z = getCellFromLocation(DIM_Z, world_z);
00311   return isCellValid(x,y,z);
00312 }
00313 
00314 } // namespace distance_field
00315 #endif /* DF_VOXEL_GRID_H_ */