kdtree.hpp

一个C++写的KdTree容器模板库

          typename _Region::_CenterPt __pt(__region,__Max_R);
          return this->find_nearest(__pt);
        }



      template <typename SearchVal, class Predicate>
         std::pair<const_iterator,distance_type>
        find_nearest_if(SearchVal __V, subvalue_type const __Max_R, Predicate predicate ) const throw ()
        {
          if (!_M_get_root())
             return std::pair<const_iterator,distance_type>(this->end(),__Max_R);
          _Region __region(_M_acc,__V);  // note: zero-area!
          typename _Region::_CenterPt __pt(__region,__Max_R);
          return this->find_nearest_if(__pt,predicate);
        }


      std::pair<const_iterator,distance_type>
        find_nearest( typename _Region::_CenterPt const& __CENTER ) const throw ()
        {
          if (_M_get_root())
          {
             // note: we set the initial 'bounds' to the exact point.
             // they expand from there outwards.
             _Region __bounds(__CENTER.first);
             std::pair<const_iterator,distance_type> best = _M_find_nearest(_M_get_root(), __CENTER, __bounds, 0, always_true());
             // ensure we return end() if we didn't find it
             // however, also return the best distance we did find, it might be useful to someone.
             if (best.second > __CENTER.second)
                best.first = this->end();
             return best;
          }
         return std::pair<const_iterator,distance_type>(this->end(),__CENTER.second);
        }


      template <class Predicate>
      std::pair<const_iterator,distance_type>
        find_nearest_if( typename _Region::_CenterPt const& __CENTER, Predicate predicate ) const throw ()
        {
          if (_M_get_root())
          {
             // note: we set the initial 'bounds' to the exact point.
             // they expand from there outwards.
             _Region __bounds(__CENTER.first);
             std::pair<const_iterator,distance_type> best = _M_find_nearest(_M_get_root(), __CENTER, __bounds, 0, predicate);
             // ensure we return end() if we didn't find it
             // however, also return the best distance we did find, it might be useful to someone.
             if (best.second > __CENTER.second)
                best.first = this->end();
             return best;
          }
         return std::pair<const_iterator,distance_type>(this->end(),__CENTER.second);
        }


      void
      optimise()
      {
        std::vector<value_type> __v(this->begin(),this->end());
        this->clear();
        _M_optimise(__v.begin(), __v.end(), 0);
      }

      void
      optimize()
      { // cater for people who cannot spell :)
        this->optimise();
      }

      void check_tree()
      {
         _M_check_node(_M_get_root(),0);
      }

    protected:

      void _M_check_children( _Link_const_type child, _Link_const_type parent, size_t const level, bool to_the_left )
      {
         assert(parent);
         if (child)
         {
            _Node_compare compare(level % __K,_M_acc);
            // REMEMBER! its a <= relationship for BOTH branches
            // for left-case (true), child<=node --> !(node<child)
            // for right-case (false), node<=child --> !(child<node)
            assert(!to_the_left or !compare(parent,child));  // check the left
            assert(to_the_left or !compare(child,parent));   // check the right
            // and recurse down the tree, checking everything
            _M_check_children(_S_left(child),parent,level,to_the_left);
            _M_check_children(_S_right(child),parent,level,to_the_left);
         }
      }

      void _M_check_node( _Link_const_type node, size_t const level )
      {
         if (node)
         {
            // (comparing on this level)
            // everything to the left of this node must be smaller than this
            _M_check_children( _S_left(node), node, level, true );
            // everything to the right of this node must be larger than this
            _M_check_children( _S_right(node), node, level, false );

            _M_check_node( _S_left(node), level+1 );
            _M_check_node( _S_right(node), level+1 );
         }
      }

      void _M_empty_initialise()
      {
        _M_set_leftmost(_M_header);
        _M_set_rightmost(_M_header);
        _M_set_root(NULL);
      }

      iterator
      _M_insert_left(_Link_type __N, const_reference __V)
      {
        _S_set_left(__N, _M_new_node(__V)); ++_M_count;
        _S_set_parent( _S_left(__N), __N );
        if (__N == _M_get_leftmost())
           _M_set_leftmost( _S_left(__N) );
        return iterator(_S_left(__N));
      }

      iterator
      _M_insert_right(_Link_type __N, const_reference __V)
      {
        _S_set_right(__N, _M_new_node(__V)); ++_M_count;
        _S_set_parent( _S_right(__N), __N );
        if (__N == _M_get_rightmost())
           _M_set_rightmost( _S_right(__N) );
        return iterator(_S_right(__N));
      }

      iterator
      _M_insert(_Link_type __N, const_reference __V,
             size_t const __L) throw (std::bad_alloc)
      {
        if (_Node_compare(__L % __K,_M_acc)(__V, __N))
          {
            if (!_S_left(__N))
              return _M_insert_left(__N, __V);
            return _M_insert(_S_left(__N), __V, __L+1);
          }
        else
          {
            if (!_S_right(__N) || __N == _M_get_rightmost())
              return _M_insert_right(__N, __V);
            return _M_insert(_S_right(__N), __V, __L+1);
          }
      }

      _Link_type
      _M_erase(_Link_type dead_dad, size_t const level) throw ()
      {
         // find a new step_dad, he will become a drop-in replacement.
        _Link_type step_dad = _M_get_erase_replacement(dead_dad, level);

         // tell dead_dad's parent that his new child is step_dad
        if (dead_dad == _M_get_root())
           _S_set_parent(_M_header, step_dad);
        else if (_S_left(_S_parent(dead_dad)) == dead_dad)
            _S_set_left(_S_parent(dead_dad), step_dad);
        else
            _S_set_right(_S_parent(dead_dad), step_dad);

        // deal with the left and right edges of the tree...
        // if the dead_dad was at the edge, then substitude...
        // but if there IS no new dead, then left_most is the dead_dad's parent
         if (dead_dad == _M_get_leftmost())
           _M_set_leftmost( (step_dad ? step_dad : _S_parent(dead_dad)) );
         if (dead_dad == _M_get_rightmost())
           _M_set_rightmost( (step_dad ? step_dad : _S_parent(dead_dad)) );

        if (step_dad)
          {
             // step_dad gets dead_dad's parent
            _S_set_parent(step_dad, _S_parent(dead_dad));

            // first tell the children that step_dad is their new dad
            if (_S_left(dead_dad))
               _S_set_parent(_S_left(dead_dad), step_dad);
            if (_S_right(dead_dad))
               _S_set_parent(_S_right(dead_dad), step_dad);

            // step_dad gets dead_dad's children
            _S_set_left(step_dad, _S_left(dead_dad));
            _S_set_right(step_dad, _S_right(dead_dad));
          }

        return step_dad;
      }



      _Link_type
      _M_get_erase_replacement(_Link_type node, size_t const level) throw ()
      {
         // if 'node' is null, then we can't do any better
        if (_S_is_leaf(node))
           return NULL;

        std::pair<_Link_type,size_t> candidate;
        // if there is nothing to the left, find a candidate on the right tree
        if (!_S_left(node))
          candidate = _M_get_j_min( std::pair<_Link_type,size_t>(_S_right(node),level), level+1);
        // ditto for the right
        else if ((!_S_right(node)))
          candidate = _M_get_j_max( std::pair<_Link_type,size_t>(_S_left(node),level), level+1);
        // we have both children ...
        else
         {
            // we need to do a little more work in order to find a good candidate
            // this is actually a technique used to choose a node from either the
            // left or right branch RANDOMLY, so that the tree has a greater change of
            // staying balanced.
            // If this were a true binary tree, we would always hunt down the right branch.
            // See top for notes.
            _Node_compare compare(level % __K,_M_acc);
            // compare the children based on this level's criteria...
            // (this gives virtually random results)
            if (compare(_S_right(node), _S_left(node)))
               // the right is smaller, get our replacement from the SMALLEST on the right
               candidate = _M_get_j_min(std::pair<_Link_type,size_t>(_S_right(node),level), level+1);
            else
               candidate = _M_get_j_max( std::pair<_Link_type,size_t>(_S_left(node),level), level+1);
         }

        // we have a candidate replacement by now.
        // remove it from the tree, but don't delete it.
        // it must be disconnected before it can be reconnected.
        _Link_type parent = _S_parent(candidate.first);
        if (_S_left(parent) == candidate.first)
           _S_set_left(parent, _M_erase(candidate.first, candidate.second));
        else
           _S_set_right(parent, _M_erase(candidate.first, candidate.second));

        return candidate.first;
      }



      std::pair<_Link_type,size_t>
      _M_get_j_min( std::pair<_Link_type,size_t> const node, size_t const level) throw ()
      {
         typedef std::pair<_Link_type,size_t> Result;
        if (_S_is_leaf(node.first))
            return Result(node.first,level);

        _Node_compare compare(node.second % __K,_M_acc);
        Result candidate = node;
        if (_S_left(node.first))
          {
            Result left = _M_get_j_min(Result(_S_left(node.first), node.second), level+1);
            if (compare(left.first, candidate.first))
                candidate = left;
          }
        if (_S_right(node.first))
          {
            Result right = _M_get_j_min( Result(_S_right(node.first),node.second), level+1);
            if (compare(right.first, candidate.first))
                candidate = right;
          }
        if (candidate.first == node.first)
           return Result(candidate.first,level);

        return candidate;
      }



      std::pair<_Link_type,size_t>
      _M_get_j_max( std::pair<_Link_type,size_t> const node, size_t const level) throw ()
      {
         typedef std::pair<_Link_type,size_t> Result;

        if (_S_is_leaf(node.first))
            return Result(node.first,level);

        _Node_compare compare(node.second % __K,_M_acc);
        Result candidate = node;
        if (_S_left(node.first))
          {
            Result left = _M_get_j_max( Result(_S_left(node.first),node.second), level+1);
            if (compare(candidate.first, left.first))
                candidate = left;
          }
        if (_S_right(node.first))
          {
            Result right = _M_get_j_max(Result(_S_right(node.first),node.second), level+1);
            if (compare(candidate.first, right.first))
                candidate = right;
          }

        if (candidate.first == node.first)
           return Result(candidate.first,level);

        return candidate;
      }


      void
      _M_erase_subtree(_Link_type __n)
      {
        while (__n)
          {
            _M_erase_subtree(_S_right(__n));
            _Link_type __t = _S_left(__n);
            _M_delete_node(__n);
            __n = __t;
          }
      }

      const_iterator
      _M_find(_Link_const_type node, const_reference value, size_t const level) const throw ()
      {
         // be aware! This is very different to normal binary searches, because of the <=
         // relationship used. See top for notes.
         // Basically we have to check ALL branches, as we may have an identical node
         // in different branches.
          const_iterator found = this->end();

        _Node_compare compare(level % __K,_M_acc);
        if (!compare(node,value))   // note, this is a <= test
          {
           // this line is the only difference between _M_find_exact() and _M_find()
            if (_M_matches_node_in_other_ds(node, value, level))
              return const_iterator(node);   // return right away
            if (_S_left(node))
               found = _M_find(_S_left(node), value, level+1);
          }
        if ( _S_right(node) && found == this->end() && !compare(value,node))   // note, this is a <= test
            found = _M_find(_S_right(node), value, level+1);
        return found;
      }

      const_iterator
      _M_find_exact(_Link_const_type node, const_reference value, size_t const level) const throw ()
      {
         // be aware! This is very different to normal binary searches, because of the <=
         // relationship used. See top for notes.
         // Basically we have to check ALL branches, as we may have an identical node
         // in different branches.
          const_iterator found = this->end();

        _Node_compare compare(level % __K,_M_acc);
        if (!compare(node,value))  // note, this is a <= test
        {
           // this line is the only difference between _M_find_exact() and _M_find()
            if (value == *const_iterator(node))
              return const_iterator(node);   // return right away
           if (_S_left(node))
            found = _M_find_exact(_S_left(node), value, level+1);
        }

        // note: no else!  items that are identical can be down both branches
        if ( _S_right(node) && found == this->end() && !compare(value,node))   // note, this is a <= test
            found = _M_find_exact(_S_right(node), value, level+1);
        return found;
      }

      bool
      _M_matches_node_in_d(_Link_const_type __N, const_reference __V,
                           size_t const __L) const throw ()
      {
        _Node_compare compare(__L % __K,_M_acc);
        return !(compare(__N, __V) || compare(__V, __N));
      }

      bool
      _M_matches_node_in_other_ds(_Link_const_type __N, const_reference __V,
                                  size_t const __L = 0) const throw ()
      {
        size_t __i = __L;
        while ((__i = (__i + 1) % __K) != __L % __K)
          if (!_M_matches_node_in_d(__N, __V, __i)) return false;
        return true;
      }

      bool
      _M_matches_node(_Link_const_type __N, const_reference __V,
                      size_t __L = 0) const throw ()
      {
        return _M_matches_node_in_d(__N, __V, __L)
          && _M_matches_node_in_other_ds(__N, __V, __L);
      }

      size_t
        _M_count_within_range(_Link_const_type __N, _Region const& __REGION,
                             _Region const& __BOUNDS,
                             size_t const __L) const throw ()
        {
           size_t count = 0;
          if (__REGION.encloses(_S_value(__N)))
            {
               ++count;
            }