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.LGPL ** 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 <ICLUtils/CompatMacros.h> 00035 #include <ICLUtils/VisualizationDescription.h> 00036 #include <ICLUtils/Rect32f.h> 00037 #include <ICLUtils/Range.h> 00038 #include <ICLUtils/StringUtils.h> 00039 #include <ICLMath/FixedVector.h> 00040 00041 #include <algorithm> 00042 #include <set> 00043 00044 namespace icl{ 00045 00046 namespace math{ 00047 00048 00050 00059 template<class Scalar, int CAPACITY=16, int SF=1, class Pt=FixedColVector<Scalar,4>, int ALLOC_CHUNK_SIZE=1024> 00060 class Octree{ 00061 public: 00062 00064 00066 struct AABB{ 00067 Pt center; 00068 Pt halfSize; 00069 00071 AABB(){} 00072 00074 AABB(const Pt ¢er, const Pt &halfSize): 00075 center(center),halfSize(halfSize){} 00076 00078 bool contains(const Pt &p) const{ 00079 return ( p[0] >= center[0] - halfSize[0] 00080 && p[1] >= center[1] - halfSize[1] 00081 && p[2] >= center[2] - halfSize[2] 00082 && p[0] <= center[0] + halfSize[0] 00083 && p[1] <= center[1] + halfSize[1] 00084 && p[1] <= center[2] + halfSize[2]); 00085 } 00086 00088 bool intersects(const AABB &o) const{ 00089 return (fabs(center[0] - o.center[0]) < (halfSize[0] + o.halfSize[0]) 00090 && fabs(center[1] - o.center[1]) < (halfSize[1] + o.halfSize[1]) 00091 && fabs(center[2] - o.center[2]) < (halfSize[2] + o.halfSize[2])); 00092 00093 } 00094 }; 00095 00097 00099 struct Node{ 00100 AABB boundary; 00101 Pt points[CAPACITY]; 00102 Pt *next; 00103 Node *children; 00104 Node *parent; 00105 float radius; 00106 00108 Node(){} 00109 00111 Node(const AABB &boundary){ 00112 this->boundary = boundary; 00113 this->next = this->points; 00114 this->children = 0; 00115 this->parent = 0; 00116 this->radius = ::sqrt( utils::sqr(boundary.halfSize[0]) + 00117 utils::sqr(boundary.halfSize[1]) + 00118 utils::sqr(boundary.halfSize[2]) ); 00119 } 00121 00122 void init(Node *parent, const AABB &boundary){ 00123 this->boundary = boundary; 00124 this->next = this->points; 00125 this->children = 0; 00126 this->parent = parent; 00127 this->radius = parent->radius/2; 00128 } 00129 00131 00134 void query(const AABB &boundary, std::vector<Pt> &found) const{ 00135 if (!this->boundary.intersects(boundary)){ 00136 return; 00137 } 00138 for(const Pt *p=points; p<next ; ++p){ 00139 if(boundary.contains(*p)){ 00140 found.push_back(scale_down(*p)); 00141 } 00142 } 00143 if(!children) return; 00144 00145 for(int i=0;i<8;++i){ 00146 children[i].query(boundary,found); 00147 } 00148 } 00149 00151 00154 void split(Node *children){ 00155 const Pt half = boundary.halfSize/2;//*0.5; 00156 00157 const Pt &c = boundary.center; 00158 this->children = children; 00159 this->children[0].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1], c[2]-half[2]),half)); 00160 this->children[1].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1], c[2]-half[2]),half)); 00161 this->children[2].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1], c[2]-half[2]),half)); 00162 this->children[3].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1], c[2]-half[2]),half)); 00163 00164 this->children[4].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1], c[2]+half[2]),half)); 00165 this->children[5].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1], c[2]+half[2]),half)); 00166 this->children[6].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1], c[2]+half[2]),half)); 00167 this->children[7].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1], c[2]+half[2]),half)); 00168 00169 } 00170 }; 00171 00173 00175 struct Allocator{ 00176 00178 std::vector<Node*> allocated; 00179 00181 int curr; 00182 00184 Allocator(){ grow(); } 00185 00187 void grow(){ 00188 allocated.push_back(new Node[ALLOC_CHUNK_SIZE*8]); 00189 curr = 0; 00190 } 00191 00193 void clear(){ 00194 for(size_t i=1;i<allocated.size();++i){ 00195 delete [] allocated[i]; 00196 } 00197 allocated.resize(1); 00198 curr = 0; 00199 } 00200 00202 Node *next(){ 00203 if(curr == ALLOC_CHUNK_SIZE) grow(); 00204 return allocated.back()+8*curr++; 00205 } 00206 00208 ~Allocator(){ 00209 for(size_t i=0;i<allocated.size();++i){ 00210 delete [] allocated[i]; 00211 } 00212 } 00213 00215 std::vector<Pt> all() const{ 00216 std::vector<Pt> pts; 00217 for(size_t i=0;i<allocated.size()-1;++i){ 00218 const Node *ns = allocated[i]; 00219 for(size_t j=0;j<allocated.size();++j){ 00220 for(const Pt* p = ns[j].points; p != ns[j].next;++p){ 00221 pts.push_back(scale_down(*p)); 00222 } 00223 } 00224 } 00225 const Node *ns = allocated.back(); 00226 for(int i=0;i<curr*4;++i){ 00227 for(const Pt* p = ns[i].points; p != ns[i].next;++p){ 00228 pts.push_back(scale_down(*p)); 00229 } 00230 } 00231 return pts; 00232 } 00233 }; 00234 00235 protected: 00236 00238 Node *root; 00239 00241 Allocator alloc; 00242 00244 int num; 00245 00246 static inline Pt scale_up(const Pt &p){ 00247 if(SF == 1) return p; 00248 Pt tmp = p; 00249 tmp[0] *= SF; 00250 tmp[1] *= SF; 00251 tmp[2] *= SF; 00252 return tmp; 00253 } 00254 00255 static inline Pt scale_down(const Pt &p){ 00256 if(SF == 1) return p; 00257 Pt tmp = p; 00258 tmp[0] /= SF; 00259 tmp[1] /= SF; 00260 tmp[2] /= SF; 00261 return tmp; 00262 } 00263 00264 static inline Pt scale_down_1(const Pt &p){ 00265 Pt sdp = scale_down(p); 00266 sdp[3] = 1; 00267 return sdp; 00268 } 00269 00270 00271 public: 00273 Octree(const Scalar &minX, const Scalar &minY, const Scalar &minZ, 00274 const Scalar &width, const Scalar &height, const Scalar &depth):num(0){ 00275 this->root = new Node(AABB(scale_up(Pt(minX+width/2, minY+height/2, minZ+depth/2)), 00276 scale_up(Pt(width/2,height/2, depth/2)))); 00277 } 00278 00280 Octree(const Scalar &min, const Scalar &len):num(0){ 00281 this->root = new Node(AABB(scale_up(Pt(min+len/2, min+len/2, min+len/2)), 00282 scale_up(Pt(len/2,len/2, len/2)))); 00283 } 00284 00286 00287 ~Octree(){ 00288 // the root node is not part of the allocator 00289 delete root; 00290 } 00291 00292 protected: 00293 00295 const Pt &nn_approx_internal(const Pt &p, double &currMinDist, const Pt *&currNN) const throw (utils::ICLException){ 00296 // 1st find cell, that continas p 00297 const Node *n = root; 00298 while(n->children){ 00299 n = (n->children 00300 + (p[0] > n->boundary.center[0]) 00301 + 2 * (p[1] > n->boundary.center[1]) 00302 + 4 * (p[2] > n->boundary.center[2])); 00303 } 00304 00305 // this cell could be empty, in this case, the parent must contain good points 00306 if(n->next == n->points){ 00307 n = n->parent; 00308 if(!n) throw utils::ICLException("no nn found for given point " + utils::str(p)); 00309 } 00310 00311 double sqrMinDist = utils::sqr(currMinDist); 00312 00313 for(const Pt *x=n->points; x < n->next; ++x){ 00314 Scalar dSqr = ( utils::sqr(x->operator[](0)-p[0]) 00315 + utils::sqr(x->operator[](1)-p[1]) 00316 + utils::sqr(x->operator[](2)-p[2]) ); 00317 if(dSqr < sqrMinDist){ 00318 sqrMinDist = dSqr; 00319 currNN = x; 00320 } 00321 } 00322 currMinDist = sqrt(sqrMinDist); 00323 00324 if(!currNN){ 00325 throw utils::ICLException("no nn found for given point " + utils::str(p.transp())); 00326 } 00327 return *currNN; 00328 } 00329 public: 00330 00332 const AABB &getRootAABB() const { return root->boundary; } 00333 00335 00344 Pt nn_approx(const Pt &p) const throw (utils::ICLException){ 00345 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00346 const Pt *currNN = 0; 00347 nn_approx_internal(scale_up(p),currMinDist,currNN); 00348 return scale_down_1(*currNN); 00349 } 00350 00352 00370 Pt nn(const Pt &pIn) const throw (utils::ICLException){ 00371 const Pt p = scale_up(pIn); 00372 std::vector<const Node*> stack; 00373 stack.reserve(128); 00374 stack.push_back(root); 00375 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00376 const Pt *currNN = 0; 00377 00378 nn_approx_internal(p,currMinDist,currNN); 00379 00380 while(stack.size()){ 00381 const Node *n = stack.back(); 00382 stack.pop_back(); 00383 if(n->children){ 00384 const AABB b(p, Pt(currMinDist,currMinDist,currMinDist)); 00385 for(int i=0;i<8;++i){ 00386 if(b.intersects(n->children[i].boundary)) stack.push_back(n->children+i); 00387 } 00388 } 00389 Scalar sqrMinDist = utils::sqr(currMinDist); 00390 00391 for(const Pt *x=n->points; x < n->next; ++x){ 00392 Scalar dSqr = (utils::sqr(x->operator[](0)-p[0]) + 00393 utils::sqr(x->operator[](1)-p[1]) + 00394 utils::sqr(x->operator[](2)-p[2])); 00395 if(dSqr < sqrMinDist){ 00396 sqrMinDist = dSqr; 00397 currNN = x; 00398 } 00399 } 00400 currMinDist = sqrt(sqrMinDist); 00401 } 00402 return scale_down_1(*currNN); 00403 } 00404 00406 00409 template<class OtherVectorType> 00410 void insert(const OtherVectorType &pIn){ 00411 ++num; 00412 Pt p = pIn; 00413 p[0] *= SF; 00414 p[1] *= SF; 00415 p[2] *= SF; 00416 00417 Node *n = root; 00418 while(true){ 00419 if(n->next != n->points+CAPACITY){ 00420 *n->next++ = p; 00421 return; 00422 } 00423 if(!n->children) n->split(alloc.next()); 00424 n = (n->children 00425 + (p[0] > n->boundary.center[0]) 00426 + 2 * (p[1] > n->boundary.center[1]) 00427 + 4 * (p[2] > n->boundary.center[2])); 00428 } 00429 } 00430 00432 00434 template<class ForwardIterator> 00435 void assign(ForwardIterator begin, ForwardIterator end){ 00436 clear(); 00437 for(; begin != end; ++begin){ 00438 insert(*begin); 00439 } 00440 } 00441 00442 00444 std::vector<Pt> query(const Scalar &minX, const Scalar &minY, const Scalar &minZ, 00445 const Scalar &width, const Scalar &height, const Scalar &depth) const{ 00446 AABB range(scale_up(Pt(minX+width/2, minY+height/2, minZ+depth/2)), 00447 scale_up(Pt(width/2,height/2, depth/2))); 00448 std::vector<Pt> found; 00449 root->query(range,found); 00450 return found; 00451 } 00452 00454 std::vector<Pt> queryAll() const { 00455 return alloc.all(); 00456 } 00457 00458 /* 00460 utils::VisualizationDescription vis() const{ 00461 utils::VisualizationDescription d; 00462 d.color(0,100,255,255); 00463 d.fill(0,0,0,0); 00464 root->vis(d); 00465 return d; 00466 } 00467 */ 00468 00470 00471 void clear(){ 00472 root->next = root->points; 00473 root->children = 0; 00474 alloc.clear(); 00475 num = 0; 00476 } 00477 00479 int size() const { 00480 return num; 00481 } 00482 00483 /* 00485 void printStructure(){ 00486 std::cout << "QuadTree{" << std::endl; 00487 root->printStructure(1); 00488 std::cout << "}" << std::endl; 00489 } 00490 */ 00491 }; 00492 00493 } // namespace math 00494 } // namespace icl 00495