Go to the documentation of this file.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 OCT_TREE_HH
00036 #define OCT_TREE_HH
00037
00038 #include <spatial_index.h>
00039 #include <ndt_cell.h>
00040 #include <vector>
00041 #include <cstdio>
00042 #include <Eigen/Core>
00043
00044 namespace lslgeneric
00045 {
00046
00047 template <typename PointT>
00048 class AdaptiveOctTree;
00056 template <typename PointT>
00057 class OctTree : public SpatialIndex<PointT>
00058 {
00059 protected:
00060 OctTree* parent_;
00061 OctTree *children_[8];
00062 NDTCell<PointT> *myCell_;
00063
00064 std::vector< Cell<PointT>* > myLeafs_;
00065 unsigned int depth_;
00066 bool leaf_;
00067 bool leafsCached_;
00068
00070 virtual size_t getIndexForPoint(const PointT &pt) const;
00071
00073 virtual void computeLeafCells();
00074
00075
00076 public:
00077
00079 int MAX_DEPTH;
00081 int MAX_POINTS;
00083 double SMALL_CELL_SIZE;
00085 double BIG_CELL_SIZE;
00086 bool parametersSet_;
00087
00089 OctTree();
00091 OctTree(PointT center, double xsize, double ysize,
00092 double zsize, NDTCell<PointT>* type, OctTree<PointT> *_parent=NULL, unsigned int _depth=0);
00093 virtual ~OctTree();
00094
00096 void setParameters(double _BIG_CELL_SIZE =4,
00097 double _SMALL_CELL_SIZE =0.5 ,
00098 int _MAX_POINTS =10,
00099 int _MAX_DEPTH =20
00100 );
00101
00103 virtual Cell<PointT>* addPoint(const PointT &point);
00104
00106 virtual Cell<PointT>* getCellForPoint(const PointT &point);
00107 inline virtual Cell<PointT>* getMyCell()
00108 {
00109 return myCell_;
00110 }
00111 virtual OctTree<PointT>* getLeafForPoint(const PointT &point);
00112
00114 virtual void setCellType(Cell<PointT> *type);
00115
00117 virtual typename SpatialIndex<PointT>::CellVectorItr begin();
00119 virtual typename SpatialIndex<PointT>::CellVectorItr end();
00120
00122 void print();
00123
00125 void writeToVRML(const char* filename);
00126 void writeToVRML(FILE* fout);
00127
00129 inline OctTree<PointT>* getChild(int idx)
00130 {
00131 if(idx<8 && idx>=0)
00132 {
00133 return children_[idx];
00134 }
00135 return NULL;
00136 }
00137 inline bool isLeaf()
00138 {
00139 return leaf_;
00140 }
00141
00143 virtual SpatialIndex<PointT>* clone() const;
00144 virtual SpatialIndex<PointT>* copy() const;
00145
00146 virtual void setCenter(const double &cx, const double &cy, const double &cz);
00147 virtual void setSize(const double &sx, const double &sy, const double &sz);
00148
00149 virtual void getNeighbors(const PointT &point, const double &radius, std::vector<Cell<PointT>*> &cells);
00150 virtual bool intersectSphere(const PointT center, const double &radius) const;
00151
00152 virtual Cell<PointT>* getClosestLeafCell(const PointT &pt);
00153 virtual NDTCell<PointT>* getClosestNDTCell(const PointT &pt);
00154
00155 friend class AdaptiveOctTree<PointT>;
00156 public:
00157 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00158 };
00159
00160 }
00161
00162 #include<impl/oc_tree.hpp>
00163
00164 #endif