add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,9 @@
|
||||
#ifndef ACCESSTATE_H
|
||||
#define ACCESSTATE_H
|
||||
|
||||
namespace GMapping {
|
||||
enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2};
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,188 @@
|
||||
#ifndef ARRAY2D_H
|
||||
#define ARRAY2D_H
|
||||
|
||||
#include <assert.h>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include "accessstate.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#ifndef __PRETTY_FUNCTION__
|
||||
#define __FUNCDNAME__
|
||||
#endif
|
||||
|
||||
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 << __PRETTY_FUNCTION__ << 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 << __PRETTY_FUNCTION__ << 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 << __PRETTY_FUNCTION__ << 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 << __PRETTY_FUNCTION__ << 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 << __PRETTY_FUNCTION__ << 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
|
||||
|
||||
@@ -0,0 +1,200 @@
|
||||
#ifndef HARRAY2D_H
|
||||
#define HARRAY2D_H
|
||||
#include <set>
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <gmapping/utils/autoptr.h>
|
||||
#include "array2d.h"
|
||||
|
||||
namespace GMapping {
|
||||
|
||||
template <class Cell>
|
||||
class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
|
||||
public:
|
||||
typedef std::set< point<int>, pointcomparator<int> > PointSet;
|
||||
HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
|
||||
HierarchicalArray2D(const HierarchicalArray2D& hg);
|
||||
HierarchicalArray2D& operator=(const HierarchicalArray2D& hg);
|
||||
virtual ~HierarchicalArray2D(){}
|
||||
void resize(int ixmin, int iymin, int ixmax, int iymax);
|
||||
inline int getPatchSize() const {return m_patchMagnitude;}
|
||||
inline int getPatchMagnitude() const {return m_patchMagnitude;}
|
||||
|
||||
inline const Cell& cell(int x, int y) const;
|
||||
inline Cell& cell(int x, int y);
|
||||
inline bool isAllocated(int x, int y) const;
|
||||
inline AccessibilityState cellState(int x, int y) const ;
|
||||
inline IntPoint patchIndexes(int x, int y) const;
|
||||
|
||||
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 bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);}
|
||||
inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); }
|
||||
inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);}
|
||||
|
||||
inline void setActiveArea(const PointSet&, bool patchCoords=false);
|
||||
const PointSet& getActiveArea() const {return m_activeArea; }
|
||||
inline void allocActiveArea();
|
||||
protected:
|
||||
virtual Array2D<Cell> * createPatch(const IntPoint& p) const;
|
||||
PointSet m_activeArea;
|
||||
int m_patchMagnitude;
|
||||
int m_patchSize;
|
||||
};
|
||||
|
||||
template <class Cell>
|
||||
HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude)
|
||||
:Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
|
||||
m_patchMagnitude=patchMagnitude;
|
||||
m_patchSize=1<<m_patchMagnitude;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
|
||||
: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
|
||||
{
|
||||
this->m_xsize=hg.m_xsize;
|
||||
this->m_ysize=hg.m_ysize;
|
||||
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
|
||||
for (int x=0; x<this->m_xsize; x++){
|
||||
this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
|
||||
for (int y=0; y<this->m_ysize; y++)
|
||||
this->m_cells[x][y]=hg.m_cells[x][y];
|
||||
}
|
||||
this->m_patchMagnitude=hg.m_patchMagnitude;
|
||||
this->m_patchSize=hg.m_patchSize;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
|
||||
int xsize=xmax-xmin;
|
||||
int ysize=ymax-ymin;
|
||||
autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
|
||||
for (int x=0; x<xsize; x++){
|
||||
newcells[x]=new autoptr< Array2D<Cell> >[ysize];
|
||||
for (int y=0; y<ysize; y++){
|
||||
newcells[x][y]=autoptr< Array2D<Cell> >(0);
|
||||
}
|
||||
}
|
||||
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>
|
||||
HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
|
||||
// Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
|
||||
if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
|
||||
for (int i=0; i<this->m_xsize; i++)
|
||||
delete [] this->m_cells[i];
|
||||
delete [] this->m_cells;
|
||||
this->m_xsize=hg.m_xsize;
|
||||
this->m_ysize=hg.m_ysize;
|
||||
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
|
||||
for (int i=0; i<this->m_xsize; i++)
|
||||
this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
|
||||
}
|
||||
for (int x=0; x<this->m_xsize; x++)
|
||||
for (int y=0; y<this->m_ysize; y++)
|
||||
this->m_cells[x][y]=hg.m_cells[x][y];
|
||||
|
||||
m_activeArea.clear();
|
||||
m_patchMagnitude=hg.m_patchMagnitude;
|
||||
m_patchSize=hg.m_patchSize;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::setActiveArea(const typename HierarchicalArray2D<Cell>::PointSet& aa, bool patchCoords){
|
||||
m_activeArea.clear();
|
||||
for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) {
|
||||
IntPoint p;
|
||||
if (patchCoords)
|
||||
p=*it;
|
||||
else
|
||||
p=patchIndexes(*it);
|
||||
m_activeArea.insert(p);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
|
||||
return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude);
|
||||
}
|
||||
|
||||
|
||||
template <class Cell>
|
||||
AccessibilityState HierarchicalArray2D<Cell>::cellState(int x, int y) const {
|
||||
if (this->isInside(patchIndexes(x,y))) {
|
||||
if(isAllocated(x,y))
|
||||
return (AccessibilityState)((int)Inside|(int)Allocated);
|
||||
else
|
||||
return Inside;
|
||||
}
|
||||
return Outside;
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
void HierarchicalArray2D<Cell>::allocActiveArea(){
|
||||
for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){
|
||||
const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
|
||||
Array2D<Cell>* patch=0;
|
||||
if (!ptr){
|
||||
patch=createPatch(*it);
|
||||
} else{
|
||||
patch=new Array2D<Cell>(*ptr);
|
||||
}
|
||||
this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (ptr != 0);
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
|
||||
if (x>=0 && y>=0)
|
||||
return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
|
||||
return IntPoint(-1, -1);
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
assert(this->isInside(c.x, c.y));
|
||||
if (!this->m_cells[c.x][c.y]){
|
||||
Array2D<Cell>* patch=createPatch(IntPoint(x,y));
|
||||
this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
|
||||
//cerr << "!!! FATAL: your dick is going to fall down" << endl;
|
||||
}
|
||||
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
|
||||
}
|
||||
|
||||
template <class Cell>
|
||||
const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
|
||||
assert(isAllocated(x,y));
|
||||
IntPoint c=patchIndexes(x,y);
|
||||
const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
|
||||
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,265 @@
|
||||
#ifndef MAP_H
|
||||
#define MAP_H
|
||||
#include <gmapping/utils/point.h>
|
||||
#include <assert.h>
|
||||
#include "accessstate.h"
|
||||
#include "array2d.h"
|
||||
|
||||
namespace GMapping {
|
||||
/**
|
||||
The cells have to define the special value Cell::Unknown to handle with the unallocated areas.
|
||||
The cells have to define (int) constructor;
|
||||
*/
|
||||
typedef Array2D<double> DoubleArray2D;
|
||||
|
||||
template <class Cell, class Storage, const bool isClass=true>
|
||||
class Map{
|
||||
public:
|
||||
Map(int mapSizeX, int mapSizeY, double delta);
|
||||
Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
|
||||
Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
|
||||
/* the standard implementation works filen in this case*/
|
||||
//Map(const Map& g);
|
||||
//Map& operator =(const Map& g);
|
||||
void resize(double xmin, double ymin, double xmax, double ymax);
|
||||
void grow(double xmin, double ymin, double xmax, double ymax);
|
||||
inline IntPoint world2map(const Point& p) const;
|
||||
inline Point map2world(const IntPoint& p) const;
|
||||
inline IntPoint world2map(double x, double y) const
|
||||
{ return world2map(Point(x,y)); }
|
||||
inline Point map2world(int x, int y) const
|
||||
{ return map2world(IntPoint(x,y)); }
|
||||
|
||||
inline Point getCenter() const {return m_center;}
|
||||
inline double getWorldSizeX() const {return m_worldSizeX;}
|
||||
inline double getWorldSizeY() const {return m_worldSizeY;}
|
||||
inline int getMapSizeX() const {return m_mapSizeX;}
|
||||
inline int getMapSizeY() const {return m_mapSizeY;}
|
||||
inline double getDelta() const { return m_delta;}
|
||||
inline double getMapResolution() const { return m_delta;}
|
||||
inline double getResolution() const { return m_delta;}
|
||||
inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
|
||||
Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1));
|
||||
xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y;
|
||||
}
|
||||
|
||||
inline Cell& cell(int x, int y) {
|
||||
return cell(IntPoint(x, y));
|
||||
}
|
||||
inline Cell& cell(const IntPoint& p);
|
||||
|
||||
inline const Cell& cell(int x, int y) const {
|
||||
return cell(IntPoint(x, y));
|
||||
}
|
||||
inline const Cell& cell(const IntPoint& p) const;
|
||||
|
||||
inline Cell& cell(double x, double y) {
|
||||
return cell(Point(x, y));
|
||||
}
|
||||
inline Cell& cell(const Point& p);
|
||||
|
||||
inline const Cell& cell(double x, double y) const {
|
||||
return cell(Point(x, y));
|
||||
}
|
||||
|
||||
inline bool isInside(int x, int y) const {
|
||||
return m_storage.cellState(IntPoint(x,y))&Inside;
|
||||
}
|
||||
inline bool isInside(const IntPoint& p) const {
|
||||
return m_storage.cellState(p)&Inside;
|
||||
}
|
||||
|
||||
inline bool isInside(double x, double y) const {
|
||||
return m_storage.cellState(world2map(x,y))&Inside;
|
||||
}
|
||||
inline bool isInside(const Point& p) const {
|
||||
return m_storage.cellState(world2map(p))&Inside;
|
||||
}
|
||||
|
||||
inline const Cell& cell(const Point& p) const;
|
||||
|
||||
inline Storage& storage() { return m_storage; }
|
||||
inline const Storage& storage() const { return m_storage; }
|
||||
DoubleArray2D* toDoubleArray() const;
|
||||
Map<double, DoubleArray2D, false>* toDoubleMap() const;
|
||||
|
||||
protected:
|
||||
Point m_center;
|
||||
double m_worldSizeX, m_worldSizeY, m_delta;
|
||||
Storage m_storage;
|
||||
int m_mapSizeX, m_mapSizeY;
|
||||
int m_sizeX2, m_sizeY2;
|
||||
static const Cell m_unknown;
|
||||
};
|
||||
|
||||
typedef Map<double, DoubleArray2D, false> DoubleMap;
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
|
||||
m_storage(mapSizeX, mapSizeY){
|
||||
m_worldSizeX=mapSizeX * delta;
|
||||
m_worldSizeY=mapSizeY * delta;
|
||||
m_delta=delta;
|
||||
m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
|
||||
m_sizeX2=m_mapSizeX>>1;
|
||||
m_sizeY2=m_mapSizeY>>1;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
|
||||
m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
|
||||
m_center=center;
|
||||
m_worldSizeX=worldSizeX;
|
||||
m_worldSizeY=worldSizeY;
|
||||
m_delta=delta;
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_sizeX2=m_mapSizeX>>1;
|
||||
m_sizeY2=m_mapSizeY>>1;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
|
||||
m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
|
||||
m_center=center;
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_delta=delta;
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
|
||||
m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
|
||||
IntPoint imin=world2map(xmin, ymin);
|
||||
IntPoint imax=world2map(xmax, ymax);
|
||||
int pxmin, pymin, pxmax, pymax;
|
||||
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
|
||||
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
|
||||
m_storage.resize(pxmin, pymin, pxmax, pymax);
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
|
||||
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
|
||||
IntPoint imin=world2map(xmin, ymin);
|
||||
IntPoint imax=world2map(xmax, ymax);
|
||||
if (isInside(imin) && isInside(imax))
|
||||
return;
|
||||
imin=min(imin, IntPoint(0,0));
|
||||
imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
|
||||
int pxmin, pymin, pxmax, pymax;
|
||||
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
|
||||
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
|
||||
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
|
||||
m_storage.resize(pxmin, pymin, pxmax, pymax);
|
||||
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
|
||||
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
|
||||
m_worldSizeX=xmax-xmin;
|
||||
m_worldSizeY=ymax-ymin;
|
||||
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
|
||||
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
|
||||
return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
|
||||
return Point( (p.x-m_sizeX2)*m_delta,
|
||||
(p.y-m_sizeY2)*m_delta)+m_center;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
|
||||
AccessibilityState s=m_storage.cellState(p);
|
||||
if (! (s&Inside))
|
||||
assert(0);
|
||||
//if (s&Allocated) return m_storage.cell(p); assert(0);
|
||||
|
||||
// this will never happend. Just to satify the compiler..
|
||||
return m_storage.cell(p);
|
||||
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
|
||||
IntPoint ip=world2map(p);
|
||||
AccessibilityState s=m_storage.cellState(ip);
|
||||
if (! (s&Inside))
|
||||
assert(0);
|
||||
//if (s&Allocated) return m_storage.cell(ip); assert(0);
|
||||
|
||||
// this will never happend. Just to satify the compiler..
|
||||
return m_storage.cell(ip);
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
|
||||
AccessibilityState s=m_storage.cellState(p);
|
||||
//if (! s&Inside) assert(0);
|
||||
if (s&Allocated)
|
||||
return m_storage.cell(p);
|
||||
return m_unknown;
|
||||
}
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
|
||||
IntPoint ip=world2map(p);
|
||||
AccessibilityState s=m_storage.cellState(ip);
|
||||
//if (! s&Inside) assert(0);
|
||||
if (s&Allocated)
|
||||
return m_storage.cell(ip);
|
||||
return m_unknown;
|
||||
}
|
||||
|
||||
|
||||
//FIXME check why the last line of the map is corrupted.
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
|
||||
DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
|
||||
for(int x=0; x<getMapSizeX()-1; x++)
|
||||
for(int y=0; y<getMapSizeY()-1; y++){
|
||||
IntPoint p(x,y);
|
||||
darr->cell(p)=cell(p);
|
||||
}
|
||||
return darr;
|
||||
}
|
||||
|
||||
|
||||
template <class Cell, class Storage, const bool isClass>
|
||||
Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
|
||||
//FIXME size the map so that m_center will be setted accordingly
|
||||
Point pmin=map2world(IntPoint(0,0));
|
||||
Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
|
||||
Point center=(pmax+pmin)*0.5;
|
||||
Map<double, DoubleArray2D, false>* plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
|
||||
for(int x=0; x<getMapSizeX()-1; x++)
|
||||
for(int y=0; y<getMapSizeY()-1; y++){
|
||||
IntPoint p(x,y);
|
||||
plainMap->cell(p)=cell(p);
|
||||
}
|
||||
return plainMap;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user