QuadTree.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLMath/src/ICLMath/QuadTree.h                         **
** Module : ICLMath                                                **
** Authors: Christof Elbrechter                                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.GPL  **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once


#include <algorithm>
#include <set>

#include <ICLMath/FixedVector.h>
#include <ICLUtils/VisualizationDescription.h>
#include <ICLUtils/Rect32f.h>
#include <ICLUtils/Range.h>
#include <ICLUtils/StringUtils.h>

namespace icl{

  namespace math{
    


    template<class Scalar, int CAPACITY=4, int SF=1, int ALLOC_CHUNK_SIZE=1024>
    class QuadTree{
      public:
      
      // 2D-point type, internally used
      typedef FixedColVector<Scalar,2> Pt;
      
      protected:


      struct AABB{
        Pt center;   
        Pt halfSize; 
        
        AABB(){}

        AABB(const Pt &center, const Pt &halfSize):
          center(center),halfSize(halfSize){}
        
        bool contains(const Pt &p) const{
          return ( p[0] >= center[0] - halfSize[0]
                   && p[1] >= center[1] - halfSize[1]
                   && p[0] <= center[0] + halfSize[0]
                   && p[1] <= center[1] + halfSize[1]);
        }
        
        bool intersects(const AABB &o) const{
          return  (fabs(center[0] - o.center[0]) < (halfSize[0] + o.halfSize[0])
                   && fabs(center[1] - o.center[1]) < (halfSize[1] + o.halfSize[1]));
        }

        
        utils::Rect32f rect() const {
          return utils::Rect32f(double(center[0])/SF - double(halfSize[0])/SF, double(center[1])/SF - double(halfSize[1])/SF,
                                double(halfSize[0])/SF*2, double(halfSize[1])/SF*2);
        }
      };
      

      struct Node{
        AABB boundary;         
        Pt points[CAPACITY];   
        Pt *next;              
        Node *children;        
        Node *parent;

        Node(){}
        
        Node(const AABB &boundary){
          init(0,boundary);
        }

        void init(Node *parent, const AABB &boundary){
          this->boundary = boundary;
          this->next = this->points;
          this->children = 0;
          this->parent = parent;
        }
    

        void query(const AABB &boundary, std::vector<Pt> &found) const{
          if (!this->boundary.intersects(boundary)){
            return;
          }
          for(const Pt *p=points; p<next ; ++p){
            if(boundary.contains(*p)){
              found.push_back(Pt((*p)[0]/SF,(*p)[1]/SF));
            }
          }
          if(!children) return;
          
          children[0].query(boundary,found);
          children[1].query(boundary,found);
          children[2].query(boundary,found);
          children[3].query(boundary,found);
        }


        void split(Node *children){
          const Pt half = boundary.halfSize/2;//*0.5;

          const Pt &c = boundary.center;
          this->children = children;
          this->children[0].init(this,AABB(Pt(c[0]-half[0],c[1]-half[1]),half));
          this->children[1].init(this,AABB(Pt(c[0]+half[0],c[1]-half[1]),half));
          this->children[2].init(this,AABB(Pt(c[0]-half[0],c[1]+half[1]),half));
          this->children[3].init(this,AABB(Pt(c[0]+half[0],c[1]+half[1]),half));
        }
        
        void vis(utils::VisualizationDescription &d) const{
          d.rect(boundary.rect());
          if(children){
            children[0].vis(d);
            children[1].vis(d);
            children[2].vis(d);
            children[3].vis(d);
          }
        }

        void printStructure(int indent){
          for(int i=0;i<indent;++i) std::cout << "  ";
          if(children){
            std::cout << "Branch (";
          }else{
            std::cout << "Leaf (";
          }
          std::cout << "AABB=" << boundary.rect() << ", ";
          std::cout << "Content=" <<(int)(next-points) << "/" << CAPACITY;
          std::cout <<  ")";
          if(children){
            std::cout << " {" << std::endl;
            children[0].printStructure(indent+1);
            children[1].printStructure(indent+1);
            children[2].printStructure(indent+1);
            children[3].printStructure(indent+1);
            for(int i=0;i<indent;++i) std::cout << "  ";
            std::cout << "}" << std::endl;;
          }
          else std::cout << std::endl;

        }

      };
      

