Orca Robotics

00001 #ifndef HYDROOGMAP_GENERICMAP_H
00002 #define HYDROOGMAP_GENERICMAP_H
00003 
00004 #include <assert.h>
00005 #include <cmath>
00006 #include <vector>
00007 #include <string>
00008 #include <ostream>
00009 #include <hydroportability/sharedlib.h>
00010 
00011 namespace hydroogmap {
00012 
00013     struct CartesianPoint2d
00014     {
00016         double x;
00018         double y;
00019     };
00020     SOEXPORT std::string toString(const CartesianPoint2d &p);
00021 
00022     struct Frame2d
00023     {
00025         CartesianPoint2d p;
00027         double o;
00028     };
00029     SOEXPORT std::string toString(const Frame2d &f);
00030 
00033     template<typename T>
00034     class SOEXPORT GenericMap
00035     {
00036     public:
00037 
00038         GenericMap()
00039             {
00040                 reallocate(0,0);
00041                 metresPerCellX_ = 0;
00042                 metresPerCellY_ = 0;
00043                 offset_.p.x = 0;
00044                 offset_.p.y = 0;
00045                 offset_.o = 0;
00046             }
00047 
00048         GenericMap( int numCellsX, 
00049                     int numCellsY, 
00050                     double offsetX,
00051                     double offsetY,
00052                     double offsetTheta,
00053                     double metresPerCellX,
00054                     double metresPerCellY,
00055                     const T &initialValue )
00056             {
00057                 reallocate( numCellsX, numCellsY );
00058                 metresPerCellX_ = metresPerCellX;
00059                 metresPerCellY_ = metresPerCellY;
00060                 offset_.p.x = offsetX;
00061                 offset_.p.y = offsetY;
00062                 offset_.o = offsetTheta;
00063                 fill( initialValue );
00064             }
00065 
00066         GenericMap( int numCellsX, 
00067                     int numCellsY, 
00068                     const Frame2d &offset,
00069                     double metresPerCellX,
00070                     double metresPerCellY,
00071                     const T &initialValue )
00072             {
00073                 reallocate( numCellsX, numCellsY );
00074                 metresPerCellX_ = metresPerCellX;
00075                 metresPerCellY_ = metresPerCellY;
00076                 offset_ = offset;
00077                 fill( initialValue );
00078             }
00079 
00080         // member access functions
00081         
00083         int numCellsX() const { return numCellsX_; };
00084 
00086         int numCellsY() const { return numCellsY_; };
00087 
00089         double worldSizeX() const { return numCellsX_*metresPerCellX_; };
00091         double worldSizeY() const { return numCellsY_*metresPerCellY_; };
00092 
00094         Frame2d &offset() { return offset_; };
00096         const Frame2d &offset() const { return offset_; };
00097         
00099         void setMetresPerCellX( double mx ) { metresPerCellX_ = mx; };
00101         double metresPerCellX() const { return metresPerCellX_; };
00102         
00104         void setMetresPerCellY( double my ) { metresPerCellY_ = my; };
00106         double metresPerCellY() const { return metresPerCellY_; };
00107 
00109         T* data() { return &data_[0]; };
00111         const T* data() const { return &(data_[0]); };
00112 
00116         double worldXCoord( int gridX ) const
00117             { return double(offset_.p.x + (gridX+0.5)*metresPerCellX_); }
00121         double worldYCoord( int gridY ) const
00122             { return double(offset_.p.y + (gridY+0.5)*metresPerCellY_); }
00123 
00127         void getWorldCoords( int gridX, int gridY, double &worldX, double &worldY ) const
00128             {
00129                 // Todo: handle non-zero orientation.
00130                 assert(offset_.o == 0.0 );
00131 
00132                 worldX = worldXCoord( gridX );
00133                 worldY = worldYCoord( gridY );
00134             }
00135 
00138         void getCellIndices( double worldX, double worldY, int &gridX, int &gridY ) const
00139             {
00140                 gridX = worldToIndexX( worldX );
00141                 gridY = worldToIndexY( worldY );
00142             }
00143 
00146         T &gridCell( int indX, int indY )
00147         {
00148             return data_[indY*numCellsX() + indX];
00149         }        
00152         const T &gridCell( int indX, int indY ) const
00153         {
00154             return data_[indY*numCellsX() + indX];
00155         }
00156     
00159         T &worldCell( double worldX, double worldY )
00160         { 
00161             assert( offset_.o == 0.0 );
00162             return gridCell( worldToIndexX(worldX), worldToIndexY(worldY) ); 
00163         }
00166         const T &worldCell( double worldX, double worldY ) const
00167         { 
00168             assert( offset_.o == 0.0 );
00169             return gridCell( worldToIndexX(worldX), worldToIndexY(worldY) ); 
00170         }
00171 
00173         bool tryGridCell( int indX, int indY, T &cell ) const
00174             {
00175                 double worldX, worldY;
00176                 getWorldCoords( indX, indY, worldX, worldY );
00177                 return tryWorldCell( worldX, worldY, cell);
00178             }
00179 
00181         bool tryWorldCell( double worldX, double worldY, T &cell ) const
00182             {
00183                 if ( coordsWithinMap( worldX, worldY ) == true )
00184                 {
00185                     cell = worldCell( worldX, worldY );
00186                     return true;
00187                 }
00188                 return false;
00189             }
00190 
00192         int worldToIndexX( double worldX ) const
00193         { assert(offset_.o==0.0); return (int) std::floor( (worldX-offset_.p.x)/metresPerCellX_ ); }
00194         int worldToIndexY( double worldY ) const
00195         { assert(offset_.o==0.0); return (int) std::floor( (worldY-offset_.p.y)/metresPerCellY_ ); }
00196     
00198         void reallocate( int numCellsX, int numCellsY )
00199             {
00200                 numCellsX_ = numCellsX;
00201                 numCellsY_ = numCellsY;
00202                 data_.resize( numCellsX_ * numCellsY_ );
00203             }
00204 
00206         void fill( const T &cellValue )
00207             {
00208                 std::fill( data_.begin(), data_.end(), cellValue );
00209             }
00210                 
00211     
00213         bool coordsWithinMap( double worldX, double worldY ) const
00214         { 
00215             assert( offset_.o == 0.0 );
00216             return ( worldX >= offset_.p.x &&
00217                      worldY >= offset_.p.y &&
00218                      worldX <  offset_.p.x+numCellsX()*metresPerCellX() &&
00219                      worldY <  offset_.p.y+numCellsY()*metresPerCellY() );
00220         }
00221 
00223         bool cellWithinMap( double gridX, double gridY ) const
00224         { 
00225             return ( gridX >= 0 &&
00226                      gridY >= 0 &&
00227                      gridX <  numCellsX() &&
00228                      gridY <  numCellsY() );
00229         }
00230 
00231     private:
00232 
00234         Frame2d offset_;
00235     
00237         double metresPerCellX_;
00239         double metresPerCellY_;
00240     
00242         std::vector<T> data_;
00243     
00245         int numCellsX_;
00247         int numCellsY_;
00248     };
00249 
00250     template<typename T>
00251     SOEXPORT std::ostream &operator<<( std::ostream &s, const GenericMap<T> &o )
00252     {
00253         s << " OgMap: \n"
00254           << "\toffset:        [" << o.offset().p.x << ", " << o.offset().p.y << ", " << o.offset().o*180.0/M_PI << "]\n"
00255           << "\tnumCells:      [" << o.numCellsX() << ", " << o.numCellsY() << "]\n"
00256           << "\tmetresPerCell: [" << o.metresPerCellX() << ", " << o.metresPerCellY() << "]\n";
00257 
00258         return s;
00259     }
00260  
00261 }
00262 
00263 #endif
genericmap.h
