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/QuadTree.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 <ICLMath/FixedVector.h> 00036 #include <ICLUtils/VisualizationDescription.h> 00037 #include <ICLUtils/Rect32f.h> 00038 #include <ICLUtils/Range.h> 00039 #include <ICLUtils/StringUtils.h> 00040 00041 #include <algorithm> 00042 #include <set> 00043 00044 namespace icl{ 00045 00046 namespace math{ 00047 00048 00050 00174 template<class Scalar, int CAPACITY=4, int SF=1, int ALLOC_CHUNK_SIZE=1024> 00175 class QuadTree{ 00176 public: 00177 00178 // 2D-point type, internally used 00179 typedef FixedColVector<Scalar,2> Pt; 00180 00181 protected: 00182 00184 00186 struct AABB{ 00187 Pt center; 00188 Pt halfSize; 00189 00191 AABB(){} 00192 00194 AABB(const Pt ¢er, const Pt &halfSize): 00195 center(center),halfSize(halfSize){} 00196 00198 bool contains(const Pt &p) const{ 00199 return ( p[0] >= center[0] - halfSize[0] 00200 && p[1] >= center[1] - halfSize[1] 00201 && p[0] <= center[0] + halfSize[0] 00202 && p[1] <= center[1] + halfSize[1]); 00203 } 00204 00206 bool intersects(const AABB &o) const{ 00207 #ifdef WIN32 00208 return (fabs((float)center[0] - o.center[0]) < (halfSize[0] + o.halfSize[0]) 00209 && fabs((float)center[1] - o.center[1]) < (halfSize[1] + o.halfSize[1])); 00210 #else 00211 return (fabs(center[0] - o.center[0]) < (halfSize[0] + o.halfSize[0]) 00212 && fabs(center[1] - o.center[1]) < (halfSize[1] + o.halfSize[1])); 00213 #endif 00214 } 00215 00216 00217 utils::Rect32f rect() const { 00218 return utils::Rect32f(double(center[0])/SF - double(halfSize[0])/SF, double(center[1])/SF - double(halfSize[1])/SF, 00219 double(halfSize[0])/SF*2, double(halfSize[1])/SF*2); 00220 } 00221 }; 00222 00224 00226 struct Node{ 00227 AABB boundary; 00228 Pt points[CAPACITY]; 00229 Pt *next; 00230 Node *children; 00231 Node *parent; 00232 00234 Node(){} 00235 00237 Node(const AABB &boundary){ 00238 init(0,boundary); 00239 } 00241 00242 void init(Node *parent, const AABB &boundary){ 00243 this->boundary = boundary; 00244 this->next = this->points; 00245 this->children = 0; 00246 this->parent = parent; 00247 } 00248 00250 00253 void query(const AABB &boundary, std::vector<Pt> &found) const{ 00254 if (!this->boundary.intersects(boundary)){ 00255 return; 00256 } 00257 for(const Pt *p=points; p<next ; ++p){ 00258 if(boundary.contains(*p)){ 00259 found.push_back(Pt((*p)[0]/SF,(*p)[1]/SF)); 00260 } 00261 } 00262 if(!children) return; 00263 00264 children[0].query(boundary,found); 00265 children[1].query(boundary,found); 00266 children[2].query(boundary,found); 00267 children[3].query(boundary,found); 00268 } 00269 00271 00274 void split(Node *children){ 00275 const Pt half = boundary.halfSize/2;//*0.5; 00276 00277 const Pt &c = boundary.center; 00278 this->children = children; 00279 this->children[0].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1]),half)); 00280 this->children[1].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1]),half)); 00281 this->children[2].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1]),half)); 00282 this->children[3].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1]),half)); 00283 } 00284 00286 void vis(utils::VisualizationDescription &d) const{ 00287 d.rect(boundary.rect()); 00288 if(children){ 00289 children[0].vis(d); 00290 children[1].vis(d); 00291 children[2].vis(d); 00292 children[3].vis(d); 00293 } 00294 } 00295 00296 void printStructure(int indent){ 00297 for(int i=0;i<indent;++i) std::cout << " "; 00298 if(children){ 00299 std::cout << "Branch ("; 00300 }else{ 00301 std::cout << "Leaf ("; 00302 } 00303 std::cout << "AABB=" << boundary.rect() << ", "; 00304 std::cout << "Content=" <<(int)(next-points) << "/" << CAPACITY; 00305 std::cout << ")"; 00306 if(children){ 00307 std::cout << " {" << std::endl; 00308 children[0].printStructure(indent+1); 00309 children[1].printStructure(indent+1); 00310 children[2].printStructure(indent+1); 00311 children[3].printStructure(indent+1); 00312 for(int i=0;i<indent;++i) std::cout << " "; 00313 std::cout << "}" << std::endl;; 00314 } 00315 else std::cout << std::endl; 00316 00317 } 00318 00319 }; 00320 00322 00324 struct Allocator{ 00325 00327 std::vector<Node*> allocated; 00328 00330 int curr; 00331 00333 Allocator(){ grow(); } 00334 00336 void grow(){ 00337 allocated.push_back(new Node[ALLOC_CHUNK_SIZE*4]); 00338 curr = 0; 00339 } 00340 00342 void clear(){ 00343 for(size_t i=1;i<allocated.size();++i){ 00344 delete [] allocated[i]; 00345 } 00346 allocated.resize(1); 00347 curr = 0; 00348 } 00349 00351 Node *next(){ 00352 if(curr == ALLOC_CHUNK_SIZE) grow(); 00353 return allocated.back()+4*curr++; 00354 } 00355 00357 ~Allocator(){ 00358 for(size_t i=0;i<allocated.size();++i){ 00359 delete [] allocated[i]; 00360 } 00361 } 00362 00364 std::vector<Pt> all() const{ 00365 std::vector<Pt> pts; 00366 for(size_t i=0;i<allocated.size()-1;++i){ 00367 const Node *ns = allocated[i]; 00368 for(size_t j=0;j<allocated.size();++j){ 00369 for(const Pt* p = ns[j].points; p != ns[j].next;++p){ 00370 pts.push_back(Pt((*p)[0]/SF,(*p)[1]/SF)); 00371 } 00372 } 00373 } 00374 const Node *ns = allocated.back(); 00375 for(int i=0;i<curr*4;++i){ 00376 for(const Pt* p = ns[i].points; p != ns[i].next;++p){ 00377 pts.push_back(Pt((*p)[0]/SF,(*p)[1]/SF)); 00378 } 00379 } 00380 return pts; 00381 } 00382 }; 00383 00385 Node *root; 00386 00388 Allocator alloc; 00389 00391 int num; 00392 00393 public: 00395 QuadTree(const Scalar &minX, const Scalar &minY, const Scalar &width, const Scalar &height):num(0){ 00396 this->root = new Node(AABB(Pt(SF*minX+SF*width/2, SF*minY+SF*height/2), 00397 Pt(SF*width/2,SF*height/2))); 00398 } 00399 00401 QuadTree(const utils::Rect32f &bounds):num(0){ 00402 this->root = new Node(AABB(Pt(SF*bounds.x+SF*bounds.width/2, SF*bounds.y+SF*bounds.height/2), 00403 Pt(SF*bounds.width/2,SF*bounds.height/2))); 00404 } 00405 00407 QuadTree(const utils::Rect &bounds):num(0){ 00408 this->root = new Node(AABB(Pt(SF*bounds.x+SF*bounds.width/2, SF*bounds.y+SF*bounds.height/2), 00409 Pt(SF*bounds.width/2,SF*bounds.height/2))); 00410 } 00411 00413 QuadTree(const Scalar &width, const Scalar &height):num(0){ 00414 this->root = new Node(AABB(Pt(SF*width/2,SF*height/2), 00415 Pt(SF*width/2,SF*height/2))); 00416 } 00417 00419 QuadTree(const utils::Size32f &size):num(0){ 00420 this->root = new Node(AABB(Pt(SF*size.width/2,SF*size.height/2), 00421 Pt(SF*size.width/2,SF*size.height/2))); 00422 } 00423 00425 QuadTree(const utils::Size &size):num(0){ 00426 this->root = new Node(AABB(Pt(SF*size.width/2,SF*size.height/2), 00427 Pt(SF*size.width/2,SF*size.height/2))); 00428 } 00429 00431 00432 ~QuadTree(){ 00433 // the root node is not part of the allocator 00434 delete root; 00435 } 00436 00437 protected: 00438 00440 const Pt &nn_approx_internal(const Pt &p, double &currMinDist, const Pt *&currNN) const throw (utils::ICLException){ 00441 // 1st find cell, that continas p 00442 const Node *n = root; 00443 while(n->children){ 00444 n = (n->children + (p[0] > n->boundary.center[0]) + 2 * (p[1] > n->boundary.center[1])); 00445 } 00446 00447 // this cell could be empty, in this case, the parent must contain good points 00448 if(n->next == n->points){ 00449 n = n->parent; 00450 if(!n) throw utils::ICLException("no nn found for given point " + utils::str(p)); 00451 } 00452 00453 double sqrMinDist = utils::sqr(currMinDist); 00454 00455 for(const Pt *x=n->points; x < n->next; ++x){ 00456 Scalar dSqr = utils::sqr(x->operator[](0)-p[0]) + utils::sqr(x->operator[](1)-p[1]); 00457 if(dSqr < sqrMinDist){ 00458 sqrMinDist = dSqr; 00459 currNN = x; 00460 } 00461 } 00462 currMinDist = sqrt(sqrMinDist); 00463 00464 if(!currNN){ 00465 throw utils::ICLException("no nn found for given point " + utils::str(p)); 00466 } 00467 return *currNN; 00468 } 00469 public: 00470 00471 00473 00482 Pt nn_approx(const Pt &p) const throw (utils::ICLException){ 00483 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00484 const Pt *currNN = 0; 00485 nn_approx_internal(Pt(SF*p[0],SF*p[1]),currMinDist,currNN); 00486 return Pt((*currNN)[0]/SF,(*currNN)[1]/SF) ; 00487 } 00488 00490 00508 Pt nn(const Pt &pIn) const throw (utils::ICLException){ 00509 const Pt p(SF*pIn[0],SF*pIn[1]); 00510 std::vector<const Node*> stack; 00511 stack.reserve(128); 00512 stack.push_back(root); 00513 double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1); 00514 const Pt *currNN = 0; 00515 00516 nn_approx_internal(p,currMinDist,currNN); 00517 00518 while(stack.size()){ 00519 const Node *n = stack.back(); 00520 stack.pop_back(); 00521 if(n->children){ 00522 const AABB b(p, Pt(currMinDist,currMinDist)); 00523 if(b.intersects(n->children[0].boundary)) stack.push_back(n->children); 00524 if(b.intersects(n->children[1].boundary)) stack.push_back(n->children+1); 00525 if(b.intersects(n->children[2].boundary)) stack.push_back(n->children+2); 00526 if(b.intersects(n->children[3].boundary)) stack.push_back(n->children+3); 00527 } 00528 Scalar sqrMinDist = utils::sqr(currMinDist); 00529 00530 for(const Pt *x=n->points; x < n->next; ++x){ 00531 Scalar dSqr = utils::sqr(x->operator[](0)-p[0]) + utils::sqr(x->operator[](1)-p[1]); 00532 if(dSqr < sqrMinDist){ 00533 sqrMinDist = dSqr; 00534 currNN = x; 00535 } 00536 } 00537 currMinDist = sqrt(sqrMinDist); 00538 } 00539 return Pt((*currNN)[0]/SF, (*currNN)[1]/SF); 00540 } 00541 00543 const utils::Point32f nn(const utils::Point32f &p) const throw (utils::ICLException){ 00544 Pt n = nn(Pt(p.x,p.y)); 00545 return utils::Point32f(n[0],n[1]); 00546 } 00547 00549 const utils::Point nn(const utils::Point &p) const throw (utils::ICLException){ 00550 Pt n = nn(Pt(p.x,p.y)); 00551 return utils::Point(n[0],n[1]); 00552 } 00553 00554 00556 00559 void insert(const Pt &pIn){ 00560 ++num; 00561 const Pt p(SF*pIn[0],SF*pIn[1]); 00562 Node *n = root; 00563 while(true){ 00564 if(n->next != n->points+CAPACITY){ 00565 *n->next++ = p; 00566 return; 00567 } 00568 if(!n->children) n->split(alloc.next()); 00569 n = (n->children + (p[0] > n->boundary.center[0]) + 2 * (p[1] > n->boundary.center[1])); 00570 } 00571 } 00572 00574 void insert(const utils::Point32f &p){ 00575 insert(Pt(p.x,p.y)); 00576 } 00577 00579 void insert(const utils::Point &p){ 00580 insert(Pt(p.x,p.y)); 00581 } 00582 00584 std::vector<Pt> query(const Scalar &minX, const Scalar &minY, 00585 const Scalar &width, const Scalar &height) const{ 00586 AABB range(Pt(SF*minX+SF*width/2, SF*minY+SF*height/2), 00587 Pt(SF*width/2,SF*height/2)); 00588 std::vector<Pt> found; 00589 root->query(range,found); 00590 return found; 00591 } 00592 00594 std::vector<Pt> query(const utils::Rect &r) const { return query(r.x,r.y,r.width,r.height); } 00595 00597 std::vector<Pt> query(const utils::Rect32f &r) const { return query(r.x,r.y,r.width,r.height); } 00598 00600 std::vector<Pt> queryAll() const { 00601 return alloc.all(); 00602 } 00603 00605 utils::VisualizationDescription vis() const{ 00606 utils::VisualizationDescription d; 00607 d.color(0,100,255,255); 00608 d.fill(0,0,0,0); 00609 root->vis(d); 00610 return d; 00611 } 00612 00614 00615 void clear(){ 00616 root->next = root->points; 00617 root->children = 0; 00618 alloc.clear(); 00619 num = 0; 00620 } 00621 00623 int size() const { 00624 return num; 00625 } 00626 00628 void printStructure(){ 00629 std::cout << "QuadTree{" << std::endl; 00630 root->printStructure(1); 00631 std::cout << "}" << std::endl; 00632 } 00633 }; 00634 00635 } // namespace math 00636 } // namespace icl 00637