00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef LSL_LAZZY_GRID_HH
00036 #define LSL_LAZZY_GRID_HH
00037
00038 #include <spatial_index.h>
00039 #include <ndt_cell.h>
00040
00041
00042
00043 namespace lslgeneric
00044 {
00045
00049 template <typename PointT>
00050 class LazyGrid : public SpatialIndex<PointT>
00051 {
00052 public:
00053 LazyGrid(double cellSize);
00054 LazyGrid(LazyGrid *prot);
00055 LazyGrid(double sizeXmeters, double sizeYmeters, double sizeZmeters,
00056 double cellSizeX, double cellSizeY, double cellSizeZ,
00057 double _centerX, double _centerY, double _centerZ,
00058 Cell<PointT> *cellPrototype );
00059 virtual ~LazyGrid();
00060
00061 virtual Cell<PointT>* getCellForPoint(const PointT &point);
00062 virtual Cell<PointT>* addPoint(const PointT &point);
00063
00064
00066 virtual typename SpatialIndex<PointT>::CellVectorItr begin();
00068 virtual typename SpatialIndex<PointT>::CellVectorItr end();
00069 virtual int size();
00070
00072 virtual SpatialIndex<PointT>* clone() const;
00073 virtual SpatialIndex<PointT>* copy() const;
00074
00076 virtual void getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells);
00077
00079 virtual void setCellType(Cell<PointT> *type);
00080
00081 virtual void setCenter(const double &cx, const double &cy, const double &cz);
00082 virtual void setSize(const double &sx, const double &sy, const double &sz);
00083
00084 virtual NDTCell<PointT>* getClosestNDTCell(const PointT &pt, bool checkForGaussian=true);
00085 virtual std::vector<NDTCell<PointT>*> getClosestNDTCells(const PointT &pt, int &n_neigh, bool checkForGaussian=true);
00086 virtual std::vector<NDTCell<PointT>*> getClosestCells(const PointT &pt);
00087
00088 virtual inline void getCellAt(int indX, int indY, int indZ, Cell<PointT>* &cell){
00089 if(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0){
00090 cell = dataArray[indX][indY][indZ];
00091 }else{
00092 cell = NULL;
00093
00094 }
00095 }
00096 virtual inline void getCellAt(const PointT& pt, Cell<PointT>* &cell){
00097 int indX,indY,indZ;
00098 this->getIndexForPoint(pt,indX,indY,indZ);
00099 this->getCellAt(indX,indY,indZ,cell);
00100 }
00101 virtual inline void getNDTCellAt(int indX, int indY, int indZ, NDTCell<PointT>* &cell){
00102 if(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0){
00103 cell = dynamic_cast<NDTCell<PointT>*> (dataArray[indX][indY][indZ]);
00104 }else{
00105 cell = NULL;
00106 }
00107 }
00108 virtual inline void getNDTCellAt(const PointT& pt, NDTCell<PointT>* &cell){
00109 int indX,indY,indZ;
00110 this->getIndexForPoint(pt,indX,indY,indZ);
00111 this->getNDTCellAt(indX,indY,indZ,cell);
00112 }
00113
00114
00115 void getCellSize(double &cx, double &cy, double &cz);
00116 void getGridSize(int &cx, int &cy, int &cz);
00117 void getGridSizeInMeters(double &cx, double &cy, double &cz);
00118 void getCenter(double &cx, double &cy, double &cz);
00119 virtual inline void getIndexForPoint(const PointT& pt, int &idx, int &idy, int &idz);
00120 Cell<PointT> * getProtoType()
00121 {
00122 return protoType;
00123 }
00124
00125
00126 virtual void initialize();
00127 virtual void initializeAll() ;
00128
00129 Cell<PointT> ****getDataArrayPtr()
00130 {
00131 return dataArray;
00132 }
00133
00135 virtual int loadFromJFF(FILE * jffin);
00136 inline bool traceLine(const Eigen::Vector3d &origin, const PointT &endpoint, const Eigen::Vector3d &diff, const double& maxz, std::vector<NDTCell<PointT>*> &cells);
00137 inline bool traceLineWithEndpoint(const Eigen::Vector3d &origin, const PointT &endpoint, const Eigen::Vector3d &diff, const double& maxz, std::vector<NDTCell<PointT>*> &cells, Eigen::Vector3d &final_point);
00138 bool isInside(const PointT& pt) {
00139 int indX,indY,indZ;
00140 this->getIndexForPoint(pt,indX,indY,indZ);
00141 return(indX < sizeX && indY < sizeY && indZ < sizeZ && indX >=0 && indY >=0 && indZ >=0);
00142 }
00143 protected:
00144 bool initialized;
00145 Cell<PointT> ****dataArray;
00146
00147 Cell<PointT> *protoType;
00148 std::vector<Cell<PointT>*> activeCells;
00149
00150 bool centerIsSet, sizeIsSet;
00151
00152
00153 double sizeXmeters, sizeYmeters, sizeZmeters;
00154 double cellSizeX, cellSizeY, cellSizeZ;
00155 double centerX, centerY, centerZ;
00156 int sizeX,sizeY,sizeZ;
00157
00158 virtual bool checkCellforNDT(int indX, int indY, int indZ, bool checkForGaussian=true);
00159
00160 };
00161
00162
00163 };
00164 #include<impl/lazy_grid.hpp>
00165
00166 #endif