      struct Allocator{
        
        std::vector<Node*> allocated;
        
        int curr;

        Allocator(){ grow(); }
        
        void grow(){
          allocated.push_back(new Node[ALLOC_CHUNK_SIZE*4]);
          curr = 0;
        }
      
        void clear(){
          for(size_t i=1;i<allocated.size();++i){
            delete [] allocated[i];
          }
          allocated.resize(1);
          curr = 0;
        }
        
        Node *next(){
          if(curr == ALLOC_CHUNK_SIZE) grow();
          return allocated.back()+4*curr++;
        }
        
        ~Allocator(){
          for(size_t i=0;i<allocated.size();++i){
            delete [] allocated[i];
          }
        }
        
        std::vector<Pt> all() const{
          std::vector<Pt> pts;
          for(size_t i=0;i<allocated.size()-1;++i){
            const Node *ns = allocated[i];
            for(size_t j=0;j<allocated.size();++j){
              for(const Pt* p = ns[j].points; p != ns[j].next;++p){
                pts.push_back(Pt((*p)[0]/SF,(*p)[1]/SF));
              }
            }
          }
          const Node *ns = allocated.back();
          for(int i=0;i<curr*4;++i){
            for(const Pt* p = ns[i].points; p != ns[i].next;++p){
              pts.push_back(Pt((*p)[0]/SF,(*p)[1]/SF));
            }
          }
          return pts;
        }
      };

      Node *root;
      
      Allocator alloc;

      int num;

      public:
      QuadTree(const Scalar &minX, const Scalar &minY, const Scalar &width, const Scalar &height):num(0){
        this->root = new Node(AABB(Pt(SF*minX+SF*width/2, SF*minY+SF*height/2),
                                   Pt(SF*width/2,SF*height/2)));
      }

      QuadTree(const utils::Rect32f &bounds):num(0){
        this->root = new Node(AABB(Pt(SF*bounds.x+SF*bounds.width/2, SF*bounds.y+SF*bounds.height/2),
                                   Pt(SF*bounds.width/2,SF*bounds.height/2)));
      }

      QuadTree(const utils::Rect &bounds):num(0){
        this->root = new Node(AABB(Pt(SF*bounds.x+SF*bounds.width/2, SF*bounds.y+SF*bounds.height/2),
                                   Pt(SF*bounds.width/2,SF*bounds.height/2)));
      }

      QuadTree(const Scalar &width, const Scalar &height):num(0){
        this->root = new Node(AABB(Pt(SF*width/2,SF*height/2), 
                                   Pt(SF*width/2,SF*height/2)));
      }

      QuadTree(const utils::Size32f &size):num(0){
        this->root = new Node(AABB(Pt(SF*size.width/2,SF*size.height/2), 
                                   Pt(SF*size.width/2,SF*size.height/2)));
      }
 
      QuadTree(const utils::Size &size):num(0){
        this->root = new Node(AABB(Pt(SF*size.width/2,SF*size.height/2), 
                                   Pt(SF*size.width/2,SF*size.height/2)));
      }


      ~QuadTree(){
        // the root node is not part of the allocator
        delete root;
      }
  
      protected:
      
      const Pt &nn_approx_internal(const Pt &p, double &currMinDist, const Pt *&currNN) const throw (utils::ICLException){
        // 1st find cell, that continas p
        const Node *n = root;
        while(n->children){
          n = (n->children + (p[0] > n->boundary.center[0]) + 2 * (p[1] > n->boundary.center[1]));
        }
        
        // this cell could be empty, in this case, the parent must contain good points
        if(n->next == n->points){
          n = n->parent;
          if(!n) throw utils::ICLException("no nn found for given point " + utils::str(p));
        }
        
        double sqrMinDist = utils::sqr(currMinDist);

        for(const Pt *x=n->points; x < n->next; ++x){
          Scalar dSqr = utils::sqr(x->operator[](0)-p[0]) + utils::sqr(x->operator[](1)-p[1]);
          if(dSqr < sqrMinDist){
            sqrMinDist = dSqr;
            currNN = x; 
          }
        }
        currMinDist = sqrt(sqrMinDist);

        if(!currNN){
          throw utils::ICLException("no nn found for given point " + utils::str(p));
        }
        return *currNN;
      }
      public:
      
      

