$search
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_ */