00001 #ifndef HARRAY2D_H
00002 #define HARRAY2D_H
00003 #include <set>
00004 #include <utils/point.h>
00005 #include <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))
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
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 {
00141 if(isAllocated(x,y))
00142 return (AccessibilityState)((int)Inside|(int)Allocated);
00143 else
00144 return Inside;
00145 }
00146 return Outside;
00147 }
00148
00149 template <class Cell>
00150 void HierarchicalArray2D<Cell>::allocActiveArea(){
00151 for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); it++){
00152 const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
00153 Array2D<Cell>* patch=0;
00154 if (!ptr){
00155 patch=createPatch(*it);
00156 } else{
00157 patch=new Array2D<Cell>(*ptr);
00158 }
00159 this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
00160 }
00161 }
00162
00163 template <class Cell>
00164 bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
00165 IntPoint c=patchIndexes(x,y);
00166 autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00167 return (ptr != 0);
00168 }
00169
00170 template <class Cell>
00171 IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
00172 if (x>=0 && y>=0)
00173 return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
00174 return IntPoint(-1, -1);
00175 }
00176
00177 template <class Cell>
00178 Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
00179 IntPoint c=patchIndexes(x,y);
00180 assert(this->isInside(c.x, c.y));
00181 if (!this->m_cells[c.x][c.y]){
00182 Array2D<Cell>* patch=createPatch(IntPoint(x,y));
00183 this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
00184
00185 }
00186 autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00187 return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
00188 }
00189
00190 template <class Cell>
00191 const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
00192 assert(isAllocated(x,y));
00193 IntPoint c=patchIndexes(x,y);
00194 const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
00195 return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
00196 }
00197
00198 };
00199
00200 #endif
00201