array2d.h

Go to the documentation of this file.

#ifndef ARRAY2D_H
#define ARRAY2D_H

#include <assert.h>
#include <gmapping/utils/point.h>
#include "gmapping/grid/accessstate.h"

#include <iostream>

namespace GMapping {

template<class Cell, const bool debug=false> class Array2D{
        public:
                Array2D(int xsize=0, int ysize=0);
                Array2D& operator=(const Array2D &);
                Array2D(const Array2D<Cell,debug> &);
                ~Array2D();
                void clear();
                void resize(int xmin, int ymin, int xmax, int ymax);
                
                
                inline bool isInside(int x, int y) const;
                inline const Cell& cell(int x, int y) const;
                inline Cell& cell(int x, int y);
                inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);}
                
                inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);}
                inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);}
                inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);}
                inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);}
                
                inline int getPatchSize() const{return 0;}
                inline int getPatchMagnitude() const{return 0;}
                inline int getXSize() const {return m_xsize;}
                inline int getYSize() const {return m_ysize;}
                inline Cell** cells() {return m_cells;}
                Cell ** m_cells;
        protected:
                int m_xsize, m_ysize;
};


template <class Cell, const bool debug>
Array2D<Cell,debug>::Array2D(int xsize, int ysize){
//      assert(xsize>0);
//      assert(ysize>0);
        m_xsize=xsize;
        m_ysize=ysize;
        if (m_xsize>0 && m_ysize>0){
                m_cells=new Cell*[m_xsize];
                for (int i=0; i<m_xsize; i++)
                        m_cells[i]=new Cell[m_ysize];
        }
        else{
                m_xsize=m_ysize=0;
                m_cells=0;
        }
        if (debug){
                std::cerr << __func__ << std::endl;
                std::cerr << "m_xsize= " << m_xsize<< std::endl;
                std::cerr << "m_ysize= " << m_ysize<< std::endl;
        }
}

template <class Cell, const bool debug>
Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g){
        if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){
                for (int i=0; i<m_xsize; i++)
                        delete [] m_cells[i];
                delete [] m_cells;
                m_xsize=g.m_xsize;
                m_ysize=g.m_ysize;
                m_cells=new Cell*[m_xsize];
                for (int i=0; i<m_xsize; i++)
                        m_cells[i]=new Cell[m_ysize];
        }
        for (int x=0; x<m_xsize; x++)
                for (int y=0; y<m_ysize; y++)
                        m_cells[x][y]=g.m_cells[x][y];
        
        if (debug){
                std::cerr << __func__ << std::endl;
                std::cerr << "m_xsize= " << m_xsize<< std::endl;
                std::cerr << "m_ysize= " << m_ysize<< std::endl;
        }
        return *this;
}

template <class Cell, const bool debug>
Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g){
        m_xsize=g.m_xsize;
        m_ysize=g.m_ysize;
        m_cells=new Cell*[m_xsize];
        for (int x=0; x<m_xsize; x++){
                m_cells[x]=new Cell[m_ysize];
                for (int y=0; y<m_ysize; y++)
                        m_cells[x][y]=g.m_cells[x][y];
        }
        if (debug){
                std::cerr << __func__ << std::endl;
                std::cerr << "m_xsize= " << m_xsize<< std::endl;
                std::cerr << "m_ysize= " << m_ysize<< std::endl;
        }
}

template <class Cell, const bool debug>
Array2D<Cell,debug>::~Array2D(){
  if (debug){
        std::cerr << __func__ << std::endl;
        std::cerr << "m_xsize= " << m_xsize<< std::endl;
        std::cerr << "m_ysize= " << m_ysize<< std::endl;
  }
  for (int i=0; i<m_xsize; i++){
    delete [] m_cells[i];
    m_cells[i]=0;
  }
  delete [] m_cells;
  m_cells=0;
}

template <class Cell, const bool debug>
void Array2D<Cell,debug>::clear(){
  if (debug){
        std::cerr << __func__ << std::endl;
        std::cerr << "m_xsize= " << m_xsize<< std::endl;
        std::cerr << "m_ysize= " << m_ysize<< std::endl;
  }
  for (int i=0; i<m_xsize; i++){
    delete [] m_cells[i];
    m_cells[i]=0;
  }
  delete [] m_cells;
  m_cells=0;
  m_xsize=0;
  m_ysize=0;
}


template <class Cell, const bool debug>
void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
        int xsize=xmax-xmin;
        int ysize=ymax-ymin;
        Cell ** newcells=new Cell *[xsize];
        for (int x=0; x<xsize; x++){
                newcells[x]=new Cell[ysize];
        }
        int dx= xmin < 0 ? 0 : xmin;
        int dy= ymin < 0 ? 0 : ymin;
        int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
        int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
        for (int x=dx; x<Dx; x++){
                for (int y=dy; y<Dy; y++){
                        newcells[x-xmin][y-ymin]=this->m_cells[x][y];
                }
                delete [] this->m_cells[x];
        }
        delete [] this->m_cells;
        this->m_cells=newcells;
        this->m_xsize=xsize;
        this->m_ysize=ysize; 
}

template <class Cell, const bool debug>
inline bool Array2D<Cell,debug>::isInside(int x, int y) const{
        return x>=0 && y>=0 && x<m_xsize && y<m_ysize; 
}

template <class Cell, const bool debug>
inline const Cell& Array2D<Cell,debug>::cell(int x, int y) const{
        assert(isInside(x,y));
        return m_cells[x][y];
}


template <class Cell, const bool debug>
inline Cell& Array2D<Cell,debug>::cell(int x, int y){
        assert(isInside(x,y));
        return m_cells[x][y];
}

};

#endif