harray2d.h
Go to the documentation of this file.
00001 #ifndef HARRAY2D_H
00002 #define HARRAY2D_H
00003 #include <set>
00004 #include <gmapping/utils/point.h>
00005 #include <gmapping/utils/autoptr.h>
00006 #include "array2d.h"
00007 
00008 namespace GMapping {
00009 
00010 template <class Cell>
00011 class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
00012         public:
00013                 typedef std::set< point<int>, pointcomparator<int> > PointSet;
00014                 HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
00015                 HierarchicalArray2D(const HierarchicalArray2D& hg);
00016                 HierarchicalArray2D& operator=(const HierarchicalArray2D& hg);
00017                 virtual ~HierarchicalArray2D(){}
00018                 void resize(int ixmin, int iymin, int ixmax, int iymax);
00019                 inline int getPatchSize() const {return m_patchMagnitude;}
00020                 inline int getPatchMagnitude() const {return m_patchMagnitude;}
00021                 
00022                 inline const Cell& cell(int x, int y) const;
00023                 inline Cell& cell(int x, int y);
00024                 inline bool isAllocated(int x, int y) const;
00025                 inline AccessibilityState cellState(int x, int y) const ;
00026                 inline IntPoint patchIndexes(int x, int y) const;
00027                 
00028                 inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); }
00029                 inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); }
00030                 inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);}
00031                 inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); }
00032                 inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);}
00033                 
00034                 inline void setActiveArea(const PointSet&, bool patchCoords=false);
00035                 const PointSet& getActiveArea() const {return m_activeArea; }
00036                 inline void allocActiveArea();
00037         protected:
00038                 virtual Array2D<Cell> * createPatch(const IntPoint& p) const;
00039                 PointSet m_activeArea;
00040                 int m_patchMagnitude;
00041                 int m_patchSize;
00042 };
00043 
00044 template <class Cell>
00045 HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) 
00046   :Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
00047         m_patchMagnitude=patchMagnitude;
00048         m_patchSize=1<<m_patchMagnitude;
00049 }
00050 
00051 template <class Cell>
00052 HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
00053   :Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude))  // added by cyrill: if you have a resize error, check this again
00054 {
00055         this->m_xsize=hg.m_xsize;
00056         this->m_ysize=hg.m_ysize;
00057         this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
00058         for (int x=0; x<this->m_xsize; x++){
00059                 this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
00060                 for (int y=0; y<this->m_ysize; y++)
00061                         this->m_cells[x][y]=hg.m_cells[x][y];
00062         }
00063         this->m_patchMagnitude=hg.m_patchMagnitude;
00064         this->m_patchSize=hg.m_patchSize;
00065 }
00066 
00067 template <class Cell>
00068 void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
00069         int xsize=xmax-xmin;
00070         int ysize=ymax-ymin;
00071         autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
00072         for (int x=0; x<xsize; x++){
00073                 newcells[x]=new autoptr< Array2D<Cell> >[ysize];
00074                 for (int y=0; y<ysize; y++){
00075                         newcells[x][y]=autoptr< Array2D<Cell> >(0);
00076                 }
00077         }
00078         int dx= xmin < 0 ? 0 : xmin;
00079         int dy= ymin < 0 ? 0 : ymin;
00080         int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
00081         int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
00082         for (int x=dx; x<Dx; x++){
00083                 for (int y=dy; y<Dy; y++){
00084                         newcells[x-xmin][y-ymin]=this->m_cells[x][y];
00085                 }
00086                 delete [] this->m_cells[x];
00087         }
00088         delete [] this->m_cells;
00089         this->m_cells=newcells;
00090         this->m_xsize=xsize;
00091         this->m_ysize=ysize; 
00092 }
00093 
00094 template <class Cell>
00095 HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
00096 //      Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
00097         if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
00098                 for (int i=0; i<this->m_xsize; i++)
00099                         delete [] this->m_cells[i];
00100                 delete [] this->m_cells;
00101                 this->m_xsize=hg.m_xsize;
00102                 this->m_ysize=hg.m_ysize;
00103                 this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
00104                 for (int i=0; i<this->m_xsize; i++)
00105                         this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
00106         }
00107         for (int x=0; x<this->m_xsize; x++)
00108                 for (int y=0; y<this->m_ysize; y++)
00109                         this->m_cells[x][y]=hg.m_cells[x][y];
00110         
00111         m_activeArea.clear();
00112         m_patchMagnitude=hg.m_patchMagnitude;
00113         m_patchSize=hg.m_patchSize;
00114         return *this;
00115 }
00116 
00117 
00118 template <class Cell>
00119 void HierarchicalArray2D<Cell>::setActiveArea(const typename HierarchicalArray2D<Cell>::PointSet& aa, bool patchCoords){
00120         m_activeArea.clear();
00121         for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) {
00122                 IntPoint p;
00123                 if (patchCoords)
00124                         p=*it;
00125                 else
00126                         p=patchIndexes(*it);
00127                 m_activeArea.insert(p);
00128         }
00129 }
00130 
00131 template <class Cell>
00132 Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
00133         return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude);
00134 }
00135 
00136 
00137 template <class Cell>
00138 AccessibilityState  HierarchicalArray2D<Cell>::cellState(int x, int y) const {
00139         if (this->isInside(patchIndexes(x,y))) {
00140                 if(isAllocated(x,y))
00141                         return (AccessibilityState)((int)Inside|(int)Allocated);
00142                 else
00143                         return Inside;
00144         }
00145         return Outside;
00146 }
00147 
00148 template <class Cell>
00149 void HierarchicalArray2D<Cell>::allocActiveArea(){
00150         for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){
00151                 const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
00152                 Array2D<Cell>* patch=0;
00153                 if (!ptr){
00154                         patch=createPatch(*it);
00155                 } else{ 
00156                         patch=new Array2D<Cell>(*ptr);
00157                 }
00158                 this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
00159         }
00160 }
00161 
00162 template <class Cell>
00163 bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
00164         IntPoint c=patchIndexes(x,y);
00165         autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00166         return (ptr != 0);
00167 }
00168 
00169 template <class Cell>
00170 IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
00171         if (x>=0 && y>=0)
00172                 return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
00173         return IntPoint(-1, -1);
00174 }
00175 
00176 template <class Cell>
00177 Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
00178         IntPoint c=patchIndexes(x,y);
00179         assert(this->isInside(c.x, c.y));
00180         if (!this->m_cells[c.x][c.y]){
00181                 Array2D<Cell>* patch=createPatch(IntPoint(x,y));
00182                 this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
00183                 //cerr << "!!! FATAL: your dick is going to fall down" << endl;
00184         }
00185         autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00186         return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
00187 }
00188 
00189 template <class Cell>
00190 const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
00191         assert(isAllocated(x,y));
00192         IntPoint c=patchIndexes(x,y);
00193         const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00194         return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
00195 }
00196 
00197 };
00198 
00199 #endif
00200 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13