grid_map_fixed.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_MAPS_GRID_MAP_FIXED_HPP_
#define ECL_MAPS_GRID_MAP_FIXED_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include "grid_map_commons.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

/*****************************************************************************
** Interface [GridMap]
*****************************************************************************/
template <typename CellType, int Rows = DynamicGridMapStorage, int Cols = Rows>
class GridMap {
public:
        /*********************
        ** Typedefs
        **********************/
        typedef CellType value_type; 
        typedef CellType* iterator; 
        typedef const CellType* const_iterator; 
        typedef CellType& reference;  
        typedef const CellType& const_reference;  
        typedef int size_type; 
        /*********************
        ** Initialisation
        **********************/
        GridMap(const double &origin_x = 0.0, const double &origin_y = 0.0, const double &scale = 1.0);
        void init(const CellType & initialValue= CellType() );

        /*********************
        ** 2D Accessors
        **********************/
        CellType& operator()(const int& x, const int& y) ecl_assert_throw_decl(StandardException);
        CellType& operator()(const double& x, const double& y) ecl_assert_throw_decl(StandardException);

        /*********************
        ** 1D Iterators
        **********************/
        size_type size() const { return Rows*Cols; } 
        iterator begin() { return cells; } 
        const_iterator begin() const { return cells; } 
        iterator end() { return cells+Rows*Cols; } 
        const_iterator end() const { return cells+Rows*Cols; } 
        reference front() { return cells[0]; } 
        const_reference front() const { return cells[0]; } 
        reference back() { return cells[Rows*Cols-1]; } 
        const_reference back() const { return cells[Rows*Cols-1]; } 
        /*********************
        ** Conversions
        **********************/
        linear_algebra::Vector2i cellCoordinates(const double &x, const double &y ) const ecl_assert_throw_decl(StandardException);
        linear_algebra::Vector2d worldCoordinates(const int &x, const int &y ) const ecl_assert_throw_decl(StandardException);

        /*********************
        ** Utility
        **********************/
        double cellWidth() const { return scale_multiplier; } 
        size_type rows() const { return Rows; } 
        size_type cols() const { return Cols; } 
private:
        double scale_multiplier;
        linear_algebra::Vector2d origin;
        linear_algebra::Vector2d limits;
        CellType cells[Rows*Cols];
};

/*****************************************************************************
** Implementation [GridMap][Constructors]
*****************************************************************************/
template <typename CellType, int Rows, int Cols>
GridMap<CellType,Rows,Cols>::GridMap(const double &origin_x, const double &origin_y,  const double &scale) :
        scale_multiplier(scale)
{
        ecl_compile_time_assert( (Rows >= 0) && (Cols >= 0) );

        origin << origin_x, origin_y;
        limits << origin[0] + scale_multiplier*Cols, origin[1] + scale_multiplier*Rows;
}

template <typename CellType, int Rows, int Cols>
void GridMap<CellType,Rows,Cols>::init(const CellType & initialValue ) {
        for ( int i = 0; i < Rows*Cols; ++i ) {
                cells[i] = initialValue;//CellType();
        }
}

/*****************************************************************************
** Implementation [GridMap][Access]
*****************************************************************************/

template <typename CellType, int Rows, int Cols>
CellType& GridMap<CellType,Rows,Cols>::operator() (const int& x, const int& y) ecl_assert_throw_decl(StandardException) {
        ecl_assert_throw( ( ( x >= 0 ) && (x <= Cols) ), StandardException(LOC,OutOfRangeError) );
        ecl_assert_throw( ( ( y >= 0 ) && (y <= Rows) ), StandardException(LOC,OutOfRangeError) );

        return cells[Cols*y+x];
}

template <typename CellType, int Rows, int Cols>
CellType& GridMap<CellType,Rows,Cols>::operator() (const double& x, const double& y) ecl_assert_throw_decl(StandardException) {

        ecl_assert_throw( ( ( x >= origin[0] ) && (x <= limits[0]) ), StandardException(LOC,OutOfRangeError) );
        ecl_assert_throw( ( ( y >= origin[1] ) && (y <= limits[1]) ), StandardException(LOC,OutOfRangeError) );

        int cell_x = static_cast<int>( ( x - origin[0] )/scale_multiplier );
        int cell_y = static_cast<int>( ( y - origin[1] )/scale_multiplier );
        return cells[Cols*cell_y+cell_x];
}

/*****************************************************************************
** Implementation [GridMap][Conversions]
*****************************************************************************/

template <typename CellType, int Rows, int Cols>
linear_algebra::Vector2i GridMap<CellType,Rows,Cols>::cellCoordinates (const double& x, const double& y) const ecl_assert_throw_decl(StandardException) {

        ecl_assert_throw( ( ( x >= origin[0] ) && (x <= limits[0]) ), StandardException(LOC,OutOfRangeError) );
        ecl_assert_throw( ( ( y >= origin[1] ) && (y <= limits[1]) ), StandardException(LOC,OutOfRangeError) );

        linear_algebra::Vector2i cell_coordinates;
        cell_coordinates << static_cast<int>( ( x - origin[0] )/scale_multiplier ),
                                                static_cast<int>( ( y - origin[1] )/scale_multiplier );
        return cell_coordinates;
}

template <typename CellType, int Rows, int Cols>
ecl::linear_algebra::Vector2d GridMap<CellType,Rows,Cols>::worldCoordinates (const int& x, const int& y) const ecl_assert_throw_decl(StandardException) {

        ecl_assert_throw( ( ( x >= 0 ) && (x <= Cols) ), StandardException(LOC,OutOfRangeError) );
        ecl_assert_throw( ( ( y >= 0 ) && (y <= Rows) ), StandardException(LOC,OutOfRangeError) );

        linear_algebra::Vector2d world_coordinates;
        world_coordinates << origin[0] + scale_multiplier*static_cast<double>(x),
                                                 origin[1] + scale_multiplier*static_cast<double>(y);
        return world_coordinates;
}


} // namespace ecl

#endif /* ECL_MAPS_GRID_MAP_FIXED_HPP_ */