00001
00046 #ifndef INCLUDE_KDTREE_KDTREE_HPP
00047 #define INCLUDE_KDTREE_KDTREE_HPP
00048
00049
00050
00051
00052
00053
00054
00055
00056 #define KDTREE_VERSION 700
00057
00058
00059
00060
00061 #define KDTREE_LIB_VERSION "0_7_0"
00062
00063
00064 #include <vector>
00065
00066 #ifdef KDTREE_CHECK_PERFORMANCE_COUNTERS
00067 # include <map>
00068 #endif
00069 #include <algorithm>
00070
00071 #ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
00072 # include <ostream>
00073 # include <stack>
00074 #endif
00075
00076 #include <cmath>
00077 #include <cstddef>
00078 #include <cassert>
00079
00080 #include "function.hpp"
00081 #include "allocator.hpp"
00082 #include "iterator.hpp"
00083 #include "node.hpp"
00084 #include "region.hpp"
00085
00086 namespace KDTree
00087 {
00088
00089 #ifdef KDTREE_CHECK_PERFORMANCE
00090 unsigned long long num_dist_calcs = 0;
00091 #endif
00092
00093 template <size_t const __K, typename _Val,
00094 typename _Acc = _Bracket_accessor<_Val>,
00095 typename _Dist = squared_difference<typename _Acc::result_type,
00096 typename _Acc::result_type>,
00097 typename _Cmp = std::less<typename _Acc::result_type>,
00098 typename _Alloc = std::allocator<_Node<_Val> > >
00099 class KDTree : protected _Alloc_base<_Val, _Alloc>
00100 {
00101 protected:
00102 typedef _Alloc_base<_Val, _Alloc> _Base;
00103 typedef typename _Base::allocator_type allocator_type;
00104
00105 typedef _Node_base* _Base_ptr;
00106 typedef _Node_base const* _Base_const_ptr;
00107 typedef _Node<_Val>* _Link_type;
00108 typedef _Node<_Val> const* _Link_const_type;
00109
00110 typedef _Node_compare<_Val, _Acc, _Cmp> _Node_compare_;
00111
00112 public:
00113 typedef _Region<__K, _Val, typename _Acc::result_type, _Acc, _Cmp>
00114 _Region_;
00115 typedef _Val value_type;
00116 typedef value_type* pointer;
00117 typedef value_type const* const_pointer;
00118 typedef value_type& reference;
00119 typedef value_type const& const_reference;
00120 typedef typename _Acc::result_type subvalue_type;
00121 typedef typename _Dist::distance_type distance_type;
00122 typedef size_t size_type;
00123 typedef ptrdiff_t difference_type;
00124
00125 KDTree(_Acc const& __acc = _Acc(), _Dist const& __dist = _Dist(),
00126 _Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
00127 : _Base(__a), _M_header(),
00128 _M_count(0), _M_acc(__acc), _M_cmp(__cmp), _M_dist(__dist)
00129 {
00130 _M_empty_initialise();
00131 }
00132
00133 KDTree(const KDTree& __x)
00134 : _Base(__x.get_allocator()), _M_header(), _M_count(0),
00135 _M_acc(__x._M_acc), _M_cmp(__x._M_cmp), _M_dist(__x._M_dist)
00136 {
00137 _M_empty_initialise();
00138
00139
00140
00141
00142
00143
00144
00145
00146 std::vector<value_type> temp;
00147 temp.reserve(__x.size());
00148 std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
00149 _M_optimise(temp.begin(), temp.end(), 0);
00150 }
00151
00152 template<typename _InputIterator>
00153 KDTree(_InputIterator __first, _InputIterator __last,
00154 _Acc const& acc = _Acc(), _Dist const& __dist = _Dist(),
00155 _Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
00156 : _Base(__a), _M_header(), _M_count(0),
00157 _M_acc(acc), _M_cmp(__cmp), _M_dist(__dist)
00158 {
00159 _M_empty_initialise();
00160
00161
00162
00163
00164
00165
00166
00167
00168 std::vector<value_type> temp;
00169 temp.reserve(std::distance(__first,__last));
00170 std::copy(__first,__last,std::back_inserter(temp));
00171 _M_optimise(temp.begin(), temp.end(), 0);
00172
00173
00174
00175
00176
00177
00178
00179
00180 }
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191 void efficient_replace_and_optimise( std::vector<value_type> & writable_vector )
00192 {
00193 this->clear();
00194 _M_optimise(writable_vector.begin(), writable_vector.end(), 0);
00195 }
00196
00197
00198
00199 KDTree&
00200 operator=(const KDTree& __x)
00201 {
00202 if (this != &__x)
00203 {
00204 _M_acc = __x._M_acc;
00205 _M_dist = __x._M_dist;
00206 _M_cmp = __x._M_cmp;
00207
00208
00209
00210
00211
00212
00213
00214
00215 std::vector<value_type> temp;
00216 temp.reserve(__x.size());
00217 std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
00218 efficient_replace_and_optimise(temp);
00219 }
00220 return *this;
00221 }
00222
00223 ~KDTree()
00224 {
00225 this->clear();
00226 }
00227
00228 allocator_type
00229 get_allocator() const
00230 {
00231 return _Base::get_allocator();
00232 }
00233
00234 size_type
00235 size() const
00236 {
00237 return _M_count;
00238 }
00239
00240 size_type
00241 max_size() const
00242 {
00243 return size_type(-1);
00244 }
00245
00246 bool
00247 empty() const
00248 {
00249 return this->size() == 0;
00250 }
00251
00252 void
00253 clear()
00254 {
00255 _M_erase_subtree(_M_get_root());
00256 _M_set_leftmost(&_M_header);
00257 _M_set_rightmost(&_M_header);
00258 _M_set_root(NULL);
00259 _M_count = 0;
00260 }
00261
00267 _Cmp
00268 value_comp() const
00269 { return _M_cmp; }
00270
00276 _Acc
00277 value_acc() const
00278 { return _M_acc; }
00279
00286 const _Dist&
00287 value_distance() const
00288 { return _M_dist; }
00289
00290 _Dist&
00291 value_distance()
00292 { return _M_dist; }
00293
00294
00295 typedef _Iterator<_Val, const_reference, const_pointer> const_iterator;
00296
00297 typedef const_iterator iterator;
00298 typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
00299 typedef std::reverse_iterator<iterator> reverse_iterator;
00300
00301
00302
00303
00304 const_iterator begin() const { return const_iterator(_M_get_leftmost()); }
00305 const_iterator end() const { return const_iterator(static_cast<_Link_const_type>(&_M_header)); }
00306 const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
00307 const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
00308
00309 iterator
00310 insert(iterator , const_reference __V)
00311 {
00312 return this->insert(__V);
00313 }
00314
00315 iterator
00316 insert(const_reference __V)
00317 {
00318 if (!_M_get_root())
00319 {
00320 _Link_type __n = _M_new_node(__V, &_M_header);
00321 ++_M_count;
00322 _M_set_root(__n);
00323 _M_set_leftmost(__n);
00324 _M_set_rightmost(__n);
00325 return iterator(__n);
00326 }
00327 return _M_insert(_M_get_root(), __V, 0);
00328 }
00329
00330 template <class _InputIterator>
00331 void insert(_InputIterator __first, _InputIterator __last) {
00332 for (; __first != __last; ++__first)
00333 this->insert(*__first);
00334 }
00335
00336 void
00337 insert(iterator __pos, size_type __n, const value_type& __x)
00338 {
00339 for (; __n > 0; --__n)
00340 this->insert(__pos, __x);
00341 }
00342
00343 template<typename _InputIterator>
00344 void
00345 insert(iterator __pos, _InputIterator __first, _InputIterator __last) {
00346 for (; __first != __last; ++__first)
00347 this->insert(__pos, *__first);
00348 }
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360 void
00361 erase(const_reference __V) {
00362 const_iterator b = this->find(__V);
00363 this->erase(b);
00364 }
00365
00366 void
00367 erase_exact(const_reference __V) {
00368 this->erase(this->find_exact(__V));
00369 }
00370
00371
00372 void
00373 erase(const_iterator const& __IT)
00374 {
00375 assert(__IT != this->end());
00376 _Link_const_type target = __IT.get_raw_node();
00377 _Link_const_type n = target;
00378 size_type level = 0;
00379 while ((n = _S_parent(n)) != &_M_header)
00380 ++level;
00381 _M_erase( const_cast<_Link_type>(target), level );
00382 _M_delete_node( const_cast<_Link_type>(target) );
00383 --_M_count;
00384 }
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406 template <class SearchVal>
00407 const_iterator
00408 find(SearchVal const& __V) const
00409 {
00410 if (!_M_get_root()) return this->end();
00411 return _M_find(_M_get_root(), __V, 0);
00412 }
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 template <class SearchVal>
00429 const_iterator
00430 find_exact(SearchVal const& __V) const
00431 {
00432 if (!_M_get_root()) return this->end();
00433 return _M_find_exact(_M_get_root(), __V, 0);
00434 }
00435
00436 size_type
00437 count_within_range(const_reference __V, subvalue_type const __R) const
00438 {
00439 if (!_M_get_root()) return 0;
00440 _Region_ __region(__V, __R, _M_acc, _M_cmp);
00441 return this->count_within_range(__region);
00442 }
00443
00444 size_type
00445 count_within_range(_Region_ const& __REGION) const
00446 {
00447 if (!_M_get_root()) return 0;
00448
00449 _Region_ __bounds(__REGION);
00450 return _M_count_within_range(_M_get_root(),
00451 __REGION, __bounds, 0);
00452 }
00453
00454 template <typename SearchVal, class Visitor>
00455 Visitor
00456 visit_within_range(SearchVal const& V, subvalue_type const R, Visitor visitor) const
00457 {
00458 if (!_M_get_root()) return visitor;
00459 _Region_ region(V, R, _M_acc, _M_cmp);
00460 return this->visit_within_range(region, visitor);
00461 }
00462
00463 template <class Visitor>
00464 Visitor
00465 visit_within_range(_Region_ const& REGION, Visitor visitor) const
00466 {
00467 if (_M_get_root())
00468 {
00469 _Region_ bounds(REGION);
00470 return _M_visit_within_range(visitor, _M_get_root(), REGION, bounds, 0);
00471 }
00472 return visitor;
00473 }
00474
00475 const_iterator
00476 find_within_range_iterative(const_reference __a, const_reference __b)
00477 {
00478 return const_iterator(begin());
00479 }
00480
00481 template <typename SearchVal, typename _OutputIterator>
00482 _OutputIterator
00483 find_within_range(SearchVal const& val, subvalue_type const range,
00484 _OutputIterator out) const
00485 {
00486 if (!_M_get_root()) return out;
00487 _Region_ region(val, range, _M_acc, _M_cmp);
00488 return this->find_within_range(region, out);
00489 }
00490
00491 template <typename _OutputIterator>
00492 _OutputIterator
00493 find_within_range(_Region_ const& region,
00494 _OutputIterator out) const
00495 {
00496 if (_M_get_root())
00497 {
00498 _Region_ bounds(region);
00499 out = _M_find_within_range(out, _M_get_root(),
00500 region, bounds, 0);
00501 }
00502 return out;
00503 }
00504
00505 template <class SearchVal>
00506 std::pair<const_iterator, distance_type>
00507 find_nearest (SearchVal const& __val) const
00508 {
00509 if (_M_get_root())
00510 {
00511 std::pair<const _Node<_Val>*,
00512 std::pair<size_type, typename _Acc::result_type> >
00513 best = _S_node_nearest (__K, 0, __val,
00514 _M_get_root(), &_M_header, _M_get_root(),
00515 sqrt(_S_accumulate_node_distance
00516 (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val)),
00517 _M_cmp, _M_acc, _M_dist,
00518 always_true<value_type>());
00519 return std::pair<const_iterator, distance_type>
00520 (best.first, best.second.second);
00521 }
00522 return std::pair<const_iterator, distance_type>(end(), 0);
00523 }
00524
00525 template <class SearchVal>
00526 std::pair<const_iterator, distance_type>
00527 find_nearest (SearchVal const& __val, distance_type __max) const
00528 {
00529 if (_M_get_root())
00530 {
00531 bool root_is_candidate = false;
00532 const _Node<_Val>* node = _M_get_root();
00533 {
00534 distance_type root_dist = sqrt(_S_accumulate_node_distance
00535 (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
00536 if (root_dist <= __max)
00537 {
00538 root_is_candidate = true;
00539 __max = root_dist;
00540 }
00541 }
00542 std::pair<const _Node<_Val>*,
00543 std::pair<size_type, typename _Acc::result_type> >
00544 best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
00545 node, __max, _M_cmp, _M_acc, _M_dist,
00546 always_true<value_type>());
00547
00548 if (root_is_candidate || best.first != _M_get_root())
00549 return std::pair<const_iterator, distance_type>
00550 (best.first, best.second.second);
00551 }
00552 return std::pair<const_iterator, distance_type>(end(), __max);
00553 }
00554
00555 template <class SearchVal, class _Predicate>
00556 std::pair<const_iterator, distance_type>
00557 find_nearest_if (SearchVal const& __val, distance_type __max,
00558 _Predicate __p) const
00559 {
00560 if (_M_get_root())
00561 {
00562 bool root_is_candidate = false;
00563 const _Node<_Val>* node = _M_get_root();
00564 if (__p(_M_get_root()->_M_value))
00565 {
00566 {
00567 distance_type root_dist = sqrt(_S_accumulate_node_distance
00568 (__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
00569 if (root_dist <= __max)
00570 {
00571 root_is_candidate = true;
00572 root_dist = __max;
00573 }
00574 }
00575 }
00576 std::pair<const _Node<_Val>*,
00577 std::pair<size_type, typename _Acc::result_type> >
00578 best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
00579 node, __max, _M_cmp, _M_acc, _M_dist, __p);
00580
00581 if (root_is_candidate || best.first != _M_get_root())
00582 return std::pair<const_iterator, distance_type>
00583 (best.first, best.second.second);
00584 }
00585 return std::pair<const_iterator, distance_type>(end(), __max);
00586 }
00587
00588 void
00589 optimise()
00590 {
00591 std::vector<value_type> __v(this->begin(),this->end());
00592 this->clear();
00593 _M_optimise(__v.begin(), __v.end(), 0);
00594 }
00595
00596 void
00597 optimize()
00598 {
00599 this->optimise();
00600 }
00601
00602 void check_tree()
00603 {
00604 _M_check_node(_M_get_root(),0);
00605 }
00606
00607 protected:
00608
00609 void _M_check_children( _Link_const_type child, _Link_const_type parent, size_type const level, bool to_the_left )
00610 {
00611 assert(parent);
00612 if (child)
00613 {
00614 _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
00615
00616
00617
00618 assert(!to_the_left || !compare(parent->_M_value,child->_M_value));
00619 assert(to_the_left || !compare(child->_M_value,parent->_M_value));
00620
00621 _M_check_children(_S_left(child),parent,level,to_the_left);
00622 _M_check_children(_S_right(child),parent,level,to_the_left);
00623 }
00624 }
00625
00626 void _M_check_node( _Link_const_type node, size_type const level )
00627 {
00628 if (node)
00629 {
00630
00631
00632 _M_check_children( _S_left(node), node, level, true );
00633
00634 _M_check_children( _S_right(node), node, level, false );
00635
00636 _M_check_node( _S_left(node), level+1 );
00637 _M_check_node( _S_right(node), level+1 );
00638 }
00639 }
00640
00641 void _M_empty_initialise()
00642 {
00643 _M_set_leftmost(&_M_header);
00644 _M_set_rightmost(&_M_header);
00645 _M_header._M_parent = NULL;
00646 _M_set_root(NULL);
00647 }
00648
00649 iterator
00650 _M_insert_left(_Link_type __N, const_reference __V)
00651 {
00652 _S_set_left(__N, _M_new_node(__V)); ++_M_count;
00653 _S_set_parent( _S_left(__N), __N );
00654 if (__N == _M_get_leftmost())
00655 _M_set_leftmost( _S_left(__N) );
00656 return iterator(_S_left(__N));
00657 }
00658
00659 iterator
00660 _M_insert_right(_Link_type __N, const_reference __V)
00661 {
00662 _S_set_right(__N, _M_new_node(__V)); ++_M_count;
00663 _S_set_parent( _S_right(__N), __N );
00664 if (__N == _M_get_rightmost())
00665 _M_set_rightmost( _S_right(__N) );
00666 return iterator(_S_right(__N));
00667 }
00668
00669 iterator
00670 _M_insert(_Link_type __N, const_reference __V,
00671 size_type const __L)
00672 {
00673 if (_Node_compare_(__L % __K, _M_acc, _M_cmp)(__V, __N->_M_value))
00674 {
00675 if (!_S_left(__N))
00676 return _M_insert_left(__N, __V);
00677 return _M_insert(_S_left(__N), __V, __L+1);
00678 }
00679 else
00680 {
00681 if (!_S_right(__N) || __N == _M_get_rightmost())
00682 return _M_insert_right(__N, __V);
00683 return _M_insert(_S_right(__N), __V, __L+1);
00684 }
00685 }
00686
00687 _Link_type
00688 _M_erase(_Link_type dead_dad, size_type const level)
00689 {
00690
00691 _Link_type step_dad = _M_get_erase_replacement(dead_dad, level);
00692
00693
00694 if (dead_dad == _M_get_root())
00695 _M_set_root(step_dad);
00696 else if (_S_left(_S_parent(dead_dad)) == dead_dad)
00697 _S_set_left(_S_parent(dead_dad), step_dad);
00698 else
00699 _S_set_right(_S_parent(dead_dad), step_dad);
00700
00701
00702
00703
00704 if (dead_dad == _M_get_leftmost())
00705 _M_set_leftmost( (step_dad ? step_dad : _S_parent(dead_dad)) );
00706 if (dead_dad == _M_get_rightmost())
00707 _M_set_rightmost( (step_dad ? step_dad : _S_parent(dead_dad)) );
00708
00709 if (step_dad)
00710 {
00711
00712 _S_set_parent(step_dad, _S_parent(dead_dad));
00713
00714
00715 if (_S_left(dead_dad))
00716 _S_set_parent(_S_left(dead_dad), step_dad);
00717 if (_S_right(dead_dad))
00718 _S_set_parent(_S_right(dead_dad), step_dad);
00719
00720
00721 _S_set_left(step_dad, _S_left(dead_dad));
00722 _S_set_right(step_dad, _S_right(dead_dad));
00723 }
00724
00725 return step_dad;
00726 }
00727
00728
00729
00730 _Link_type
00731 _M_get_erase_replacement(_Link_type node, size_type const level)
00732 {
00733
00734 if (_S_is_leaf(node))
00735 return NULL;
00736
00737 std::pair<_Link_type,size_type> candidate;
00738
00739 if (!_S_left(node))
00740 candidate = _M_get_j_min( std::pair<_Link_type,size_type>(_S_right(node),level), level+1);
00741
00742 else if ((!_S_right(node)))
00743 candidate = _M_get_j_max( std::pair<_Link_type,size_type>(_S_left(node),level), level+1);
00744
00745 else
00746 {
00747
00748
00749
00750
00751
00752
00753 _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
00754
00755
00756 if (compare(_S_right(node)->_M_value, _S_left(node)->_M_value))
00757
00758 candidate = _M_get_j_min(std::pair<_Link_type,size_type>(_S_right(node),level), level+1);
00759 else
00760 candidate = _M_get_j_max( std::pair<_Link_type,size_type>(_S_left(node),level), level+1);
00761 }
00762
00763
00764
00765
00766 _Link_type parent = _S_parent(candidate.first);
00767 if (_S_left(parent) == candidate.first)
00768 _S_set_left(parent, _M_erase(candidate.first, candidate.second));
00769 else
00770 _S_set_right(parent, _M_erase(candidate.first, candidate.second));
00771
00772 return candidate.first;
00773 }
00774
00775
00776
00777 std::pair<_Link_type,size_type>
00778 _M_get_j_min( std::pair<_Link_type,size_type> const node, size_type const level)
00779 {
00780 typedef std::pair<_Link_type,size_type> Result;
00781 if (_S_is_leaf(node.first))
00782 return Result(node.first,level);
00783
00784 _Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
00785 Result candidate = node;
00786 if (_S_left(node.first))
00787 {
00788 Result left = _M_get_j_min(Result(_S_left(node.first), node.second), level+1);
00789 if (compare(left.first->_M_value, candidate.first->_M_value))
00790 candidate = left;
00791 }
00792 if (_S_right(node.first))
00793 {
00794 Result right = _M_get_j_min( Result(_S_right(node.first),node.second), level+1);
00795 if (compare(right.first->_M_value, candidate.first->_M_value))
00796 candidate = right;
00797 }
00798 if (candidate.first == node.first)
00799 return Result(candidate.first,level);
00800
00801 return candidate;
00802 }
00803
00804
00805
00806 std::pair<_Link_type,size_type>
00807 _M_get_j_max( std::pair<_Link_type,size_type> const node, size_type const level)
00808 {
00809 typedef std::pair<_Link_type,size_type> Result;
00810
00811 if (_S_is_leaf(node.first))
00812 return Result(node.first,level);
00813
00814 _Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
00815 Result candidate = node;
00816 if (_S_left(node.first))
00817 {
00818 Result left = _M_get_j_max( Result(_S_left(node.first),node.second), level+1);
00819 if (compare(candidate.first->_M_value, left.first->_M_value))
00820 candidate = left;
00821 }
00822 if (_S_right(node.first))
00823 {
00824 Result right = _M_get_j_max(Result(_S_right(node.first),node.second), level+1);
00825 if (compare(candidate.first->_M_value, right.first->_M_value))
00826 candidate = right;
00827 }
00828
00829 if (candidate.first == node.first)
00830 return Result(candidate.first,level);
00831
00832 return candidate;
00833 }
00834
00835
00836 void
00837 _M_erase_subtree(_Link_type __n)
00838 {
00839 while (__n)
00840 {
00841 _M_erase_subtree(_S_right(__n));
00842 _Link_type __t = _S_left(__n);
00843 _M_delete_node(__n);
00844 __n = __t;
00845 }
00846 }
00847
00848 const_iterator
00849 _M_find(_Link_const_type node, const_reference value, size_type const level) const
00850 {
00851
00852
00853
00854
00855 const_iterator found = this->end();
00856
00857 _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
00858 if (!compare(node->_M_value,value))
00859 {
00860
00861 if (_M_matches_node(node, value, level))
00862 return const_iterator(node);
00863 if (_S_left(node))
00864 found = _M_find(_S_left(node), value, level+1);
00865 }
00866 if ( _S_right(node) && found == this->end() && !compare(value,node->_M_value))
00867 found = _M_find(_S_right(node), value, level+1);
00868 return found;
00869 }
00870
00871 const_iterator
00872 _M_find_exact(_Link_const_type node, const_reference value, size_type const level) const
00873 {
00874
00875
00876
00877
00878 const_iterator found = this->end();
00879
00880 _Node_compare_ compare(level % __K, _M_acc, _M_cmp);
00881 if (!compare(node->_M_value,value))
00882 {
00883
00884 if (value == *const_iterator(node))
00885 return const_iterator(node);
00886 if (_S_left(node))
00887 found = _M_find_exact(_S_left(node), value, level+1);
00888 }
00889
00890
00891 if ( _S_right(node) && found == this->end() && !compare(value,node->_M_value))
00892 found = _M_find_exact(_S_right(node), value, level+1);
00893 return found;
00894 }
00895
00896 bool
00897 _M_matches_node_in_d(_Link_const_type __N, const_reference __V,
00898 size_type const __L) const
00899 {
00900 _Node_compare_ compare(__L % __K, _M_acc, _M_cmp);
00901 return !(compare(__N->_M_value, __V) || compare(__V, __N->_M_value));
00902 }
00903
00904 bool
00905 _M_matches_node_in_other_ds(_Link_const_type __N, const_reference __V,
00906 size_type const __L = 0) const
00907 {
00908 size_type __i = __L;
00909 while ((__i = (__i + 1) % __K) != __L % __K)
00910 if (!_M_matches_node_in_d(__N, __V, __i)) return false;
00911 return true;
00912 }
00913
00914 bool
00915 _M_matches_node(_Link_const_type __N, const_reference __V,
00916 size_type __L = 0) const
00917 {
00918 return _M_matches_node_in_d(__N, __V, __L)
00919 && _M_matches_node_in_other_ds(__N, __V, __L);
00920 }
00921
00922 size_type
00923 _M_count_within_range(_Link_const_type __N, _Region_ const& __REGION,
00924 _Region_ const& __BOUNDS,
00925 size_type const __L) const
00926 {
00927 size_type count = 0;
00928 if (__REGION.encloses(_S_value(__N)))
00929 {
00930 ++count;
00931 }
00932 if (_S_left(__N))
00933 {
00934 _Region_ __bounds(__BOUNDS);
00935 __bounds.set_high_bound(_S_value(__N), __L);
00936 if (__REGION.intersects_with(__bounds))
00937 count += _M_count_within_range(_S_left(__N),
00938 __REGION, __bounds, __L+1);
00939 }
00940 if (_S_right(__N))
00941 {
00942 _Region_ __bounds(__BOUNDS);
00943 __bounds.set_low_bound(_S_value(__N), __L);
00944 if (__REGION.intersects_with(__bounds))
00945 count += _M_count_within_range(_S_right(__N),
00946 __REGION, __bounds, __L+1);
00947 }
00948
00949 return count;
00950 }
00951
00952
00953 template <class Visitor>
00954 Visitor
00955 _M_visit_within_range(Visitor visitor,
00956 _Link_const_type N, _Region_ const& REGION,
00957 _Region_ const& BOUNDS,
00958 size_type const L) const
00959 {
00960 if (REGION.encloses(_S_value(N)))
00961 {
00962 visitor(_S_value(N));
00963 }
00964 if (_S_left(N))
00965 {
00966 _Region_ bounds(BOUNDS);
00967 bounds.set_high_bound(_S_value(N), L);
00968 if (REGION.intersects_with(bounds))
00969 visitor = _M_visit_within_range(visitor, _S_left(N),
00970 REGION, bounds, L+1);
00971 }
00972 if (_S_right(N))
00973 {
00974 _Region_ bounds(BOUNDS);
00975 bounds.set_low_bound(_S_value(N), L);
00976 if (REGION.intersects_with(bounds))
00977 visitor = _M_visit_within_range(visitor, _S_right(N),
00978 REGION, bounds, L+1);
00979 }
00980
00981 return visitor;
00982 }
00983
00984
00985
00986 template <typename _OutputIterator>
00987 _OutputIterator
00988 _M_find_within_range(_OutputIterator out,
00989 _Link_const_type __N, _Region_ const& __REGION,
00990 _Region_ const& __BOUNDS,
00991 size_type const __L) const
00992 {
00993 if (__REGION.encloses(_S_value(__N)))
00994 {
00995 *out++ = _S_value(__N);
00996 }
00997 if (_S_left(__N))
00998 {
00999 _Region_ __bounds(__BOUNDS);
01000 __bounds.set_high_bound(_S_value(__N), __L);
01001 if (__REGION.intersects_with(__bounds))
01002 out = _M_find_within_range(out, _S_left(__N),
01003 __REGION, __bounds, __L+1);
01004 }
01005 if (_S_right(__N))
01006 {
01007 _Region_ __bounds(__BOUNDS);
01008 __bounds.set_low_bound(_S_value(__N), __L);
01009 if (__REGION.intersects_with(__bounds))
01010 out = _M_find_within_range(out, _S_right(__N),
01011 __REGION, __bounds, __L+1);
01012 }
01013
01014 return out;
01015 }
01016
01017
01018 template <typename _Iter>
01019 void
01020 _M_optimise(_Iter const& __A, _Iter const& __B,
01021 size_type const __L)
01022 {
01023 if (__A == __B) return;
01024 _Node_compare_ compare(__L % __K, _M_acc, _M_cmp);
01025 _Iter __m = __A + (__B - __A) / 2;
01026 std::nth_element(__A, __m, __B, compare);
01027 this->insert(*__m);
01028 if (__m != __A) _M_optimise(__A, __m, __L+1);
01029 if (++__m != __B) _M_optimise(__m, __B, __L+1);
01030 }
01031
01032 _Link_const_type
01033 _M_get_root() const
01034 {
01035 return const_cast<_Link_const_type>(_M_root);
01036 }
01037
01038 _Link_type
01039 _M_get_root()
01040 {
01041 return _M_root;
01042 }
01043
01044 void _M_set_root(_Link_type n)
01045 {
01046 _M_root = n;
01047 }
01048
01049 _Link_const_type
01050 _M_get_leftmost() const
01051 {
01052 return static_cast<_Link_type>(_M_header._M_left);
01053 }
01054
01055 void
01056 _M_set_leftmost( _Node_base * a )
01057 {
01058 _M_header._M_left = a;
01059 }
01060
01061 _Link_const_type
01062 _M_get_rightmost() const
01063 {
01064 return static_cast<_Link_type>( _M_header._M_right );
01065 }
01066
01067 void
01068 _M_set_rightmost( _Node_base * a )
01069 {
01070 _M_header._M_right = a;
01071 }
01072
01073 static _Link_type
01074 _S_parent(_Base_ptr N)
01075 {
01076 return static_cast<_Link_type>( N->_M_parent );
01077 }
01078
01079 static _Link_const_type
01080 _S_parent(_Base_const_ptr N)
01081 {
01082 return static_cast<_Link_const_type>( N->_M_parent );
01083 }
01084
01085 static void
01086 _S_set_parent(_Base_ptr N, _Base_ptr p)
01087 {
01088 N->_M_parent = p;
01089 }
01090
01091 static void
01092 _S_set_left(_Base_ptr N, _Base_ptr l)
01093 {
01094 N->_M_left = l;
01095 }
01096
01097 static _Link_type
01098 _S_left(_Base_ptr N)
01099 {
01100 return static_cast<_Link_type>( N->_M_left );
01101 }
01102
01103 static _Link_const_type
01104 _S_left(_Base_const_ptr N)
01105 {
01106 return static_cast<_Link_const_type>( N->_M_left );
01107 }
01108
01109 static void
01110 _S_set_right(_Base_ptr N, _Base_ptr r)
01111 {
01112 N->_M_right = r;
01113 }
01114
01115 static _Link_type
01116 _S_right(_Base_ptr N)
01117 {
01118 return static_cast<_Link_type>( N->_M_right );
01119 }
01120
01121 static _Link_const_type
01122 _S_right(_Base_const_ptr N)
01123 {
01124 return static_cast<_Link_const_type>( N->_M_right );
01125 }
01126
01127 static bool
01128 _S_is_leaf(_Base_const_ptr N)
01129 {
01130 return !_S_left(N) && !_S_right(N);
01131 }
01132
01133 static const_reference
01134 _S_value(_Link_const_type N)
01135 {
01136 return N->_M_value;
01137 }
01138
01139 static const_reference
01140 _S_value(_Base_const_ptr N)
01141 {
01142 return static_cast<_Link_const_type>(N)->_M_value;
01143 }
01144
01145 static _Link_const_type
01146 _S_minimum(_Link_const_type __X)
01147 {
01148 return static_cast<_Link_const_type> ( _Node_base::_S_minimum(__X) );
01149 }
01150
01151 static _Link_const_type
01152 _S_maximum(_Link_const_type __X)
01153 {
01154 return static_cast<_Link_const_type>( _Node_base::_S_maximum(__X) );
01155 }
01156
01157
01158 _Link_type
01159 _M_new_node(const_reference __V,
01160 _Base_ptr const __PARENT = NULL,
01161 _Base_ptr const __LEFT = NULL,
01162 _Base_ptr const __RIGHT = NULL)
01163 {
01164 typename _Base::NoLeakAlloc noleak(this);
01165 _Link_type new_node = noleak.get();
01166 _M_construct_node(new_node, __V, __PARENT, __LEFT, __RIGHT);
01167 noleak.disconnect();
01168 return new_node;
01169 }
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
01180
01181 void
01182 _M_delete_node(_Link_type __p)
01183 {
01184 _M_destroy_node(__p);
01185 _M_deallocate_node(__p);
01186 }
01187
01188 _Link_type _M_root;
01189 _Node_base _M_header;
01190 size_type _M_count;
01191 _Acc _M_acc;
01192 _Cmp _M_cmp;
01193 _Dist _M_dist;
01194
01195 #ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
01196 friend std::ostream&
01197 operator<<(std::ostream& o,
01198 KDTree<__K, _Val, _Acc, _Dist, _Cmp, _Alloc> const& tree)
01199 {
01200 o << "meta node: " << tree._M_header << std::endl;
01201 o << "root node: " << tree._M_root << std::endl;
01202
01203 if (tree.empty())
01204 return o << "[empty " << __K << "d-tree " << &tree << "]";
01205
01206 o << "nodes total: " << tree.size() << std::endl;
01207 o << "dimensions: " << __K << std::endl;
01208
01209 typedef KDTree<__K, _Val, _Acc, _Dist, _Cmp, _Alloc> _Tree;
01210 typedef typename _Tree::_Link_type _Link_type;
01211
01212 std::stack<_Link_const_type> s;
01213 s.push(tree._M_get_root());
01214
01215 while (!s.empty())
01216 {
01217 _Link_const_type n = s.top();
01218 s.pop();
01219 o << *n << std::endl;
01220 if (_Tree::_S_left(n)) s.push(_Tree::_S_left(n));
01221 if (_Tree::_S_right(n)) s.push(_Tree::_S_right(n));
01222 }
01223
01224 return o;
01225 }
01226 #endif
01227
01228 };
01229
01230
01231 }
01232
01233 #endif // include guard
01234
01235
01236
01237
01238
01239
01240
01241
01242
01243
01244
01245
01246
01247