ecl_maps: ecl_maps: grid_map_dynamic.hpp Source File

00001 
00008 /*****************************************************************************
00009 ** Ifdefs
00010 *****************************************************************************/
00011 
00012 #ifndef ECL_MAPS_GRID_MAP_DYNAMIC_HPP_
00013 #define ECL_MAPS_GRID_MAP_DYNAMIC_HPP_
00014 
00015 /*****************************************************************************
00016 ** Includes
00017 *****************************************************************************/
00018 
00019 #include "grid_map_fixed.hpp"
00020 
00021 /*****************************************************************************
00022 ** Namespaces
00023 *****************************************************************************/
00024 
00025 namespace ecl {
00026 
00027 /*****************************************************************************
00028 ** Interface [GridMap]
00029 *****************************************************************************/
00048 template <typename CellType>
00049 class GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage> {
00050 public:
00051         /*********************
00052         ** Typedefs
00053         **********************/
00054         typedef CellType value_type; 
00055         typedef CellType* iterator; 
00056         typedef const CellType* const_iterator; 
00057         typedef CellType& reference;  
00058         typedef const CellType& const_reference;  
00059         typedef int size_type; 
00061         /*********************
00062         ** Initialisation
00063         **********************/
00064         GridMap(const int & numOfRows=1, const int & numOfCols=1,
00065                         const double &origin_x = 0.0, const double &origin_y = 0.0,
00066                         const double &scale = 1.0);
00067         ~GridMap();
00068         void init(const CellType & initialValue= CellType() );
00069 
00070         /*********************
00071         ** 2D Accessors
00072         **********************/
00073         CellType& operator()(const int& x, const int& y) ecl_assert_throw_decl(StandardException);
00074         CellType& operator()(const double& x, const double& y) ecl_assert_throw_decl(StandardException);
00075 
00076         /*********************
00077         ** 1D Iterators
00078         **********************/
00079         size_type size() const { return num_of_rows*num_of_cols; } 
00080         iterator begin() { return cells; } 
00081         const_iterator begin() const { return cells; } 
00082         iterator end() { return cells+num_of_rows*num_of_cols; } 
00083         const_iterator end() const { return cells+num_of_rows*num_of_cols; } 
00084         reference front() { return cells[0]; } 
00085         const_reference front() const { return cells[0]; } 
00086         reference back() { return cells[num_of_rows*num_of_cols-1]; } 
00087         const_reference back() const { return cells[num_of_rows*num_of_cols-1]; } 
00089         /*********************
00090         ** Conversions
00091         **********************/
00092         linear_algebra::Vector2i cellCoordinates(const double &x, const double &y ) const ecl_assert_throw_decl(StandardException);
00093         linear_algebra::Vector2d worldCoordinates(const int &x, const int &y ) const ecl_assert_throw_decl(StandardException);
00094         iterator cell(const int &x, const int &y) const ecl_assert_throw_decl(StandardException)
00095         {
00096                 ecl_assert_throw( ( ( x >= 0 ) && (x <= num_of_cols) ), StandardException(LOC,OutOfRangeError) );
00097                 ecl_assert_throw( ( ( y >= 0 ) && (y <= num_of_rows) ), StandardException(LOC,OutOfRangeError) );
00098                 return (cells +num_of_cols*y+x);
00099         }
00100         iterator cell(const double &x, const double &y) const ecl_assert_throw_decl(StandardException)
00101         {
00102                 int cx = static_cast<int>( ( x - origin[0] )/scale_multiplier );
00103                 int cy = static_cast<int>( ( y - origin[1] )/scale_multiplier );
00104                 return cell(cx,cy);
00105         }
00106 
00107         bool cell(const int &x, const int &y, iterator & it) const
00108         {
00109                 if( x < 0 || x>= num_of_cols ) return false;
00110                 if( y < 0 || y>= num_of_rows ) return false;
00111                 it = cells + num_of_cols*y+x;
00112                 return true;
00113         }
00114         bool cell(const double &x, const double &y, iterator & it) const
00115         {
00116                 int cx = static_cast<int>( ( x - origin[0] )/scale_multiplier );
00117                 int cy = static_cast<int>( ( y - origin[1] )/scale_multiplier );
00118                 return cell(cx,cy, it);
00119         }
00120 
00121 
00122         /*********************
00123         ** Utility
00124         **********************/
00125         double cellWidth() const { return scale_multiplier; } 
00126         size_type rows() const { return num_of_rows; } 
00127         size_type cols() const { return num_of_cols; } 
00128         void remap(const int & numOfRows=1, const int & numOfCols=1,
00129                                 const double &origin_x = 0.0, const double &origin_y = 0.0,
00130                                 const double &scale = 1.0);
00131         linear_algebra::Vector2d originVector() const { return origin; }
00132         void operator=(const GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage> & gridMap );
00133         bool operator == (const GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage> & gridMap )
00134         {
00135                 if( num_of_rows != gridMap.rows() ||
00136                         num_of_cols != gridMap.cols() ||
00137                         origin[0] != gridMap.originVector()[0] ||
00138                         origin[1] != gridMap.originVector()[1] ||
00139                         scale_multiplier != gridMap.cellWidth() )
00140                         return false;
00141                 else
00142                         return true;
00143         }
00144 
00145         bool operator != (const GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage> & gridMap )
00146         {
00147                 if( num_of_rows != gridMap.rows() ||
00148                         num_of_cols != gridMap.cols() ||
00149                         origin[0] != gridMap.originVector()[0] ||
00150                         origin[1] != gridMap.originVector()[1] ||
00151                         scale_multiplier != gridMap.cellWidth() )
00152                         return true;
00153                 else
00154                         return false;
00155         }
00156 
00157 private:
00158         double scale_multiplier;
00159         linear_algebra::Vector2d origin;
00160         linear_algebra::Vector2d limits;
00161         CellType * cells;
00162         int num_of_rows;
00163         int num_of_cols;
00164 };
00165 
00166 /*****************************************************************************
00167 ** Implementation [GridMap][Constructors]
00168 *****************************************************************************/
00189 template <typename CellType>
00190 GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00191 ::GridMap( const int & numOfRows, const int & numOfCols, const double &origin_x, const double &origin_y, const double &scale)
00192     :scale_multiplier(scale),
00193         num_of_rows(numOfRows),
00194         num_of_cols(numOfCols)
00195 {
00196         ecl_assert_throw( (num_of_rows > 0) && (num_of_cols > 0), StandardException(LOC,OutOfRangeError) );
00197 
00198         origin << origin_x, origin_y;
00199         limits << origin[0] + scale_multiplier*num_of_cols, origin[1] + scale_multiplier*num_of_rows;
00200 
00201         cells = new CellType[ num_of_rows * num_of_cols ];
00202 }
00203 
00204 template <typename CellType>
00205 void GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00206 ::remap( const int & numOfRows, const int & numOfCols, const double &origin_x, const double &origin_y, const double &scale)
00207 {
00208         scale_multiplier = scale;
00209         num_of_rows = numOfRows;
00210         num_of_cols = numOfCols;
00211 
00212         ecl_assert_throw( (num_of_rows > 0) && (num_of_cols > 0), StandardException(LOC,OutOfRangeError) );
00213 
00214         origin << origin_x, origin_y;
00215         limits << origin[0] + scale_multiplier*num_of_cols, origin[1] + scale_multiplier*num_of_rows;
00216 
00217         if( cells != 0 )        delete [] cells;
00218 
00219         cells = new CellType[ num_of_rows * num_of_cols ];
00220 }
00221 
00222 template<typename CellType>
00223 GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00224 ::~GridMap()
00225 {
00226         delete [] cells;
00227 }
00228 
00229 
00235 template<typename CellType>
00236 void GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00237 ::init(const CellType & initialValue ) {
00238         for ( int i = 0; i < num_of_rows*num_of_cols; ++i ) {
00239                 cells[i] = initialValue;//CellType();
00240         }
00241 }
00242 
00243 /*****************************************************************************
00244 ** Implementation [GridMap][Access]
00245 *****************************************************************************/
00246 
00247 template<typename CellType>
00248 CellType& GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00249 ::operator() (const int& x, const int& y) ecl_assert_throw_decl(StandardException) {
00250         ecl_assert_throw( ( ( x >= 0 ) && (x <= num_of_cols) ), StandardException(LOC,OutOfRangeError) );
00251         ecl_assert_throw( ( ( y >= 0 ) && (y <= num_of_rows) ), StandardException(LOC,OutOfRangeError) );
00252 
00253         return cells[num_of_cols*y+x];
00254 }
00255 
00256 template<typename CellType>
00257 CellType& GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00258 ::operator() (const double& x, const double& y) ecl_assert_throw_decl(StandardException) {
00259 
00260         ecl_assert_throw( ( ( x >= origin[0] ) && (x <= limits[0]) ), StandardException(LOC,OutOfRangeError) );
00261         ecl_assert_throw( ( ( y >= origin[1] ) && (y <= limits[1]) ), StandardException(LOC,OutOfRangeError) );
00262 
00263         int cell_x = static_cast<int>( ( x - origin[0] )/scale_multiplier );
00264         int cell_y = static_cast<int>( ( y - origin[1] )/scale_multiplier );
00265         return cells[num_of_cols*cell_y+cell_x];
00266 }
00267 
00268 /*****************************************************************************
00269 ** Implementation [GridMap][Conversions]
00270 *****************************************************************************/
00271 
00272 template<typename CellType>
00273 linear_algebra::Vector2i GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00274 ::cellCoordinates (const double& x, const double& y) const ecl_assert_throw_decl(StandardException) {
00275 
00276         ecl_assert_throw( ( ( x >= origin[0] ) && (x <= limits[0]) ), StandardException(LOC,OutOfRangeError) );
00277         ecl_assert_throw( ( ( y >= origin[1] ) && (y <= limits[1]) ), StandardException(LOC,OutOfRangeError) );
00278 
00279         linear_algebra::Vector2i cell_coordinates;
00280         cell_coordinates << static_cast<int>( ( x - origin[0] )/scale_multiplier ),
00281                                                 static_cast<int>( ( y - origin[1] )/scale_multiplier );
00282         return cell_coordinates;
00283 }
00284 
00285 template<typename CellType>
00286 ecl::linear_algebra::Vector2d GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00287 ::worldCoordinates (const int& x, const int& y) const ecl_assert_throw_decl(StandardException) {
00288 
00289         ecl_assert_throw( ( ( x >= 0 ) && (x <= num_of_cols) ), StandardException(LOC,OutOfRangeError) );
00290         ecl_assert_throw( ( ( y >= 0 ) && (y <= num_of_rows) ), StandardException(LOC,OutOfRangeError) );
00291 
00292         linear_algebra::Vector2d world_coordinates;
00293         world_coordinates << origin[0] + scale_multiplier*static_cast<double>(x),
00294                                                  origin[1] + scale_multiplier*static_cast<double>(y);
00295         return world_coordinates;
00296 }
00297 
00298 
00299 template<typename CellType>
00300 void GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage>
00301 ::operator = (const GridMap<CellType,DynamicGridMapStorage,DynamicGridMapStorage> & gridMap )
00302 {
00303         if( num_of_rows != gridMap.rows() ||
00304                 num_of_cols != gridMap.cols() ||
00305                 origin[0] != gridMap.originVector()[0] ||
00306                 origin[1] != gridMap.originVector()[1] ||
00307                 scale_multiplier != gridMap.cellWidth() )
00308         {
00309                 remap( gridMap.rows(), gridMap.cols(), gridMap.originVector()[0], gridMap.originVector()[1], gridMap.cellWidth() );
00310         }
00311 
00312         std::copy( gridMap.begin(), gridMap.end(), begin());
00313 }
00314 
00315 
00316 
00317 } // namespace ecl
00318 
00319 #endif /* ECL_MAPS_GRID_MAP_DYNAMIC_HPP_ */
00320 
00321