INTRODUCTION
Overview
Download and Install
Quick Start
Documentation
Publications

NONFRAMEWORK CODE
Driver Interfaces
Drivers
Libraries
Utilities

FRAMEWORK CODE
Interfaces
Components
Libraries
Utilities

Full Software Listings

DEVELOPER
Tutorials
Examples
Dev Guide
Dashboard

PEOPLE
Contributors
Users

SourceForge.net Logo
Project
Download
Mailing lists

 

         

genericmap.h

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 
00014     struct WorldCoords {
00015         WorldCoords( double worldX, double worldY )
00016             : x(worldX), y(worldY) {}
00017         WorldCoords() {}
00018         double x;
00019         double y;
00020     };
00021     SOEXPORT std::string toString(const WorldCoords &w);
00022     inline std::ostream &operator<<( std::ostream &s, const WorldCoords &w )
00023     { return s << toString(w); }
00024 
00026     struct GridIndices {
00027         GridIndices( int gridX, int gridY )
00028             : x(gridX), y(gridY) {}
00029         GridIndices() {}
00030         int x;
00031         int y;
00032     };
00033     SOEXPORT std::string toString(const GridIndices &g );
00034     inline std::ostream &operator<<( std::ostream &s, const GridIndices &g )
00035     { return s << toString(g); }
00036 
00037     struct Frame2d
00038     {
00039         Frame2d( const WorldCoords &offsetP, double offsetO )
00040             : p(offsetP), o(offsetO) {}
00041         Frame2d( double offsetX, double offsetY, double offsetO )
00042             : p(offsetX,offsetY), o(offsetO) {}
00043 
00045         WorldCoords p;
00047         double o;
00048     };
00049     SOEXPORT std::string toString(const Frame2d &f);
00050     inline std::ostream &operator<<( std::ostream &s, const Frame2d &f )
00051     { return s << toString(f); }
00052     bool operator==( const Frame2d &f1, const Frame2d &f2 );
00053 
00054     // Describes the map's size and place in the world: everything except the actual cell values.
00055     struct MetaData {
00056         MetaData()
00057             : numCellsX(0),numCellsY(0),metresPerCell(0),offset(0,0,0) {}
00058         MetaData( int            pNumCellsX, 
00059                   int            pNumCellsY, 
00060                   double         pMetresPerCell,
00061                   const Frame2d &pOffset )
00062             : numCellsX(pNumCellsX),
00063               numCellsY(pNumCellsY),
00064               metresPerCell(pMetresPerCell),
00065               offset(pOffset) 
00066             {
00067                 assert(offset.o==0);
00068             }
00069 
00071         bool coordsWithinMap( const WorldCoords &worldCoords ) const
00072             { return cellWithinMap(gridIndices(worldCoords)); }
00074         bool coordsWithinMap( double worldX, double worldY ) const
00075             { return coordsWithinMap(WorldCoords(worldX,worldY)); }
00076 
00078         bool cellWithinMap( int gridX, int gridY ) const
00079             { return cellWithinMap(GridIndices(gridX,gridY)); }
00081         bool cellWithinMap( const GridIndices &gridIndices ) const
00082         { 
00083             return ( gridIndices.x >= 0 &&
00084                      gridIndices.y >= 0 &&
00085                      gridIndices.x <  numCellsX &&
00086                      gridIndices.y <  numCellsY );
00087         }
00088 
00092         double worldXCoord( int gridX ) const
00093             { return double(offset.p.x + (gridX+0.5)*metresPerCell); }
00097         double worldYCoord( int gridY ) const
00098             { return double(offset.p.y + (gridY+0.5)*metresPerCell); }
00099 
00101         int worldToIndexX( double worldX ) const
00102         { return (int) std::floor( (worldX-offset.p.x)/metresPerCell ); }
00103         int worldToIndexY( double worldY ) const
00104         { return (int) std::floor( (worldY-offset.p.y)/metresPerCell ); }
00105 
00109         WorldCoords worldCoords( const GridIndices &gridIndices ) const
00110             { 
00111                 return WorldCoords( worldXCoord(gridIndices.x), worldYCoord(gridIndices.y) );
00112             }
00113         WorldCoords worldCoords( int gridX, int gridY ) const
00114             { return worldCoords( GridIndices(gridX,gridY) ); }
00115 
00118         GridIndices gridIndices( const WorldCoords &worldCoords ) const
00119             { return GridIndices( worldToIndexX(worldCoords.x), worldToIndexY(worldCoords.y) ); }
00120         GridIndices gridIndices( double worldX, double worldY ) const
00121             { return gridIndices( WorldCoords(worldX,worldY) ); }
00122 
00124         double worldSizeX() const { return numCellsX*metresPerCell; }
00126         double worldSizeY() const { return numCellsY*metresPerCell; }
00127 
00128         double minWorldX() const { return offset.p.x; }
00129         double minWorldY() const { return offset.p.y; }
00130         double maxWorldX() const { return minWorldX()+worldSizeX(); }
00131         double maxWorldY() const { return minWorldY()+worldSizeY(); }
00132 
00133         int numCells() const { return numCellsX*numCellsY; }
00134 
00136         int     numCellsX;
00138         int     numCellsY;
00140         double  metresPerCell;
00142         Frame2d offset;
00143     };
00144     SOEXPORT std::string toString(const MetaData &metaData );
00145     inline std::ostream &operator<<( std::ostream &s, const MetaData &m )
00146     { return s << toString(m); }
00147     bool operator==( const MetaData &m1, const MetaData &m2 );
00148     inline bool operator!=( const MetaData &m1, const MetaData &m2 )
00149     { return !(m1==m2); }
00150 
00187     template<typename T>
00188     class SOEXPORT GenericMap
00189     {
00190     public:
00191 
00192         explicit GenericMap()
00193             : metaData_(0,0,0,Frame2d(0,0,0))
00194             { reallocate(0,0); }
00195 
00196         explicit GenericMap( int      numCellsX, 
00197                              int      numCellsY, 
00198                              double   offsetX,
00199                              double   offsetY,
00200                              double   offsetTheta,
00201                              double   metresPerCell,
00202                              const T &initialValue )
00203             : metaData_(numCellsX,numCellsY,metresPerCell,Frame2d(offsetX,offsetY,offsetTheta))
00204             {
00205                 reallocate( metaData_.numCellsX, metaData_.numCellsY );
00206                 fill( initialValue );
00207             }
00208 
00209         explicit GenericMap( int            numCellsX, 
00210                              int            numCellsY, 
00211                              const Frame2d &offset,
00212                              double         metresPerCell,
00213                              const T       &initialValue )
00214             : metaData_(numCellsX,numCellsY,metresPerCell,offset)
00215             {
00216                 reallocate( metaData_.numCellsX, metaData_.numCellsY );
00217                 fill( initialValue );
00218             }
00219 
00220         explicit GenericMap( int            numCellsX, 
00221                              int            numCellsY, 
00222                              const Frame2d &offset,
00223                              double         metresPerCell )
00224             : metaData_(numCellsX,numCellsY,metresPerCell,offset)
00225             { reallocate( metaData_.numCellsX, metaData_.numCellsY ); }
00226 
00227         explicit GenericMap( const MetaData &metaData )
00228             : metaData_(metaData)
00229             { reallocate( metaData_.numCellsX, metaData_.numCellsY ); }
00230 
00231         explicit GenericMap( const MetaData &metaData,
00232                              const T        &initialValue )
00233             : metaData_(metaData)
00234             {
00235                 reallocate( metaData_.numCellsX, metaData_.numCellsY );
00236                 fill( initialValue );
00237             }
00238 
00239         // member access functions
00240         
00242         int numCellsX() const { return metaData_.numCellsX; };
00243 
00245         int numCellsY() const { return metaData_.numCellsY; };
00246 
00248         int numCells() const { return data_.size(); }
00249 
00250         void setMetaData( const MetaData &metaData )
00251             {
00252                 metaData_ = metaData;
00253                 reallocate( metaData_.numCellsX, metaData_.numCellsY );
00254             }
00255         const MetaData &metaData() const { return metaData_; }
00256 
00258         double worldSizeX() const { return metaData_.worldSizeX(); }
00260         double worldSizeY() const { return metaData_.worldSizeY(); }
00261 
00262         double minWorldX() const { return metaData_.minWorldX(); }
00263         double minWorldY() const { return metaData_.minWorldY(); }
00264         double maxWorldX() const { return metaData_.maxWorldX(); }
00265         double maxWorldY() const { return metaData_.maxWorldY(); }
00266 
00268         Frame2d &offset() { return metaData_.offset; };
00270         const Frame2d &offset() const { return metaData_.offset; };
00271         
00273         void setMetresPerCell( double mPerCell ) { metaData_.metresPerCell = mPerCell; }
00275         double metresPerCell() const { return metaData_.metresPerCell; }
00276 
00278         T *data() { return &data_[0]; };
00280         const T *data() const { return &data_[0]; };
00281 
00283         const std::vector<T> &dataVec() const { return data_; };
00284         std::vector<T> &dataVec() { return data_; };
00285 
00289         double worldXCoord( int gridX ) const
00290             { return metaData_.worldXCoord(gridX); }
00294         double worldYCoord( int gridY ) const
00295             { return metaData_.worldYCoord(gridY); }
00296 
00300         void getWorldCoords( int gridX, int gridY, double &worldX, double &worldY ) const
00301             {
00302                 WorldCoords world = worldCoords( GridIndices(gridX,gridY) );
00303                 worldX = world.x; worldY = world.y;
00304             }
00308         WorldCoords worldCoords( const GridIndices &gridIndices ) const
00309             { return metaData_.worldCoords(gridIndices); }
00310         WorldCoords worldCoords( int gridX, int gridY ) const
00311             { return metaData_.worldCoords( GridIndices(gridX,gridY) ); }
00312 
00315         void getCellIndices( double worldX, double worldY, int &gridX, int &gridY ) const
00316             {
00317                 gridX = worldToIndexX( worldX );
00318                 gridY = worldToIndexY( worldY );
00319             }
00320         GridIndices gridIndices( const WorldCoords &worldCoords ) const
00321             { return metaData_.gridIndices(worldCoords); }
00322         GridIndices gridIndices( double worldX, double worldY ) const
00323             { return metaData_.gridIndices( WorldCoords(worldX,worldY) ); }
00324 
00325         // Look up the index into the flat data array.
00326         int dataIndex( const GridIndices &gridIndices ) const
00327             { return gridIndices.y*numCellsX() + gridIndices.x; }
00328         int dataIndex( int x, int y ) const
00329             { return dataIndex(GridIndices(x,y)); }
00330 
00333         T &gridCell( int indX, int indY )
00334             { return gridCell(GridIndices(indX,indY)); }
00337         const T &gridCell( int indX, int indY ) const
00338             { return gridCell(GridIndices(indX,indY)); }
00339 
00342         T &gridCell( const GridIndices &gridIndices )
00343         { return data_[dataIndex(gridIndices)]; }
00346         const T &gridCell( const GridIndices &gridIndices ) const
00347         { return data_[dataIndex(gridIndices)]; }
00348     
00349         
00350 
00353         T &worldCell( double worldX, double worldY )
00354             { return worldCell(WorldCoords(worldX,worldY)); }
00357         const T &worldCell( double worldX, double worldY ) const
00358             { return worldCell(WorldCoords(worldX,worldY)); }
00359 
00362         T &worldCell( const WorldCoords &worldCoords )
00363         { 
00364             return gridCell( gridIndices(worldCoords) );
00365         }
00368         const T &worldCell( const WorldCoords &worldCoords ) const
00369         { 
00370             return gridCell( gridIndices(worldCoords) );
00371         }
00372 
00374         bool tryGridCell( int indX, int indY, T &cell ) const
00375             { return tryGridCell( GridIndices(indX,indY), cell ); }
00376 
00378         bool tryWorldCell( double worldX, double worldY, T &cell ) const
00379             { return tryWorldCell( WorldCoords(worldX,worldY), cell ); }
00380 
00382         bool tryGridCell( const GridIndices &indices, T &cell ) const
00383             { return tryWorldCell( worldCoords(indices), cell); }
00384 
00386         bool tryWorldCell( const WorldCoords &worldCoords, T &cell ) const
00387             {
00388                 if ( !coordsWithinMap( worldCoords ) ) return false;
00389                 cell = worldCell( worldCoords );
00390                 return true;
00391             }
00392 
00394         int worldToIndexX( double worldX ) const
00395         { return metaData_.worldToIndexX(worldX); }
00396         int worldToIndexY( double worldY ) const
00397         { return metaData_.worldToIndexY(worldY); }
00398     
00400         void reallocate( int numCellsX, int numCellsY )
00401             {
00402                 metaData_.numCellsX = numCellsX;
00403                 metaData_.numCellsY = numCellsY;
00404                 data_.resize( metaData_.numCellsX * metaData_.numCellsY );
00405             }
00406 
00408         void fill( const T &cellValue )
00409             {
00410                 std::fill( data_.begin(), data_.end(), cellValue );
00411             }
00412                 
00413     
00415         bool coordsWithinMap( double worldX, double worldY ) const
00416             { return coordsWithinMap(WorldCoords(worldX,worldY)); }
00418         bool coordsWithinMap( const WorldCoords &worldCoords ) const
00419             { return metaData_.coordsWithinMap(worldCoords); }
00420 
00422         bool cellWithinMap( int gridX, int gridY ) const
00423             { return metaData_.cellWithinMap(gridX,gridY); }
00425         bool cellWithinMap( const GridIndices &gridIndices ) const
00426             { return metaData_.cellWithinMap(gridIndices); }
00427 
00428     private:
00429 
00430         // Describes the map's size and position in the world
00431         MetaData metaData_;
00432         // The values
00433         std::vector<T> data_;
00434     };
00435 
00436     template<typename T>
00437     SOEXPORT std::ostream &operator<<( std::ostream &s, const GenericMap<T> &o )
00438     {
00439         s << " OgMap: "
00440           << o.metaData();
00441         return s;
00442     }
00443 }
00444 
00445 #endif
 

Webmaster: Tobias Kaupp (tobasco at users.sourceforge.net)


Generated for Orca Robotics by  doxygen 1.4.5