      Pt nn_approx(const Pt &p) const throw (utils::ICLException){
        double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1);
        const Pt *currNN  = 0;
        nn_approx_internal(Pt(SF*p[0],SF*p[1]),currMinDist,currNN);
        return Pt((*currNN)[0]/SF,(*currNN)[1]/SF) ;
      }


      Pt nn(const Pt &pIn) const throw (utils::ICLException){
        const Pt p(SF*pIn[0],SF*pIn[1]);
        std::vector<const Node*> stack;
        stack.reserve(128);
        stack.push_back(root);
        double currMinDist = sqrt(utils::Range<Scalar>::limits().maxVal-1);
        const Pt *currNN  = 0;
        
        nn_approx_internal(p,currMinDist,currNN);
        
        while(stack.size()){
          const Node *n = stack.back();
          stack.pop_back();
          if(n->children){
            const AABB b(p, Pt(currMinDist,currMinDist));
            if(b.intersects(n->children[0].boundary)) stack.push_back(n->children);
            if(b.intersects(n->children[1].boundary)) stack.push_back(n->children+1);
            if(b.intersects(n->children[2].boundary)) stack.push_back(n->children+2);
            if(b.intersects(n->children[3].boundary)) stack.push_back(n->children+3);
          }
          Scalar sqrMinDist = utils::sqr(currMinDist);

          for(const Pt *x=n->points; x < n->next; ++x){
            Scalar dSqr = utils::sqr(x->operator[](0)-p[0]) + utils::sqr(x->operator[](1)-p[1]);
            if(dSqr < sqrMinDist){
              sqrMinDist = dSqr;
              currNN = x; 
            }
          }
          currMinDist = sqrt(sqrMinDist);
        }
        return Pt((*currNN)[0]/SF, (*currNN)[1]/SF);
      }

      const utils::Point32f nn(const utils::Point32f &p) const throw (utils::ICLException){
        Pt n = nn(Pt(p.x,p.y));
        return utils::Point32f(n[0],n[1]);
      }

      const utils::Point nn(const utils::Point &p) const throw (utils::ICLException){
        Pt n = nn(Pt(p.x,p.y));
        return utils::Point(n[0],n[1]);
      }

      

      void insert(const Pt &pIn){
        ++num;
        const Pt p(SF*pIn[0],SF*pIn[1]);
        Node *n = root;
        while(true){
          if(n->next != n->points+CAPACITY){
            *n->next++ = p;
            return;
          }
          if(!n->children) n->split(alloc.next());
          n = (n->children + (p[0] > n->boundary.center[0]) + 2 * (p[1] > n->boundary.center[1]));
        }
      }
      
      void insert(const utils::Point32f &p){
        insert(Pt(p.x,p.y));
      }

      void insert(const utils::Point &p){
        insert(Pt(p.x,p.y));
      }

      std::vector<Pt> query(const Scalar &minX, const Scalar &minY, 
                            const Scalar &width, const Scalar &height) const{
        AABB range(Pt(SF*minX+SF*width/2, SF*minY+SF*height/2),
                   Pt(SF*width/2,SF*height/2));
        std::vector<Pt> found;
        root->query(range,found);
        return found;
      }

      std::vector<Pt> query(const utils::Rect &r) const {  return query(r.x,r.y,r.width,r.height);  }

      std::vector<Pt> query(const utils::Rect32f &r) const {  return query(r.x,r.y,r.width,r.height);  }
      
      std::vector<Pt> queryAll() const {
        return alloc.all();
      }

      utils::VisualizationDescription vis() const{
        utils::VisualizationDescription d;
        d.color(0,100,255,255);
        d.fill(0,0,0,0);
        root->vis(d);
        return d;
      }
      

      void clear(){
        root->next = root->points;
        root->children = 0;
        alloc.clear();
        num = 0;
      }

      int size() const {
        return num;
      }

      void printStructure(){
        std::cout << "QuadTree{" << std::endl;
        root->printStructure(1);
        std::cout << "}" << std::endl;
      }
    };

  } // namespace math
} // namespace icl