00001 #ifndef MAP_H
00002 #define MAP_H
00003 #include <gmapping/utils/point.h>
00004 #include <assert.h>
00005 #include "accessstate.h"
00006 #include "array2d.h"
00007
00008 namespace GMapping {
00013 typedef Array2D<double> DoubleArray2D;
00014
00015 template <class Cell, class Storage, const bool isClass=true>
00016 class Map{
00017 public:
00018 Map(int mapSizeX, int mapSizeY, double delta);
00019 Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
00020 Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
00021
00022
00023
00024 void resize(double xmin, double ymin, double xmax, double ymax);
00025 void grow(double xmin, double ymin, double xmax, double ymax);
00026 inline IntPoint world2map(const Point& p) const;
00027 inline Point map2world(const IntPoint& p) const;
00028 inline IntPoint world2map(double x, double y) const
00029 { return world2map(Point(x,y)); }
00030 inline Point map2world(int x, int y) const
00031 { return map2world(IntPoint(x,y)); }
00032
00033 inline Point getCenter() const {return m_center;}
00034 inline double getWorldSizeX() const {return m_worldSizeX;}
00035 inline double getWorldSizeY() const {return m_worldSizeY;}
00036 inline int getMapSizeX() const {return m_mapSizeX;}
00037 inline int getMapSizeY() const {return m_mapSizeY;}
00038 inline double getDelta() const { return m_delta;}
00039 inline double getMapResolution() const { return m_delta;}
00040 inline double getResolution() const { return m_delta;}
00041 inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
00042 Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1));
00043 xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y;
00044 }
00045
00046 inline Cell& cell(int x, int y) {
00047 return cell(IntPoint(x, y));
00048 }
00049 inline Cell& cell(const IntPoint& p);
00050
00051 inline const Cell& cell(int x, int y) const {
00052 return cell(IntPoint(x, y));
00053 }
00054 inline const Cell& cell(const IntPoint& p) const;
00055
00056 inline Cell& cell(double x, double y) {
00057 return cell(Point(x, y));
00058 }
00059 inline Cell& cell(const Point& p);
00060
00061 inline const Cell& cell(double x, double y) const {
00062 return cell(Point(x, y));
00063 }
00064
00065 inline bool isInside(int x, int y) const {
00066 return m_storage.cellState(IntPoint(x,y))&Inside;
00067 }
00068 inline bool isInside(const IntPoint& p) const {
00069 return m_storage.cellState(p)&Inside;
00070 }
00071
00072 inline bool isInside(double x, double y) const {
00073 return m_storage.cellState(world2map(x,y))&Inside;
00074 }
00075 inline bool isInside(const Point& p) const {
00076 return m_storage.cellState(world2map(p))&Inside;
00077 }
00078
00079 inline const Cell& cell(const Point& p) const;
00080
00081 inline Storage& storage() { return m_storage; }
00082 inline const Storage& storage() const { return m_storage; }
00083 DoubleArray2D* toDoubleArray() const;
00084 Map<double, DoubleArray2D, false>* toDoubleMap() const;
00085
00086 protected:
00087 Point m_center;
00088 double m_worldSizeX, m_worldSizeY, m_delta;
00089 Storage m_storage;
00090 int m_mapSizeX, m_mapSizeY;
00091 int m_sizeX2, m_sizeY2;
00092 static const Cell m_unknown;
00093 };
00094
00095 typedef Map<double, DoubleArray2D, false> DoubleMap;
00096
00097 template <class Cell, class Storage, const bool isClass>
00098 const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
00099
00100 template <class Cell, class Storage, const bool isClass>
00101 Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
00102 m_storage(mapSizeX, mapSizeY){
00103 m_worldSizeX=mapSizeX * delta;
00104 m_worldSizeY=mapSizeY * delta;
00105 m_delta=delta;
00106 m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
00107 m_sizeX2=m_mapSizeX>>1;
00108 m_sizeY2=m_mapSizeY>>1;
00109 }
00110
00111 template <class Cell, class Storage, const bool isClass>
00112 Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
00113 m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
00114 m_center=center;
00115 m_worldSizeX=worldSizeX;
00116 m_worldSizeY=worldSizeY;
00117 m_delta=delta;
00118 m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00119 m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00120 m_sizeX2=m_mapSizeX>>1;
00121 m_sizeY2=m_mapSizeY>>1;
00122 }
00123
00124 template <class Cell, class Storage, const bool isClass>
00125 Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
00126 m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
00127 m_center=center;
00128 m_worldSizeX=xmax-xmin;
00129 m_worldSizeY=ymax-ymin;
00130 m_delta=delta;
00131 m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00132 m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00133 m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
00134 m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
00135 }
00136
00137 template <class Cell, class Storage, const bool isClass>
00138 void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
00139 IntPoint imin=world2map(xmin, ymin);
00140 IntPoint imax=world2map(xmax, ymax);
00141 int pxmin, pymin, pxmax, pymax;
00142 pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
00143 pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
00144 pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
00145 pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
00146 m_storage.resize(pxmin, pymin, pxmax, pymax);
00147 m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00148 m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00149 m_worldSizeX=xmax-xmin;
00150 m_worldSizeY=ymax-ymin;
00151 m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
00152 m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
00153 }
00154
00155 template <class Cell, class Storage, const bool isClass>
00156 void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
00157 IntPoint imin=world2map(xmin, ymin);
00158 IntPoint imax=world2map(xmax, ymax);
00159 if (isInside(imin) && isInside(imax))
00160 return;
00161 imin=min(imin, IntPoint(0,0));
00162 imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
00163 int pxmin, pymin, pxmax, pymax;
00164 pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
00165 pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
00166 pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
00167 pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
00168 m_storage.resize(pxmin, pymin, pxmax, pymax);
00169 m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00170 m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00171 m_worldSizeX=xmax-xmin;
00172 m_worldSizeY=ymax-ymin;
00173 m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
00174 m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
00175 }
00176
00177
00178 template <class Cell, class Storage, const bool isClass>
00179 IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
00180 return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
00181 }
00182
00183 template <class Cell, class Storage, const bool isClass>
00184 Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
00185 return Point( (p.x-m_sizeX2)*m_delta,
00186 (p.y-m_sizeY2)*m_delta)+m_center;
00187 }
00188
00189
00190 template <class Cell, class Storage, const bool isClass>
00191 Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
00192 AccessibilityState s=m_storage.cellState(p);
00193 if (! (s&Inside))
00194 assert(0);
00195
00196
00197
00198 return m_storage.cell(p);
00199
00200 }
00201
00202 template <class Cell, class Storage, const bool isClass>
00203 Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
00204 IntPoint ip=world2map(p);
00205 AccessibilityState s=m_storage.cellState(ip);
00206 if (! (s&Inside))
00207 assert(0);
00208
00209
00210
00211 return m_storage.cell(ip);
00212 }
00213
00214 template <class Cell, class Storage, const bool isClass>
00215 const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
00216 AccessibilityState s=m_storage.cellState(p);
00217
00218 if (s&Allocated)
00219 return m_storage.cell(p);
00220 return m_unknown;
00221 }
00222
00223 template <class Cell, class Storage, const bool isClass>
00224 const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
00225 IntPoint ip=world2map(p);
00226 AccessibilityState s=m_storage.cellState(ip);
00227
00228 if (s&Allocated)
00229 return m_storage.cell(ip);
00230 return m_unknown;
00231 }
00232
00233
00234
00235 template <class Cell, class Storage, const bool isClass>
00236 DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
00237 DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
00238 for(int x=0; x<getMapSizeX()-1; x++)
00239 for(int y=0; y<getMapSizeY()-1; y++){
00240 IntPoint p(x,y);
00241 darr->cell(p)=cell(p);
00242 }
00243 return darr;
00244 }
00245
00246
00247 template <class Cell, class Storage, const bool isClass>
00248 Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
00249
00250 Point pmin=map2world(IntPoint(0,0));
00251 Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
00252 Point center=(pmax+pmin)*0.5;
00253 Map<double, DoubleArray2D, false>* plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
00254 for(int x=0; x<getMapSizeX()-1; x++)
00255 for(int y=0; y<getMapSizeY()-1; y++){
00256 IntPoint p(x,y);
00257 plainMap->cell(p)=cell(p);
00258 }
00259 return plainMap;
00260 }
00261
00262 };
00263
00264 #endif
00265