Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLMath/src/ICLMath/Octree.h ** 00010 ** Module : ICLMath ** 00011 ** Authors: Christof Elbrechter ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.GPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 #pragma once 00032 00033 00034 #include <algorithm> 00035 #include <set> 00036 00037 #include <ICLMath/FixedVector.h> 00038 #include <ICLUtils/VisualizationDescription.h> 00039 #include <ICLUtils/Rect32f.h> 00040 #include <ICLUtils/Range.h> 00041 #include <ICLUtils/StringUtils.h> 00042 00043 namespace icl{ 00044 00045 namespace math{ 00046 00047 00049 00058 template<class Scalar, int CAPACITY=16, int SF=1, class Pt=FixedColVector<Scalar,4>, int ALLOC_CHUNK_SIZE=1024> 00059 class Octree{ 00060 public: 00061 00063 00065 struct AABB{ 00066 Pt center; 00067 Pt halfSize; 00068 00070 AABB(){} 00071 00073 AABB(const Pt ¢er, const Pt &halfSize): 00074 center(center),halfSize(halfSize){} 00075 00077 bool contains(const Pt &p) const{ 00078 return ( p[0] >= center[0] - halfSize[0] 00079 && p[1] >= center[1] - halfSize[1] 00080 && p[2] >= center[2] - halfSize[2] 00081 && p[0] <= center[0] + halfSize[0] 00082 && p[1] <= center[1] + halfSize[1] 00083 && p[1] <= center[2] + halfSize[2]); 00084 } 00085 00087 bool intersects(const AABB &o) const{ 00088 return (fabs(center[0] - o.center[0]) < (halfSize[0] + o.halfSize[0]) 00089 && fabs(center[1] - o.center[1]) < (halfSize[1] + o.halfSize[1]) 00090 && fabs(center[2] - o.center[2]) < (halfSize[2] + o.halfSize[2])); 00091 00092 } 00093 }; 00094 00096 00098 struct Node{ 00099 AABB boundary; 00100 Pt points[CAPACITY]; 00101 Pt *next; 00102 Node *children; 00103 Node *parent; 00104 float radius; 00105 00107 Node(){} 00108 00110 Node(const AABB &boundary){ 00111 this->boundary = boundary; 00112 this->next = this->points; 00113 this->children = 0; 00114 this->parent = 0; 00115 this->radius = ::sqrt( utils::sqr(boundary.halfSize[0]) + 00116 utils::sqr(boundary.halfSize[1]) + 00117 utils::sqr(boundary.halfSize[2]) ); 00118 } 00120 00121 void init(Node *parent, const AABB &boundary){ 00122 this->boundary = boundary; 00123 this->next = this->points; 00124 this->children = 0; 00125 this->parent = parent; 00126 this->radius = parent->radius/2; 00127 } 00128 00130 00133 void query(const AABB &boundary, std::vector<Pt> &found) const{ 00134 if (!this->boundary.intersects(boundary)){ 00135 return; 00136 } 00137 for(const Pt *p=points; p<next ; ++p){ 00138 if(boundary.contains(*p)){ 00139 found.push_back(scale_down(*p)); 00140 } 00141 } 00142 if(!children) return; 00143 00144 for(int i=0;i<8;++i){ 00145 children[i].query(boundary,found); 00146 } 00147 } 00148 00150 00153 void split(Node *children){ 00154 const Pt half = boundary.halfSize/2;//*0.5; 00155 00156 const Pt &c = boundary.center; 00157 this->children = children; 00158 this->children[0].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1], c[2]-half[2]),half)); 00159 this->children[1].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1], c[2]-half[2]),half)); 00160 this->children[2].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1], c[2]-half[2]),half)); 00161 this->children[3].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1], c[2]-half[2]),half)); 00162 00163 this->children[4].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1], c[2]+half[2]),half)); 00164 this->children[5].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1], c[2]+half[2]),half)); 00165 this->children[6].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1], c[2]+half[2]),half)); 00166 this->children[7].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1], c[2]+half[2]),half)); 00167 00168 } 00169 }; 00170 00172 00174 struct Allocator{ 00175 00177 std::vector<Node*> allocated; 00178 00180 int curr; 00181 00183 Allocator(){ grow(); } 00184 00186 void grow(){ 00187 allocated.push_back(new Node[ALLOC_CHUNK_SIZE*8]); 00188 curr = 0; 00189 } 00190 00192 void clear(){ 00193 for(size_t i=1;i<allocated.size();++i){ 00194 delete [] allocated[i]; 00195 } 00196 allocated.resize(1); 00197 curr = 0; 00198 } 00199 00201 Node *next(){ 00202 if(curr == ALLOC_CHUNK_SIZE) grow(); 00203 return allocated.back()+8*curr++; 00204 } 00205 00207 ~Allocator(){ 00208 for(size_t i=0;i<allocated.size();++i){ 00209 delete [] allocated[i]; 00210 } 00211 } 00212 00214 std::vector<Pt> all() const{ 00215 std::vector<Pt> pts; 00216 for(size_t i=0;i<allocated.size()-1;++i){ 00217 const Node *ns = allocated[i]; 00218 for(size_t j=0;j<allocated.size();++j){ 00219 for(const Pt* p = ns[j].points; p != ns[j].next;++p){ 00220 pts.push_back(scale_down(*p)); 00221 } 00222 } 00223 } 00224 const Node *ns = allocated.back(); 00225 for(int i=0;i<curr*4;++i){ 00226 for(const Pt* p = ns[i].points; p != ns[i].next;++p){ 00227 pts.push_back(scale_down(*p)); 00228 } 00229 } 00230 return pts; 00231 } 00232 }; 00233 00234 protected: 00235 00237 Node *root; 00238 00240 Allocator alloc; 00241 00243 int num; 00244 00245 static inline Pt scale_up(const Pt &p){ 00246 if(SF == 1) return p; 00247 Pt tmp = p; 00248 tmp[0] *= SF; 00249 tmp[1] *= SF; 00250 tmp[2] *= SF; 00251 return tmp; 00252 } 00253 00254 static inline Pt scale_down(const Pt &p){ 00255 if(SF == 1) return p; 00256 Pt tmp = p; 00257 tmp[0] /= SF; 00258 tmp[1] /= SF; 00259 tmp[2] /= SF; 00260 return tmp; 00261 } 00262 00263 static inline Pt scale_down_1(const Pt &p){ 00264 Pt sdp = scale_down(p); 00265 sdp[3] = 1; 00266 return sdp; 00267 } 00268 00269 00270 public: 00272 Octree(const Scalar &minX, const Scalar &minY, const Scalar &minZ, 00273 const Scalar &width, const Scalar &height, const Scalar &depth):num(0){ 00274 this->root = new Node(AABB(scale_up(Pt(minX+width/2, minY+height/2, minZ+depth/2)), 00275 scale_up(Pt(width/2,height/2, depth/2)))); 00276 } 00277 00279 Octree(const Scalar &min, const Scalar &len):num(0){ 00280 this->root = new Node(AABB(scale_up(Pt(min+len/2, min+len/2, min+len/2)), 00281 scale_up(Pt(len/2,len/2, len/2)))); 00282 } 00283 00285 00286 ~Octree(){ 00287 // the root node is not part of the allocator 00288 delete root; 00289 } 00290 00291 protected: 00292 00294 const Pt &nn_approx_internal(const Pt &p, double &currMinDist, const Pt *&currNN) const throw (utils::ICLException){ 00295 // 1st find cell, that continas p 00296 const Node *n = root; 00297 while(n->children){ 00298 n = (n->children 00299 + (p[0] > n->boundary.center[0]) 00300 + 2 * (p[1] > n->boundary.center[1]) 00301 + 4 * (p[2] > n->boundary.center[2])); 00302 } 00303 00304 // this cell could be empty, in this case, the parent must contain good points 00305 if(n->next == n->points){ 00306 n = n->parent; 00307 if(!n) throw utils::ICLException("no nn found for given point " + utils::str(p)); 00308 } 00309 00310 double sqrMinDist = utils::sqr(currMinDist); 00311 00312 for(const Pt *x=n->points; x < n->next; ++x){ 00313 Scalar dSqr = ( utils::sqr(x->operator[](0)-p[0]) 00314 + utils::sqr(x->operator[](1)-p[1]) 00315 + utils::sqr(x->operator[](2)-p[2]) ); 00316 if(dSqr < sqrMinDist){ 00317 sqrMinDist = dSqr; 00318 currNN = x; 00319 } 00320 } 00321 currMinDist = sqrt(sqrMinDist); 00322 00323 if(!currNN){ 00324 throw utils::ICLException("no nn found for given point " + utils::str(p.transp())); 00325 } 00326 return *currNN; 00327 } 00328 public: 00329 00331 const AABB &getRootAABB() const { return root->boundary; } 00332 00334 00343 Pt nn_approx(const Pt &p) const throw (utils::ICLException){ 00344 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00345 const Pt *currNN = 0; 00346 nn_approx_internal(scale_up(p),currMinDist,currNN); 00347 return scale_down_1(*currNN); 00348 } 00349 00351 00369 Pt nn(const Pt &pIn) const throw (utils::ICLException){ 00370 const Pt p = scale_up(pIn); 00371 std::vector<const Node*> stack; 00372 stack.reserve(128); 00373 stack.push_back(root); 00374 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00375 const Pt *currNN = 0; 00376 00377 nn_approx_internal(p,currMinDist,currNN); 00378 00379 while(stack.size()){ 00380 const Node *n = stack.back(); 00381 stack.pop_back(); 00382 if(n->children){ 00383 const AABB b(p, Pt(currMinDist,currMinDist,currMinDist)); 00384 for(int i=0;i<8;++i){ 00385 if(b.intersects(n->children[i].boundary)) stack.push_back(n->children+i); 00386 } 00387 } 00388 Scalar sqrMinDist = utils::sqr(currMinDist); 00389 00390 for(const Pt *x=n->points; x < n->next; ++x){ 00391 Scalar dSqr = (utils::sqr(x->operator[](0)-p[0]) + 00392 utils::sqr(x->operator[](1)-p[1]) + 00393 utils::sqr(x->operator[](2)-p[2])); 00394 if(dSqr < sqrMinDist){ 00395 sqrMinDist = dSqr; 00396 currNN = x; 00397 } 00398 } 00399 currMinDist = sqrt(sqrMinDist); 00400 } 00401 return scale_down_1(*currNN); 00402 } 00403 00405 00408 template<class OtherVectorType> 00409 void insert(const OtherVectorType &pIn){ 00410 ++num; 00411 Pt p = pIn; 00412 p[0] *= SF; 00413 p[1] *= SF; 00414 p[2] *= SF; 00415 00416 Node *n = root; 00417 while(true){ 00418 if(n->next != n->points+CAPACITY){ 00419 *n->next++ = p; 00420 return; 00421 } 00422 if(!n->children) n->split(alloc.next()); 00423 n = (n->children 00424 + (p[0] > n->boundary.center[0]) 00425 + 2 * (p[1] > n->boundary.center[1]) 00426 + 4 * (p[2] > n->boundary.center[2])); 00427 } 00428 } 00429 00431 std::vector<Pt> query(const Scalar &minX, const Scalar &minY, const Scalar &minZ, 00432 const Scalar &width, const Scalar &height, const Scalar &depth) const{ 00433 AABB range(scale_up(Pt(minX+width/2, minY+height/2, minZ+depth/2)), 00434 scale_up(Pt(width/2,height/2, depth/2))); 00435 std::vector<Pt> found; 00436 root->query(range,found); 00437 return found; 00438 } 00439 00441 std::vector<Pt> queryAll() const { 00442 return alloc.all(); 00443 } 00444 00445 /* 00447 utils::VisualizationDescription vis() const{ 00448 utils::VisualizationDescription d; 00449 d.color(0,100,255,255); 00450 d.fill(0,0,0,0); 00451 root->vis(d); 00452 return d; 00453 } 00454 */ 00455 00457 00458 void clear(){ 00459 root->next = root->points; 00460 root->children = 0; 00461 alloc.clear(); 00462 num = 0; 00463 } 00464 00466 int size() const { 00467 return num; 00468 } 00469 00470 /* 00472 void printStructure(){ 00473 std::cout << "QuadTree{" << std::endl; 00474 root->printStructure(1); 00475 std::cout << "}" << std::endl; 00476 } 00477 */ 00478 }; 00479 00480 } // namespace math 00481 } // namespace icl 00482