-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathkdtree.hpp
1293 lines (1128 loc) · 41.8 KB
/
kdtree.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/** \file
* Defines the interface for the KDTree class.
*
* \author Martin F. Krafft <[email protected]>
*
* Paul Harris figured this stuff out (below)
* Notes:
* This is similar to a binary tree, but its not the same.
* There are a few important differences:
*
* * Each level is sorted by a different criteria (this is fundamental to the design).
*
* * It is possible to have children IDENTICAL to its parent in BOTH branches
* This is different to a binary tree, where identical children are always to the right
* So, KDTree has the relationships:
* * The left branch is <= its parent (in binary tree, this relationship is a plain < )
* * The right branch is <= its parent (same as binary tree)
*
* This is done for mostly for performance.
* Its a LOT easier to maintain a consistent tree if we use the <= relationship.
* Note that this relationship only makes a difference when searching for an exact
* item with find() or find_exact, other search, erase and insert functions don't notice
* the difference.
*
* In the case of binary trees, you can safely assume that the next identical item
* will be the child leaf,
* but in the case of KDTree, the next identical item might
* be a long way down a subtree, because of the various different sort criteria.
*
* So erase()ing a node from a KDTree could require serious and complicated
* tree rebalancing to maintain consistency... IF we required binary-tree-like relationships.
*
* This has no effect on insert()s, a < test is good enough to keep consistency.
*
* It has an effect on find() searches:
* * Instead of using compare(child,node) for a < relationship and following 1 branch,
* we must use !compare(node,child) for a <= relationship, and test BOTH branches, as
* we could potentially go down both branches.
*
* It has no real effect on bounds-based searches (like find_nearest, find_within_range)
* as it compares vs a boundary and would follow both branches if required.
*
* This has no real effect on erase()s, a < test is good enough to keep consistency.
*/
#ifndef INCLUDE_KDTREE_KDTREE_HPP
#define INCLUDE_KDTREE_KDTREE_HPP
//
// This number is guarenteed to change with every release.
//
// KDTREE_VERSION % 100 is the patch level
// KDTREE_VERSION / 100 % 1000 is the minor version
// KDTREE_VERSION / 100000 is the major version
#define KDTREE_VERSION 701
//
// KDTREE_LIB_VERSION must be defined to be the same as KDTREE_VERSION
// but as a *string* in the form "x_y[_z]" where x is the major version
// number, y is the minor version number, and z is the patch level if not 0.
#define KDTREE_LIB_VERSION "0_7_1"
#include <vector>
#ifdef KDTREE_CHECK_PERFORMANCE_COUNTERS
# include <map>
#endif
#include <algorithm>
#ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
# include <ostream>
# include <stack>
#endif
#ifdef KDTREE_SERIALIZATION
# include <boost/serialization/split_member.hpp>
# include <boost/serialization/string.hpp>
#endif
#include <cmath>
#include <cstddef>
#include <cassert>
#include "function.hpp"
#include "allocator.hpp"
#include "iterator.hpp"
#include "node.hpp"
#include "region.hpp"
namespace KDTree
{
#ifdef KDTREE_CHECK_PERFORMANCE
unsigned long long num_dist_calcs = 0;
#endif
template <size_t const __K, typename _Val,
typename _Acc = _Bracket_accessor<_Val>,
typename _Dist = squared_difference<typename _Acc::result_type,
typename _Acc::result_type>,
typename _Cmp = std::less<typename _Acc::result_type>,
typename _Alloc = std::allocator<_Node<_Val> > >
class KDTree : protected _Alloc_base<_Val, _Alloc>
{
protected:
typedef _Alloc_base<_Val, _Alloc> _Base;
typedef typename _Base::allocator_type allocator_type;
typedef _Node_base* _Base_ptr;
typedef _Node_base const* _Base_const_ptr;
typedef _Node<_Val>* _Link_type;
typedef _Node<_Val> const* _Link_const_type;
typedef _Node_compare<_Val, _Acc, _Cmp> _Node_compare_;
public:
typedef _Region<__K, _Val, typename _Acc::result_type, _Acc, _Cmp>
_Region_;
typedef _Val value_type;
typedef value_type* pointer;
typedef value_type const* const_pointer;
typedef value_type& reference;
typedef value_type const& const_reference;
typedef typename _Acc::result_type subvalue_type;
typedef typename _Dist::distance_type distance_type;
typedef size_t size_type;
typedef ptrdiff_t difference_type;
KDTree(_Acc const& __acc = _Acc(), _Dist const& __dist = _Dist(),
_Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
: _Base(__a), _M_header(),
_M_count(0), _M_acc(__acc), _M_cmp(__cmp), _M_dist(__dist)
{
_M_empty_initialise();
}
KDTree(const KDTree& __x)
: _Base(__x.get_allocator()), _M_header(), _M_count(0),
_M_acc(__x._M_acc), _M_cmp(__x._M_cmp), _M_dist(__x._M_dist)
{
_M_empty_initialise();
// this is slow:
// this->insert(begin(), __x.begin(), __x.end());
// this->optimise();
// this is much faster, as it skips a lot of useless work
// do the optimisation before inserting
// Needs to be stored in a vector first as _M_optimise()
// sorts the data in the passed iterators directly.
std::vector<value_type> temp;
temp.reserve(__x.size());
std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
_M_optimise(temp.begin(), temp.end(), 0);
}
template<typename _InputIterator>
KDTree(_InputIterator __first, _InputIterator __last,
_Acc const& acc = _Acc(), _Dist const& __dist = _Dist(),
_Cmp const& __cmp = _Cmp(), const allocator_type& __a = allocator_type())
: _Base(__a), _M_header(), _M_count(0),
_M_acc(acc), _M_cmp(__cmp), _M_dist(__dist)
{
_M_empty_initialise();
// this is slow:
// this->insert(begin(), __first, __last);
// this->optimise();
// this is much faster, as it skips a lot of useless work
// do the optimisation before inserting
// Needs to be stored in a vector first as _M_optimise()
// sorts the data in the passed iterators directly.
std::vector<value_type> temp;
temp.reserve(std::distance(__first,__last));
std::copy(__first,__last,std::back_inserter(temp));
_M_optimise(temp.begin(), temp.end(), 0);
// NOTE: this will BREAK users that are passing in
// read-once data via the iterator...
// We increment __first all the way to __last once within
// the distance() call, and again within the copy() call.
//
// This should end up using some funky C++ concepts or
// type traits to check that the iterators can be used in this way...
}
// this will CLEAR the tree and fill it with the contents
// of 'writable_vector'. it will use the passed vector directly,
// and will basically resort the vector many times over while
// optimising the tree.
//
// Paul: I use this when I have already built up a vector of data
// that I want to add, and I don't mind if its contents get shuffled
// by the kdtree optimise routine.
void efficient_replace_and_optimise( std::vector<value_type> & writable_vector )
{
this->clear();
_M_optimise(writable_vector.begin(), writable_vector.end(), 0);
}
KDTree&
operator=(const KDTree& __x)
{
if (this != &__x)
{
_M_acc = __x._M_acc;
_M_dist = __x._M_dist;
_M_cmp = __x._M_cmp;
// this is slow:
// this->insert(begin(), __x.begin(), __x.end());
// this->optimise();
// this is much faster, as it skips a lot of useless work
// do the optimisation before inserting
// Needs to be stored in a vector first as _M_optimise()
// sorts the data in the passed iterators directly.
std::vector<value_type> temp;
temp.reserve(__x.size());
std::copy(__x.begin(),__x.end(),std::back_inserter(temp));
efficient_replace_and_optimise(temp);
}
return *this;
}
~KDTree()
{
this->clear();
}
allocator_type
get_allocator() const
{
return _Base::get_allocator();
}
size_type
size() const
{
return _M_count;
}
size_type
max_size() const
{
return size_type(-1);
}
bool
empty() const
{
return this->size() == 0;
}
void
clear()
{
_M_erase_subtree(_M_get_root());
_M_set_leftmost(&_M_header);
_M_set_rightmost(&_M_header);
_M_set_root(NULL);
_M_count = 0;
}
/*! \brief Comparator for the values in the KDTree.
The comparator shall not be modified, it could invalidate the tree.
\return a copy of the comparator used by the KDTree.
*/
_Cmp
value_comp() const
{ return _M_cmp; }
/*! \brief Accessor to the value's elements.
This accessor shall not be modified, it could invalidate the tree.
\return a copy of the accessor used by the KDTree.
*/
_Acc
value_acc() const
{ return _M_acc; }
/*! \brief Distance calculator between 2 value's element.
This functor can be modified. It's modification will only affect the
behavior of the find and find_nearest functions.
\return a reference to the distance calculator used by the KDTree.
*/
const _Dist&
value_distance() const
{ return _M_dist; }
_Dist&
value_distance()
{ return _M_dist; }
// typedef _Iterator<_Val, reference, pointer> iterator;
typedef _Iterator<_Val, const_reference, const_pointer> const_iterator;
// No mutable iterator at this stage
typedef const_iterator iterator;
typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
typedef std::reverse_iterator<iterator> reverse_iterator;
// Note: the static_cast in end() is invalid (_M_header is not convertable to a _Link_type), but
// thats ok as it just means undefined behaviour if the user dereferences the end() iterator.
const_iterator begin() const { return const_iterator(_M_get_leftmost()); }
const_iterator end() const { return const_iterator(static_cast<_Link_const_type>(&_M_header)); }
const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }
iterator
insert(iterator /* ignored */, const_reference __V)
{
return this->insert(__V);
}
iterator
insert(const_reference __V)
{
if (!_M_get_root())
{
_Link_type __n = _M_new_node(__V, &_M_header);
++_M_count;
_M_set_root(__n);
_M_set_leftmost(__n);
_M_set_rightmost(__n);
return iterator(__n);
}
return _M_insert(_M_get_root(), __V, 0);
}
template <class _InputIterator>
void insert(_InputIterator __first, _InputIterator __last) {
for (; __first != __last; ++__first)
this->insert(*__first);
}
void
insert(iterator __pos, size_type __n, const value_type& __x)
{
for (; __n > 0; --__n)
this->insert(__pos, __x);
}
template<typename _InputIterator>
void
insert(iterator __pos, _InputIterator __first, _InputIterator __last) {
for (; __first != __last; ++__first)
this->insert(__pos, *__first);
}
// Note: this uses the find() to location the item you want to erase.
// find() compares by equivalence of location ONLY. See the comments
// above find_exact() for why you may not want this.
//
// If you want to erase ANY item that has the same location as __V,
// then use this function.
//
// If you want to erase a PARTICULAR item, and not any other item
// that might happen to have the same location, then you should use
// erase_exact().
void
erase(const_reference __V) {
const_iterator b = this->find(__V);
this->erase(b);
}
void
erase_exact(const_reference __V) {
this->erase(this->find_exact(__V));
}
// note: kept as const because its easier to const-cast it away
void
erase(const_iterator const& __IT)
{
assert(__IT != this->end());
_Link_const_type target = __IT.get_raw_node();
_Link_const_type n = target;
size_type level = 0;
while ((n = _S_parent(n)) != &_M_header)
++level;
_M_erase( const_cast<_Link_type>(target), level );
_M_delete_node( const_cast<_Link_type>(target) );
--_M_count;
}
/* this does not work since erasure changes sort order
void
erase(const_iterator __A, const_iterator const& __B)
{
if (0 && __A == this->begin() && __B == this->end())
{
this->clear();
}
else
{
while (__A != __B)
this->erase(__A++);
}
}
*/
// compares via equivalence
// so if you are looking for any item with the same location,
// according to the standard accessor comparisions,
// then this is the function for you.
template <class SearchVal>
const_iterator
find(SearchVal const& __V) const
{
if (!_M_get_root()) return this->end();
return _M_find(_M_get_root(), __V, 0);
}
// compares via equality
// if you are looking for a particular item in the tree,
// and (for example) it has an ID that is checked via an == comparison
// eg
// struct Item
// {
// size_type unique_id;
// bool operator==(Item const& a, Item const& b) { return a.unique_id == b.unique_id; }
// Location location;
// };
// Two items may be equivalent in location. find() would return
// either one of them. But no two items have the same ID, so
// find_exact() would always return the item with the same location AND id.
//
template <class SearchVal>
const_iterator
find_exact(SearchVal const& __V) const
{
if (!_M_get_root()) return this->end();
return _M_find_exact(_M_get_root(), __V, 0);
}
// NOTE: see notes on find_within_range().
size_type
count_within_range(const_reference __V, subvalue_type const __R) const
{
if (!_M_get_root()) return 0;
_Region_ __region(__V, __R, _M_acc, _M_cmp);
return this->count_within_range(__region);
}
size_type
count_within_range(_Region_ const& __REGION) const
{
if (!_M_get_root()) return 0;
_Region_ __bounds(__REGION);
return _M_count_within_range(_M_get_root(),
__REGION, __bounds, 0);
}
// NOTE: see notes on find_within_range().
template <typename SearchVal, class Visitor>
Visitor
visit_within_range(SearchVal const& V, subvalue_type const R, Visitor visitor) const
{
if (!_M_get_root()) return visitor;
_Region_ region(V, R, _M_acc, _M_cmp);
return this->visit_within_range(region, visitor);
}
template <class Visitor>
Visitor
visit_within_range(_Region_ const& REGION, Visitor visitor) const
{
if (_M_get_root())
{
_Region_ bounds(REGION);
return _M_visit_within_range(visitor, _M_get_root(), REGION, bounds, 0);
}
return visitor;
}
// NOTE: this will visit points based on 'Manhattan distance' aka city-block distance
// aka taxicab metric. Meaning it will find all points within:
// max(x_dist,max(y_dist,z_dist));
// AND NOT than what you would expect: sqrt(x_dist*x_dist + y_dist*y_dist + z_dist*z_dist)
//
// This is because it converts the distance into a bounding-box 'region' and compares
// against that.
//
// If you want the sqrt() behaviour, ask on the mailing list for different options.
//
template <typename SearchVal, typename _OutputIterator>
_OutputIterator
find_within_range(SearchVal const& val, subvalue_type const range,
_OutputIterator out) const
{
if (!_M_get_root()) return out;
_Region_ region(val, range, _M_acc, _M_cmp);
return this->find_within_range(region, out);
}
template <typename _OutputIterator>
_OutputIterator
find_within_range(_Region_ const& region,
_OutputIterator out) const
{
if (_M_get_root())
{
_Region_ bounds(region);
out = _M_find_within_range(out, _M_get_root(),
region, bounds, 0);
}
return out;
}
template <class SearchVal>
std::pair<const_iterator, distance_type>
find_nearest (SearchVal const& __val) const
{
if (_M_get_root())
{
std::pair<const _Node<_Val>*,
std::pair<size_type, typename _Acc::result_type> >
best = _S_node_nearest (__K, 0, __val,
_M_get_root(), &_M_header, _M_get_root(),
std::sqrt(_S_accumulate_node_distance
(__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val)),
_M_cmp, _M_acc, _M_dist,
always_true<value_type>());
return std::pair<const_iterator, distance_type>
(best.first, best.second.second);
}
return std::pair<const_iterator, distance_type>(end(), 0);
}
template <class SearchVal>
std::pair<const_iterator, distance_type>
find_nearest (SearchVal const& __val, distance_type __max) const
{
if (_M_get_root())
{
bool root_is_candidate = false;
const _Node<_Val>* node = _M_get_root();
{ // scope to ensure we don't use 'root_dist' anywhere else
distance_type root_dist = std::sqrt(_S_accumulate_node_distance
(__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
if (root_dist <= __max)
{
root_is_candidate = true;
__max = root_dist;
}
}
std::pair<const _Node<_Val>*,
std::pair<size_type, typename _Acc::result_type> >
best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
node, __max, _M_cmp, _M_acc, _M_dist,
always_true<value_type>());
// make sure we didn't just get stuck with the root node...
if (root_is_candidate || best.first != _M_get_root())
return std::pair<const_iterator, distance_type>
(best.first, best.second.second);
}
return std::pair<const_iterator, distance_type>(end(), __max);
}
template <class SearchVal, class _Predicate>
std::pair<const_iterator, distance_type>
find_nearest_if (SearchVal const& __val, distance_type __max,
_Predicate __p) const
{
if (_M_get_root())
{
bool root_is_candidate = false;
const _Node<_Val>* node = _M_get_root();
if (__p(_M_get_root()->_M_value))
{
{ // scope to ensure we don't use root_dist anywhere else
distance_type root_dist = std::sqrt(_S_accumulate_node_distance
(__K, _M_dist, _M_acc, _M_get_root()->_M_value, __val));
if (root_dist <= __max)
{
root_is_candidate = true;
root_dist = __max;
}
}
}
std::pair<const _Node<_Val>*,
std::pair<size_type, typename _Acc::result_type> >
best = _S_node_nearest (__K, 0, __val, _M_get_root(), &_M_header,
node, __max, _M_cmp, _M_acc, _M_dist, __p);
// make sure we didn't just get stuck with the root node...
if (root_is_candidate || best.first != _M_get_root())
return std::pair<const_iterator, distance_type>
(best.first, best.second.second);
}
return std::pair<const_iterator, distance_type>(end(), __max);
}
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_type const level, bool to_the_left )
{
assert(parent);
if (child)
{
_Node_compare_ compare(level % __K, _M_acc, _M_cmp);
// 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 || !compare(parent->_M_value,child->_M_value)); // check the left
assert(to_the_left || !compare(child->_M_value,parent->_M_value)); // 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_type 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_header._M_parent = NULL;
_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_type const __L)
{
if (_Node_compare_(__L % __K, _M_acc, _M_cmp)(__V, __N->_M_value))
{
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_type const level)
{
// 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())
_M_set_root(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_type const level)
{
// if 'node' is null, then we can't do any better
if (_S_is_leaf(node))
return NULL;
std::pair<_Link_type,size_type> 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_type>(_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_type>(_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, _M_cmp);
// compare the children based on this level's criteria...
// (this gives virtually random results)
if (compare(_S_right(node)->_M_value, _S_left(node)->_M_value))
// the right is smaller, get our replacement from the SMALLEST on the right
candidate = _M_get_j_min(std::pair<_Link_type,size_type>(_S_right(node),level), level+1);
else
candidate = _M_get_j_max( std::pair<_Link_type,size_type>(_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_type>
_M_get_j_min( std::pair<_Link_type,size_type> const node, size_type const level)
{
typedef std::pair<_Link_type,size_type> Result;
if (_S_is_leaf(node.first))
return Result(node.first,level);
_Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
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->_M_value, candidate.first->_M_value))
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->_M_value, candidate.first->_M_value))
candidate = right;
}
if (candidate.first == node.first)
return Result(candidate.first,level);
return candidate;
}
std::pair<_Link_type,size_type>
_M_get_j_max( std::pair<_Link_type,size_type> const node, size_type const level)
{
typedef std::pair<_Link_type,size_type> Result;
if (_S_is_leaf(node.first))
return Result(node.first,level);
_Node_compare_ compare(node.second % __K, _M_acc, _M_cmp);
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->_M_value, left.first->_M_value))
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->_M_value, right.first->_M_value))
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_type const level) const
{
// 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, _M_cmp);
if (!compare(node->_M_value,value)) // note, this is a <= test
{
// this line is the only difference between _M_find_exact() and _M_find()
if (_M_matches_node(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->_M_value)) // 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_type const level) const
{
// 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, _M_cmp);
if (!compare(node->_M_value,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->_M_value)) // 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_type const __L) const
{
_Node_compare_ compare(__L % __K, _M_acc, _M_cmp);
return !(compare(__N->_M_value, __V) || compare(__V, __N->_M_value));
}
bool
_M_matches_node_in_other_ds(_Link_const_type __N, const_reference __V,
size_type const __L = 0) const
{
size_type __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_type __L = 0) const
{
return _M_matches_node_in_d(__N, __V, __L)
&& _M_matches_node_in_other_ds(__N, __V, __L);
}
size_type
_M_count_within_range(_Link_const_type __N, _Region_ const& __REGION,
_Region_ const& __BOUNDS,
size_type const __L) const
{
size_type count = 0;
if (__REGION.encloses(_S_value(__N)))
{
++count;
}
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_type const L) const
{
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,