kdtree.hpp

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

          if (_S_left(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_high_bound(_S_value(__N), __L);
              if (__REGION.intersects_with(__bounds))
                count += _M_count_within_range(_S_left(__N),
                                     __REGION, __bounds, __L+1);
            }
          if (_S_right(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_low_bound(_S_value(__N), __L);
              if (__REGION.intersects_with(__bounds))
                count += _M_count_within_range(_S_right(__N),
                                     __REGION, __bounds, __L+1);
            }

          return count;
        }


      template <class Visitor>
        Visitor
        _M_visit_within_range(Visitor visitor,
                             _Link_const_type N, _Region const& REGION,
                             _Region const& BOUNDS,
                             size_t const L) const throw ()
        {
          if (REGION.encloses(_S_value(N)))
            {
              visitor(_S_value(N));
            }
          if (_S_left(N))
            {
              _Region bounds(BOUNDS);
              bounds.set_high_bound(_S_value(N), L);
              if (REGION.intersects_with(bounds))
                visitor = _M_visit_within_range(visitor, _S_left(N),
                                     REGION, bounds, L+1);
            }
          if (_S_right(N))
            {
              _Region bounds(BOUNDS);
              bounds.set_low_bound(_S_value(N), L);
              if (REGION.intersects_with(bounds))
                visitor = _M_visit_within_range(visitor, _S_right(N),
                                     REGION, bounds, L+1);
            }

          return visitor;
        }



      template <typename _OutputIterator>
        _OutputIterator
        _M_find_within_range(_OutputIterator __out,
                             _Link_const_type __N, _Region const& __REGION,
                             _Region const& __BOUNDS,
                             size_t const __L) const throw ()
        {
          if (__REGION.encloses(_S_value(__N)))
            {
              *__out++ = _S_value(__N);
            }
          if (_S_left(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_high_bound(_S_value(__N), __L);
              if (__REGION.intersects_with(__bounds))
                __out = _M_find_within_range(__out, _S_left(__N),
                                     __REGION, __bounds, __L+1);
            }
          if (_S_right(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_low_bound(_S_value(__N), __L);
              if (__REGION.intersects_with(__bounds))
                __out = _M_find_within_range(__out, _S_right(__N),
                                     __REGION, __bounds, __L+1);
            }

          return __out;
        }

        // quick little power function
        // next-best is __gnu_cxx::power
        // std::pow() is probably not ideal for simple powers. I forget exact details...
        distance_type _M_square( distance_type x ) const { return x*x; }

       // WARNING: Calculates and RETURNS dist^2 (for speed)
       // NOTE: CENTER is a region of zero area.  It is the point we are aiming for.
       //
       // How it works: Starting with a centerpt (single-pt in a region, with a range
       // attached to it), and bounds, it first calculates the distance to THIS node,
       // and adjusts the center's range DOWN if its closer.  No point looking further
       // than it needs to look.  A form of a dynamic find_within_range.
       // It expands the bounds as usual and sees if it intersects the centerpt+range.
       // And so it goes ...

         // substitude predicate for normal find_nearest()s
         struct always_true
         {
            bool operator()( _Val const& ) const { return true; }
         };

         template <class Predicate>
         std::pair<const_iterator,distance_type>
        _M_find_nearest( _Link_const_type __N, typename _Region::_CenterPt __CENTER,
                             _Region const& __BOUNDS,
                             size_t const __L,
                             Predicate predicate ) const throw ()
        {
           std::pair<const_iterator,distance_type> best(this->end(),__CENTER.second);

           // we ignore this node if the predicate isn't true
           if (predicate(*const_iterator(__N)))
           {
              distance_type dist = 0;
              for ( size_t i = 0; i != __K; ++i )
              {
                 dist += _M_square( __CENTER.first._M_low_bounds[i] - _M_acc(_S_value(__N),i) );
              }
#ifdef KDTREE_CHECK_PERFORMANCE
              ++num_dist_calcs;
#endif
              dist = sqrt(dist);

              best.first = __N;
              best.second = dist;
           }

           // adjust our CENTER target
           __CENTER.second = std::min(__CENTER.second,best.second);

          if (_S_left(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_high_bound(_S_value(__N), __L);
              if (__bounds.intersects_with(__CENTER))
              {
                 std::pair<const_iterator,distance_type> left =
                    _M_find_nearest( _S_left(__N), __CENTER, __bounds, __L+1, predicate);
                 // check if better than what I found
                 if (left.second < best.second) best = left;
              }
            }

           // adjust our center target (only useful if left found something closer)
           __CENTER.second = std::min(__CENTER.second,best.second);

          if (_S_right(__N))
            {
              _Region __bounds(__BOUNDS);
              __bounds.set_low_bound(_S_value(__N), __L);
              if (__bounds.intersects_with(__CENTER))
              {
                 std::pair<const_iterator,distance_type> right =
                    _M_find_nearest( _S_right(__N), __CENTER, __bounds, __L+1, predicate);
                 // check if better than what I found
                 if (right.second < best.second) best = right;
               }
            }

          return best;
        }

      template <typename _Iter>
        void
        _M_optimise(_Iter const& __A, _Iter const& __B,
                    size_t const __L) throw ()
      {
        if (__A == __B) return;
        _Node_compare compare(__L % __K,_M_acc);
        std::sort(__A, __B, compare);
        _Iter __m = __A + (__B - __A) / 2;
        this->insert(*__m);
        if (__m != __A) _M_optimise(__A, __m, __L+1);
        if (++__m != __B) _M_optimise(__m, __B, __L+1);
      }

      _Link_const_type
      _M_get_root() const
      {
         return static_cast<_Link_const_type>( _M_header->_M_parent );
      }

      _Link_type
      _M_get_root()
      {
         return static_cast<_Link_type>( _M_header->_M_parent );
      }

      void _M_set_root(_Node_base * n)
      {
         _M_header->_M_parent = n;
      }

      _Link_const_type
      _M_get_leftmost() const
      {
        return static_cast<_Link_type>( _M_header->_M_left );
      }

      void
      _M_set_leftmost( _Node_base * a )
      {
         _M_header->_M_left = a;
      }

      _Link_const_type
      _M_get_rightmost() const
      {
        return static_cast<_Link_type>( _M_header->_M_right );
      }

      void
      _M_set_rightmost( _Node_base * a )
      {
         _M_header->_M_right = a;
      }

      static _Link_type
      _S_parent(_Base_ptr N)
      {
        return static_cast<_Link_type>( N->_M_parent );
      }

      static _Link_const_type
      _S_parent(_Base_const_ptr N)
      {
        return static_cast<_Link_const_type>( N->_M_parent );
      }

      static void
      _S_set_parent(_Base_ptr N, _Base_ptr p)
      {
        N->_M_parent = p;
      }

      static void
      _S_set_left(_Base_ptr N, _Base_ptr l)
      {
        N->_M_left = l;
      }

      static _Link_type
      _S_left(_Base_ptr N)
      {
        return static_cast<_Link_type>( N->_M_left );
      }

      static _Link_const_type
      _S_left(_Base_const_ptr N)
      {
        return static_cast<_Link_const_type>( N->_M_left );
      }

      static void
      _S_set_right(_Base_ptr N, _Base_ptr r)
      {
        N->_M_right = r;
      }

      static _Link_type
      _S_right(_Base_ptr N)
      {
        return static_cast<_Link_type>( N->_M_right );
      }

      static _Link_const_type
      _S_right(_Base_const_ptr N)
      {
        return static_cast<_Link_const_type>( N->_M_right );
      }

      static bool
      _S_is_leaf(_Base_const_ptr N)
      {
        return !_S_left(N) && !_S_right(N);
      }

      static const_reference
      _S_value(_Link_const_type N)
      {
        return N->_M_value;
      }

      static const_reference
      _S_value(_Base_const_ptr N)
      {
        return static_cast<_Link_const_type>(N)->_M_value;
      }

      static _Link_const_type
      _S_minimum(_Link_const_type __X)
      {
        return static_cast<_Link_const_type> ( _Node_base::_S_minimum(__X) );
      }

      static _Link_const_type
      _S_maximum(_Link_const_type __X)
      {
        return static_cast<_Link_const_type>( _Node_base::_S_maximum(__X) );
      }

      _Link_type
      _M_new_node(const_reference __V, //  = value_type(),
                  _Base_ptr const __PARENT = NULL,
                  _Base_ptr const __LEFT = NULL,
                  _Base_ptr const __RIGHT = NULL)
      {
        _Link_type __ret = _Base::_M_allocate_node();
        try
          {
            _M_construct_node(__ret, __V, __PARENT, __LEFT, __RIGHT);
          }
        catch(...)
          {
            _M_deallocate_node(__ret);
            __throw_exception_again;
          }
        return __ret;
      }

      /* WHAT was this for?
      _Link_type
      _M_clone_node(_Link_const_type __X)
      {
        _Link_type __ret = _M_allocate_node(__X->_M_value);
        // TODO
        return __ret;
      }
      */

      void
      _M_delete_node(_Link_type __p)
      {
        _M_destroy_node(__p);
        _M_deallocate_node(__p);
      }

      _Link_type _M_header;
      size_type _M_count;
      _Acc _M_acc;

#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
      friend std::ostream&
      operator<<(std::ostream& o,
                    KDTree<__K, _Val, _Acc, _Cmp, _Alloc> const& tree) throw ()
    {
      o << "meta node:   " << *tree._M_header << std::endl;

      if (tree.empty())
        return o << "[empty " << __K << "d-tree " << &tree << "]";

      o << "nodes total: " << tree.size() << std::endl;
      o << "dimensions:  " << __K << std::endl;

      typedef KDTree<__K, _Val, _Acc, _Cmp, _Alloc> _Tree;
      typedef typename _Tree::_Link_type _Link_type;

      std::stack<_Link_const_type> s;
      s.push(tree._M_get_root());

      while (!s.empty())
        {
          _Link_const_type n = s.top();
          s.pop();
          o << *n << std::endl;
          if (_Tree::_S_left(n)) s.push(_Tree::_S_left(n));
          if (_Tree::_S_right(n)) s.push(_Tree::_S_right(n));
        }

      return o;
    }
#endif

  };


} // namespace KDTree

#endif // include guard

/* COPYRIGHT --
 *
 * This file is part of libkdtree++, a C++ template KD-Tree sorting container.
 * libkdtree++ is (c) 2004-2007 Martin F. Krafft <[e-mail]>
 * and Sylvain Bougerel <[e-mail]> distributed under the
 * terms of the Artistic License 2.0. See the ./COPYING file in the source tree
 * root for more information.
 * Parts of this file are (c) 2004-2007 Paul Harris <[e-mail]>.
 *
 * THIS PACKAGE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTIES
 * OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 */