Horizon
rtree.h
1//TITLE
2//
3// R-TREES: A DYNAMIC INDEX STRUCTURE FOR SPATIAL SEARCHING
4//
5//DESCRIPTION
6//
7// A C++ templated version of the RTree algorithm.
8// For more information please read the comments in RTree.h
9//
10//AUTHORS
11//
12// * 1983 Original algorithm and test code by Antonin Guttman and Michael Stonebraker, UC Berkely
13// * 1994 ANCI C ported from original test code by Melinda Green - melinda@superliminal.com
14// * 1995 Sphere volume fix for degeneracy problem submitted by Paul Brook
15// * 2004 Templated C++ port by Greg Douglas
16// * 2013 CERN (www.cern.ch)
17//
18//LICENSE:
19//
20// Entirely free for all uses. Enjoy!
21
22#ifndef RTREE_H
23#define RTREE_H
24
25// NOTE This file compiles under MSVC 6 SP5 and MSVC .Net 2003 it may not work on other compilers without modification.
26
27// NOTE These next few lines may be win32 specific, you may need to modify them to compile on other platform
28#include <stdio.h>
29#include <math.h>
30#include <assert.h>
31#include <stdlib.h>
32
33#include <algorithm>
34#include <functional>
35
36#define ASSERT assert // RTree uses ASSERT( condition )
37
38//
39// RTree.h
40//
41
42#define RTREE_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
43 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES>
44#define RTREE_SEARCH_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
45 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES, class VISITOR>
46#define RTREE_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
47 TMINNODES>
48#define RTREE_SEARCH_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
49 TMINNODES, VISITOR>
50
51#define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one.
52#define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems
53
54// Fwd decl
55class RTFileStream; // File I/O helper class, look below for implementation and notes.
56
57
74template <class DATATYPE, class ELEMTYPE, int NUMDIMS,
75 class ELEMTYPEREAL = ELEMTYPE, int TMAXNODES = 8, int TMINNODES = TMAXNODES / 2>
76class RTree
77{
78protected:
79
80 struct Node; // Fwd decl. Used by other internal structs and iterator
81public:
82
83 // These constant must be declared after Branch and before Node struct
84 // Stuck up here for MSVC 6 compiler. NSVC .NET 2003 is much happier.
85 enum {
86 MAXNODES = TMAXNODES,
87 MINNODES = TMINNODES,
88 };
89
90 struct Statistics {
91 int maxDepth;
92 int avgDepth;
93
94 int maxNodeLoad;
95 int avgNodeLoad;
96 int totalItems;
97 };
98
99public:
100
101 RTree();
102 virtual ~RTree();
103
108 void Insert( const ELEMTYPE a_min[NUMDIMS],
109 const ELEMTYPE a_max[NUMDIMS],
110 const DATATYPE& a_dataId );
111
117 bool Remove( const ELEMTYPE a_min[NUMDIMS],
118 const ELEMTYPE a_max[NUMDIMS],
119 const DATATYPE& a_dataId );
120
126 int Search( const ELEMTYPE a_min[NUMDIMS],
127 const ELEMTYPE a_max[NUMDIMS],
128 std::function<bool (const DATATYPE&)> a_callback ) const;
129
130 template <class VISITOR>
131 int Search( const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], VISITOR& a_visitor )
132 {
133 #ifdef _DEBUG
134
135 for( int index = 0; index<NUMDIMS; ++index )
136 {
137 ASSERT( a_min[index] <= a_max[index] );
138 }
139
140 #endif // _DEBUG
141
142 Rect rect;
143
144 for( int axis = 0; axis<NUMDIMS; ++axis )
145 {
146 rect.m_min[axis] = a_min[axis];
147 rect.m_max[axis] = a_max[axis];
148 }
149
150
151 // NOTE: May want to return search result another way, perhaps returning the number of found elements here.
152 int cnt = 0;
153
154 Search( m_root, &rect, a_visitor, cnt );
155
156 return cnt;
157 }
158
160
162
164 void RemoveAll();
165
167 int Count();
168
170 bool Load( const char* a_fileName );
171
173 bool Load( RTFileStream& a_stream );
174
175
177 bool Save( const char* a_fileName );
178
180 bool Save( RTFileStream& a_stream );
181
187 DATATYPE NearestNeighbor( const ELEMTYPE a_point[NUMDIMS] );
188
196 DATATYPE NearestNeighbor( const ELEMTYPE a_point[NUMDIMS],
197 ELEMTYPE a_squareDistanceCallback( const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
198 ELEMTYPE* a_squareDistance );
199
202 {
203 private:
204
205 enum { MAX_STACK = 32 }; // Max stack size. Allows almost n^32 where n is number of branches in node
206
207 struct StackElement
208 {
209 Node* m_node;
210 int m_branchIndex;
211 };
212 public:
213
214 Iterator() { Init(); }
215
216 ~Iterator() { }
217
219 bool IsNull() { return m_tos <= 0; }
220
222 bool IsNotNull() { return m_tos > 0; }
223
225 DATATYPE& operator*()
226 {
227 ASSERT( IsNotNull() );
228 StackElement& curTos = m_stack[m_tos - 1];
229 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
230 }
231
233 const DATATYPE& operator*() const
234 {
235 ASSERT( IsNotNull() );
236 StackElement& curTos = m_stack[m_tos - 1];
237 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
238 }
239
241 bool operator++() { return FindNextData(); }
242
244 void GetBounds( ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS] )
245 {
246 ASSERT( IsNotNull() );
247 StackElement& curTos = m_stack[m_tos - 1];
248 Branch& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex];
249
250 for( int index = 0; index < NUMDIMS; ++index )
251 {
252 a_min[index] = curBranch.m_rect.m_min[index];
253 a_max[index] = curBranch.m_rect.m_max[index];
254 }
255 }
256
257 private:
258
260 void Init() { m_tos = 0; }
261
263 bool FindNextData()
264 {
265 for( ; ; )
266 {
267 if( m_tos <= 0 )
268 {
269 return false;
270 }
271
272 StackElement curTos = Pop(); // Copy stack top cause it may change as we use it
273
274 if( curTos.m_node->IsLeaf() )
275 {
276 // Keep walking through data while we can
277 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
278 {
279 // There is more data, just point to the next one
280 Push( curTos.m_node, curTos.m_branchIndex + 1 );
281 return true;
282 }
283
284 // No more data, so it will fall back to previous level
285 }
286 else
287 {
288 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
289 {
290 // Push sibling on for future tree walk
291 // This is the 'fall back' node when we finish with the current level
292 Push( curTos.m_node, curTos.m_branchIndex + 1 );
293 }
294
295 // Since cur node is not a leaf, push first of next level to get deeper into the tree
296 Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
297 Push( nextLevelnode, 0 );
298
299 // If we pushed on a new leaf, exit as the data is ready at TOS
300 if( nextLevelnode->IsLeaf() )
301 {
302 return true;
303 }
304 }
305 }
306 }
307
309 void Push( Node* a_node, int a_branchIndex )
310 {
311 m_stack[m_tos].m_node = a_node;
312 m_stack[m_tos].m_branchIndex = a_branchIndex;
313 ++m_tos;
314 ASSERT( m_tos <= MAX_STACK );
315 }
316
318 StackElement& Pop()
319 {
320 ASSERT( m_tos > 0 );
321 --m_tos;
322 return m_stack[m_tos];
323 }
324
325 StackElement m_stack[MAX_STACK];
326 int m_tos;
327
328 friend class RTree; // Allow hiding of non-public functions while allowing manipulation by logical owner
329 };
330
331
333 void GetFirst( Iterator& a_it )
334 {
335 a_it.Init();
336 Node* first = m_root;
337
338 while( first )
339 {
340 if( first->IsInternalNode() && first->m_count > 1 )
341 {
342 a_it.Push( first, 1 ); // Descend sibling branch later
343 }
344 else if( first->IsLeaf() )
345 {
346 if( first->m_count )
347 {
348 a_it.Push( first, 0 );
349 }
350
351 break;
352 }
353
354 first = first->m_branch[0].m_child;
355 }
356 }
357
359 void GetNext( Iterator& a_it ) { ++a_it; }
360
362 bool IsNull( Iterator& a_it ) { return a_it.IsNull(); }
363
365 DATATYPE& GetAt( Iterator& a_it ) { return *a_it; }
366protected:
367
369 struct Rect
370 {
371 ELEMTYPE m_min[NUMDIMS];
372 ELEMTYPE m_max[NUMDIMS];
373 };
374
378 struct Branch
379 {
381 union
382 {
384 DATATYPE m_data;
385 };
386 };
387
389 struct Node
390 {
391 bool IsInternalNode() { return m_level > 0; } // Not a leaf, but a internal node
392 bool IsLeaf() { return m_level == 0; } // A leaf, contains data
393
397 };
398
400 struct ListNode
401 {
404 };
405
408 {
409 int m_partition[MAXNODES + 1];
410 int m_total;
411 int m_minFill;
412 int m_taken[MAXNODES + 1];
413 int m_count[2];
414 Rect m_cover[2];
415 ELEMTYPEREAL m_area[2];
416
417 Branch m_branchBuf[MAXNODES + 1];
418 int m_branchCount;
419 Rect m_coverSplit;
420 ELEMTYPEREAL m_coverSplitArea;
421 };
422
424 struct NNNode
425 {
426 Branch m_branch;
427 ELEMTYPE minDist;
428 bool isLeaf;
429 };
430
431 Node* AllocNode();
432 void FreeNode( Node* a_node );
433 void InitNode( Node* a_node );
434 void InitRect( Rect* a_rect );
435 bool InsertRectRec( Rect* a_rect,
436 const DATATYPE& a_id,
437 Node* a_node,
438 Node** a_newNode,
439 int a_level );
440 bool InsertRect( Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level );
441 Rect NodeCover( Node* a_node );
442 bool AddBranch( Branch* a_branch, Node* a_node, Node** a_newNode );
443 void DisconnectBranch( Node* a_node, int a_index );
444 int PickBranch( Rect* a_rect, Node* a_node );
445 Rect CombineRect( Rect* a_rectA, Rect* a_rectB );
446 void SplitNode( Node* a_node, Branch* a_branch, Node** a_newNode );
447 ELEMTYPEREAL RectSphericalVolume( Rect* a_rect );
448 ELEMTYPEREAL RectVolume( Rect* a_rect );
449 ELEMTYPEREAL CalcRectVolume( Rect* a_rect );
450 void GetBranches( Node* a_node, Branch* a_branch, PartitionVars* a_parVars );
451 void ChoosePartition( PartitionVars* a_parVars, int a_minFill );
452 void LoadNodes( Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars );
453 void InitParVars( PartitionVars* a_parVars, int a_maxRects, int a_minFill );
454 void PickSeeds( PartitionVars* a_parVars );
455 void Classify( int a_index, int a_group, PartitionVars* a_parVars );
456 bool RemoveRect( Rect* a_rect, const DATATYPE& a_id, Node** a_root );
457 bool RemoveRectRec( Rect* a_rect,
458 const DATATYPE& a_id,
459 Node* a_node,
460 ListNode** a_listNode );
461 ListNode* AllocListNode();
462 void FreeListNode( ListNode* a_listNode );
463 bool Overlap( Rect* a_rectA, Rect* a_rectB ) const;
464 void ReInsert( Node* a_node, ListNode** a_listNode );
465 ELEMTYPE MinDist( const ELEMTYPE a_point[NUMDIMS], Rect* a_rect );
466 void InsertNNListSorted( std::vector<NNNode*>* nodeList, NNNode* newNode );
467
468 bool Search( Node * a_node, Rect * a_rect, int& a_foundCount,
469 std::function<bool (const DATATYPE&)> a_callback ) const;
470
471 template <class VISITOR>
472 bool Search( Node* a_node, Rect* a_rect, VISITOR& a_visitor, int& a_foundCount )
473 {
474 ASSERT( a_node );
475 ASSERT( a_node->m_level >= 0 );
476 ASSERT( a_rect );
477
478 if( a_node->IsInternalNode() ) // This is an internal node in the tree
479 {
480 for( int index = 0; index < a_node->m_count; ++index )
481 {
482 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
483 {
484 if( !Search( a_node->m_branch[index].m_child, a_rect, a_visitor, a_foundCount ) )
485 {
486 return false; // Don't continue searching
487 }
488 }
489 }
490 }
491 else // This is a leaf node
492 {
493 for( int index = 0; index < a_node->m_count; ++index )
494 {
495 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
496 {
497 DATATYPE& id = a_node->m_branch[index].m_data;
498
499 if( !a_visitor( id ) )
500 return false;
501
502 a_foundCount++;
503 }
504 }
505 }
506
507 return true; // Continue searching
508 }
509
510 void RemoveAllRec( Node* a_node );
511 void Reset();
512 void CountRec( Node* a_node, int& a_count );
513
514 bool SaveRec( Node* a_node, RTFileStream& a_stream );
515 bool LoadRec( Node* a_node, RTFileStream& a_stream );
516
518 ELEMTYPEREAL m_unitSphereVolume;
519};
520
521
522// Because there is not stream support, this is a quick and dirty file I/O helper.
523// Users will likely replace its usage with a Stream implementation from their favorite API.
525{
526 FILE* m_file;
527public:
528
529
531 {
532 m_file = NULL;
533 }
534
536 {
537 Close();
538 }
539
540 bool OpenRead( const char* a_fileName )
541 {
542 m_file = fopen( a_fileName, "rb" );
543
544 if( !m_file )
545 {
546 return false;
547 }
548
549 return true;
550 }
551
552 bool OpenWrite( const char* a_fileName )
553 {
554 m_file = fopen( a_fileName, "wb" );
555
556 if( !m_file )
557 {
558 return false;
559 }
560
561 return true;
562 }
563
564 void Close()
565 {
566 if( m_file )
567 {
568 fclose( m_file );
569 m_file = NULL;
570 }
571 }
572
573 template <typename TYPE>
574 size_t Write( const TYPE& a_value )
575 {
576 ASSERT( m_file );
577 return fwrite( (void*) &a_value, sizeof(a_value), 1, m_file );
578 }
579
580 template <typename TYPE>
581 size_t WriteArray( const TYPE* a_array, int a_count )
582 {
583 ASSERT( m_file );
584 return fwrite( (void*) a_array, sizeof(TYPE) * a_count, 1, m_file );
585 }
586
587 template <typename TYPE>
588 size_t Read( TYPE& a_value )
589 {
590 ASSERT( m_file );
591 return fread( (void*) &a_value, sizeof(a_value), 1, m_file );
592 }
593
594 template <typename TYPE>
595 size_t ReadArray( TYPE* a_array, int a_count )
596 {
597 ASSERT( m_file );
598 return fread( (void*) a_array, sizeof(TYPE) * a_count, 1, m_file );
599 }
600};
601
602
603RTREE_TEMPLATE RTREE_QUAL::RTree()
604{
605 ASSERT( MAXNODES > MINNODES );
606 ASSERT( MINNODES > 0 );
607
608
609 // We only support machine word size simple data type eg. integer index or object pointer.
610 // Since we are storing as union with non data branch
611 ASSERT( sizeof(DATATYPE) == sizeof(void*) || sizeof(DATATYPE) == sizeof(int) );
612
613 // Precomputed volumes of the unit spheres for the first few dimensions
614 const float UNIT_SPHERE_VOLUMES[] =
615 {
616 0.000000f, 2.000000f, 3.141593f, // Dimension 0,1,2
617 4.188790f, 4.934802f, 5.263789f, // Dimension 3,4,5
618 5.167713f, 4.724766f, 4.058712f, // Dimension 6,7,8
619 3.298509f, 2.550164f, 1.884104f, // Dimension 9,10,11
620 1.335263f, 0.910629f, 0.599265f, // Dimension 12,13,14
621 0.381443f, 0.235331f, 0.140981f, // Dimension 15,16,17
622 0.082146f, 0.046622f, 0.025807f, // Dimension 18,19,20
623 };
624
625 m_root = AllocNode();
626 m_root->m_level = 0;
627 m_unitSphereVolume = (ELEMTYPEREAL) UNIT_SPHERE_VOLUMES[NUMDIMS];
628}
629
630
631RTREE_TEMPLATE
632RTREE_QUAL::~RTree() {
633 Reset(); // Free, or reset node memory
634}
635
636
637RTREE_TEMPLATE
638void RTREE_QUAL::Insert( const ELEMTYPE a_min[NUMDIMS],
639 const ELEMTYPE a_max[NUMDIMS],
640 const DATATYPE& a_dataId )
641{
642#ifdef _DEBUG
643
644 for( int index = 0; index<NUMDIMS; ++index )
645 {
646 ASSERT( a_min[index] <= a_max[index] );
647 }
648
649#endif // _DEBUG
650
651 Rect rect;
652
653 for( int axis = 0; axis<NUMDIMS; ++axis )
654 {
655 rect.m_min[axis] = a_min[axis];
656 rect.m_max[axis] = a_max[axis];
657 }
658
659 InsertRect( &rect, a_dataId, &m_root, 0 );
660}
661
662
663RTREE_TEMPLATE
664bool RTREE_QUAL::Remove( const ELEMTYPE a_min[NUMDIMS],
665 const ELEMTYPE a_max[NUMDIMS],
666 const DATATYPE& a_dataId )
667{
668#ifdef _DEBUG
669
670 for( int index = 0; index<NUMDIMS; ++index )
671 {
672 ASSERT( a_min[index] <= a_max[index] );
673 }
674
675#endif // _DEBUG
676
677 Rect rect;
678
679 for( int axis = 0; axis<NUMDIMS; ++axis )
680 {
681 rect.m_min[axis] = a_min[axis];
682 rect.m_max[axis] = a_max[axis];
683 }
684
685 return RemoveRect( &rect, a_dataId, &m_root );
686}
687
688
689RTREE_TEMPLATE
690int RTREE_QUAL::Search( const ELEMTYPE a_min[NUMDIMS],
691 const ELEMTYPE a_max[NUMDIMS],
692 std::function<bool (const DATATYPE&)> a_callback ) const
693{
694#ifdef _DEBUG
695
696 for( int index = 0; index<NUMDIMS; ++index )
697 {
698 ASSERT( a_min[index] <= a_max[index] );
699 }
700
701#endif // _DEBUG
702
703 Rect rect;
704
705 for( int axis = 0; axis<NUMDIMS; ++axis )
706 {
707 rect.m_min[axis] = a_min[axis];
708 rect.m_max[axis] = a_max[axis];
709 }
710
711 // NOTE: May want to return search result another way, perhaps returning the number of found elements here.
712
713 int foundCount = 0;
714 Search( m_root, &rect, foundCount, a_callback );
715 return foundCount;
716}
717
718
719RTREE_TEMPLATE
720DATATYPE RTREE_QUAL::NearestNeighbor( const ELEMTYPE a_point[NUMDIMS] )
721{
722 return this->NearestNeighbor( a_point, 0, 0 );
723}
724
725
726RTREE_TEMPLATE
727DATATYPE RTREE_QUAL::NearestNeighbor( const ELEMTYPE a_point[NUMDIMS],
728 ELEMTYPE a_squareDistanceCallback( const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
729 ELEMTYPE* a_squareDistance )
730{
731 typedef typename std::vector<NNNode*>::iterator iterator;
732 std::vector<NNNode*> nodeList;
733 Node* node = m_root;
734 NNNode* closestNode = 0;
735 while( !closestNode || !closestNode->isLeaf )
736 {
737 //check every node on this level
738 for( int index = 0; index < node->m_count; ++index )
739 {
740 NNNode* newNode = new NNNode;
741 newNode->isLeaf = node->IsLeaf();
742 newNode->m_branch = node->m_branch[index];
743 if( newNode->isLeaf && a_squareDistanceCallback )
744 newNode->minDist = a_squareDistanceCallback( a_point, newNode->m_branch.m_data );
745 else
746 newNode->minDist = this->MinDist( a_point, &(node->m_branch[index].m_rect) );
747
748 //TODO: a custom list could be more efficient than a vector
749 this->InsertNNListSorted( &nodeList, newNode );
750 }
751 if( nodeList.size() == 0 )
752 {
753 return 0;
754 }
755 closestNode = nodeList.back();
756 node = closestNode->m_branch.m_child;
757 nodeList.pop_back();
758 free(closestNode);
759 }
760
761 // free memory used for remaining NNNodes in nodeList
762 for( iterator iter = nodeList.begin(); iter != nodeList.end(); ++iter )
763 {
764 NNNode* nnode = *iter;
765 free(nnode);
766 }
767
768 *a_squareDistance = closestNode->minDist;
769 return closestNode->m_branch.m_data;
770}
771
772
773RTREE_TEMPLATE
774int RTREE_QUAL::Count()
775{
776 int count = 0;
777
778 CountRec( m_root, count );
779
780 return count;
781}
782
783
784RTREE_TEMPLATE
785void RTREE_QUAL::CountRec( Node* a_node, int& a_count )
786{
787 if( a_node->IsInternalNode() ) // not a leaf node
788 {
789 for( int index = 0; index < a_node->m_count; ++index )
790 {
791 CountRec( a_node->m_branch[index].m_child, a_count );
792 }
793 }
794 else // A leaf node
795 {
796 a_count += a_node->m_count;
797 }
798}
799
800
801RTREE_TEMPLATE
802bool RTREE_QUAL::Load( const char* a_fileName )
803{
804 RemoveAll(); // Clear existing tree
805
806 RTFileStream stream;
807
808 if( !stream.OpenRead( a_fileName ) )
809 {
810 return false;
811 }
812
813 bool result = Load( stream );
814
815 stream.Close();
816
817 return result;
818}
819
820
821RTREE_TEMPLATE
822bool RTREE_QUAL::Load( RTFileStream& a_stream )
823{
824 // Write some kind of header
825 int _dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
826 int _dataSize = sizeof(DATATYPE);
827 int _dataNumDims = NUMDIMS;
828 int _dataElemSize = sizeof(ELEMTYPE);
829 int _dataElemRealSize = sizeof(ELEMTYPEREAL);
830 int _dataMaxNodes = TMAXNODES;
831 int _dataMinNodes = TMINNODES;
832
833 int dataFileId = 0;
834 int dataSize = 0;
835 int dataNumDims = 0;
836 int dataElemSize = 0;
837 int dataElemRealSize = 0;
838 int dataMaxNodes = 0;
839 int dataMinNodes = 0;
840
841 a_stream.Read( dataFileId );
842 a_stream.Read( dataSize );
843 a_stream.Read( dataNumDims );
844 a_stream.Read( dataElemSize );
845 a_stream.Read( dataElemRealSize );
846 a_stream.Read( dataMaxNodes );
847 a_stream.Read( dataMinNodes );
848
849 bool result = false;
850
851 // Test if header was valid and compatible
852 if( (dataFileId == _dataFileId)
853 && (dataSize == _dataSize)
854 && (dataNumDims == _dataNumDims)
855 && (dataElemSize == _dataElemSize)
856 && (dataElemRealSize == _dataElemRealSize)
857 && (dataMaxNodes == _dataMaxNodes)
858 && (dataMinNodes == _dataMinNodes)
859 )
860 {
861 // Recursively load tree
862 result = LoadRec( m_root, a_stream );
863 }
864
865 return result;
866}
867
868
869RTREE_TEMPLATE
870bool RTREE_QUAL::LoadRec( Node* a_node, RTFileStream& a_stream )
871{
872 a_stream.Read( a_node->m_level );
873 a_stream.Read( a_node->m_count );
874
875 if( a_node->IsInternalNode() ) // not a leaf node
876 {
877 for( int index = 0; index < a_node->m_count; ++index )
878 {
879 Branch* curBranch = &a_node->m_branch[index];
880
881 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
882 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
883
884 curBranch->m_child = AllocNode();
885 LoadRec( curBranch->m_child, a_stream );
886 }
887 }
888 else // A leaf node
889 {
890 for( int index = 0; index < a_node->m_count; ++index )
891 {
892 Branch* curBranch = &a_node->m_branch[index];
893
894 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
895 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
896
897 a_stream.Read( curBranch->m_data );
898 }
899 }
900
901 return true; // Should do more error checking on I/O operations
902}
903
904
905RTREE_TEMPLATE
906bool RTREE_QUAL::Save( const char* a_fileName )
907{
908 RTFileStream stream;
909
910 if( !stream.OpenWrite( a_fileName ) )
911 {
912 return false;
913 }
914
915 bool result = Save( stream );
916
917 stream.Close();
918
919 return result;
920}
921
922
923RTREE_TEMPLATE
924bool RTREE_QUAL::Save( RTFileStream& a_stream )
925{
926 // Write some kind of header
927 int dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
928 int dataSize = sizeof(DATATYPE);
929 int dataNumDims = NUMDIMS;
930 int dataElemSize = sizeof(ELEMTYPE);
931 int dataElemRealSize = sizeof(ELEMTYPEREAL);
932 int dataMaxNodes = TMAXNODES;
933 int dataMinNodes = TMINNODES;
934
935 a_stream.Write( dataFileId );
936 a_stream.Write( dataSize );
937 a_stream.Write( dataNumDims );
938 a_stream.Write( dataElemSize );
939 a_stream.Write( dataElemRealSize );
940 a_stream.Write( dataMaxNodes );
941 a_stream.Write( dataMinNodes );
942
943 // Recursively save tree
944 bool result = SaveRec( m_root, a_stream );
945
946 return result;
947}
948
949
950RTREE_TEMPLATE
951bool RTREE_QUAL::SaveRec( Node* a_node, RTFileStream& a_stream )
952{
953 a_stream.Write( a_node->m_level );
954 a_stream.Write( a_node->m_count );
955
956 if( a_node->IsInternalNode() ) // not a leaf node
957 {
958 for( int index = 0; index < a_node->m_count; ++index )
959 {
960 Branch* curBranch = &a_node->m_branch[index];
961
962 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
963 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
964
965 SaveRec( curBranch->m_child, a_stream );
966 }
967 }
968 else // A leaf node
969 {
970 for( int index = 0; index < a_node->m_count; ++index )
971 {
972 Branch* curBranch = &a_node->m_branch[index];
973
974 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
975 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
976
977 a_stream.Write( curBranch->m_data );
978 }
979 }
980
981 return true; // Should do more error checking on I/O operations
982}
983
984
985RTREE_TEMPLATE
986void RTREE_QUAL::RemoveAll()
987{
988 // Delete all existing nodes
989 Reset();
990
991 m_root = AllocNode();
992 m_root->m_level = 0;
993}
994
995
996RTREE_TEMPLATE
997void RTREE_QUAL::Reset()
998{
999#ifdef RTREE_DONT_USE_MEMPOOLS
1000 // Delete all existing nodes
1001 RemoveAllRec( m_root );
1002#else // RTREE_DONT_USE_MEMPOOLS
1003 // Just reset memory pools. We are not using complex types
1004 // EXAMPLE
1005#endif // RTREE_DONT_USE_MEMPOOLS
1006}
1007
1008
1009RTREE_TEMPLATE
1010void RTREE_QUAL::RemoveAllRec( Node* a_node )
1011{
1012 ASSERT( a_node );
1013 ASSERT( a_node->m_level >= 0 );
1014
1015 if( a_node->IsInternalNode() ) // This is an internal node in the tree
1016 {
1017 for( int index = 0; index < a_node->m_count; ++index )
1018 {
1019 RemoveAllRec( a_node->m_branch[index].m_child );
1020 }
1021 }
1022
1023 FreeNode( a_node );
1024}
1025
1026
1027RTREE_TEMPLATE
1028typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
1029{
1030 Node* newNode;
1031
1032#ifdef RTREE_DONT_USE_MEMPOOLS
1033 newNode = new Node;
1034#else // RTREE_DONT_USE_MEMPOOLS
1035 // EXAMPLE
1036#endif // RTREE_DONT_USE_MEMPOOLS
1037 InitNode( newNode );
1038 return newNode;
1039}
1040
1041
1042RTREE_TEMPLATE
1043void RTREE_QUAL::FreeNode( Node* a_node )
1044{
1045 ASSERT( a_node );
1046
1047#ifdef RTREE_DONT_USE_MEMPOOLS
1048 delete a_node;
1049#else // RTREE_DONT_USE_MEMPOOLS
1050 // EXAMPLE
1051#endif // RTREE_DONT_USE_MEMPOOLS
1052}
1053
1054
1055// Allocate space for a node in the list used in DeletRect to
1056// store Nodes that are too empty.
1057RTREE_TEMPLATE
1058typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
1059{
1060#ifdef RTREE_DONT_USE_MEMPOOLS
1061 return new ListNode;
1062#else // RTREE_DONT_USE_MEMPOOLS
1063 // EXAMPLE
1064#endif // RTREE_DONT_USE_MEMPOOLS
1065}
1066
1067
1068RTREE_TEMPLATE
1069void RTREE_QUAL::FreeListNode( ListNode* a_listNode )
1070{
1071#ifdef RTREE_DONT_USE_MEMPOOLS
1072 delete a_listNode;
1073#else // RTREE_DONT_USE_MEMPOOLS
1074 // EXAMPLE
1075#endif // RTREE_DONT_USE_MEMPOOLS
1076}
1077
1078
1079RTREE_TEMPLATE
1080void RTREE_QUAL::InitNode( Node* a_node )
1081{
1082 a_node->m_count = 0;
1083 a_node->m_level = -1;
1084}
1085
1086
1087RTREE_TEMPLATE
1088void RTREE_QUAL::InitRect( Rect* a_rect )
1089{
1090 for( int index = 0; index < NUMDIMS; ++index )
1091 {
1092 a_rect->m_min[index] = (ELEMTYPE) 0;
1093 a_rect->m_max[index] = (ELEMTYPE) 0;
1094 }
1095}
1096
1097
1098// Inserts a new data rectangle into the index structure.
1099// Recursively descends tree, propagates splits back up.
1100// Returns 0 if node was not split. Old node updated.
1101// If node was split, returns 1 and sets the pointer pointed to by
1102// new_node to point to the new node. Old node updated to become one of two.
1103// The level argument specifies the number of steps up from the leaf
1104// level to insert; e.g. a data rectangle goes in at level = 0.
1105RTREE_TEMPLATE
1106bool RTREE_QUAL::InsertRectRec( Rect* a_rect,
1107 const DATATYPE& a_id,
1108 Node* a_node,
1109 Node** a_newNode,
1110 int a_level )
1111{
1112 ASSERT( a_rect && a_node && a_newNode );
1113 ASSERT( a_level >= 0 && a_level <= a_node->m_level );
1114
1115 int index;
1116 Branch branch;
1117 Node* otherNode;
1118
1119 // Still above level for insertion, go down tree recursively
1120 if( a_node->m_level > a_level )
1121 {
1122 index = PickBranch( a_rect, a_node );
1123
1124 if( !InsertRectRec( a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level ) )
1125 {
1126 // Child was not split
1127 a_node->m_branch[index].m_rect =
1128 CombineRect( a_rect, &(a_node->m_branch[index].m_rect) );
1129 return false;
1130 }
1131 else // Child was split
1132 {
1133 a_node->m_branch[index].m_rect = NodeCover( a_node->m_branch[index].m_child );
1134 branch.m_child = otherNode;
1135 branch.m_rect = NodeCover( otherNode );
1136 return AddBranch( &branch, a_node, a_newNode );
1137 }
1138 }
1139 else if( a_node->m_level == a_level ) // Have reached level for insertion. Add rect, split if necessary
1140 {
1141 branch.m_rect = *a_rect;
1142 branch.m_child = (Node*) a_id;
1143 // Child field of leaves contains id of data record
1144 return AddBranch( &branch, a_node, a_newNode );
1145 }
1146 else
1147 {
1148 // Should never occur
1149 ASSERT( 0 );
1150 return false;
1151 }
1152}
1153
1154
1155// Insert a data rectangle into an index structure.
1156// InsertRect provides for splitting the root;
1157// returns 1 if root was split, 0 if it was not.
1158// The level argument specifies the number of steps up from the leaf
1159// level to insert; e.g. a data rectangle goes in at level = 0.
1160// InsertRect2 does the recursion.
1161//
1162RTREE_TEMPLATE
1163bool RTREE_QUAL::InsertRect( Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level )
1164{
1165 ASSERT( a_rect && a_root );
1166 ASSERT( a_level >= 0 && a_level <= (*a_root)->m_level );
1167#ifdef _DEBUG
1168
1169 for( int index = 0; index < NUMDIMS; ++index )
1170 {
1171 ASSERT( a_rect->m_min[index] <= a_rect->m_max[index] );
1172 }
1173
1174#endif // _DEBUG
1175
1176 Node* newRoot;
1177 Node* newNode;
1178 Branch branch;
1179
1180 if( InsertRectRec( a_rect, a_id, *a_root, &newNode, a_level ) ) // Root split
1181 {
1182 newRoot = AllocNode(); // Grow tree taller and new root
1183 newRoot->m_level = (*a_root)->m_level + 1;
1184 branch.m_rect = NodeCover( *a_root );
1185 branch.m_child = *a_root;
1186 AddBranch( &branch, newRoot, NULL );
1187 branch.m_rect = NodeCover( newNode );
1188 branch.m_child = newNode;
1189 AddBranch( &branch, newRoot, NULL );
1190 *a_root = newRoot;
1191 return true;
1192 }
1193
1194 return false;
1195}
1196
1197
1198// Find the smallest rectangle that includes all rectangles in branches of a node.
1199RTREE_TEMPLATE
1200typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover( Node* a_node )
1201{
1202 ASSERT( a_node );
1203
1204 int firstTime = true;
1205 Rect rect;
1206 InitRect( &rect );
1207
1208 for( int index = 0; index < a_node->m_count; ++index )
1209 {
1210 if( firstTime )
1211 {
1212 rect = a_node->m_branch[index].m_rect;
1213 firstTime = false;
1214 }
1215 else
1216 {
1217 rect = CombineRect( &rect, &(a_node->m_branch[index].m_rect) );
1218 }
1219 }
1220
1221 return rect;
1222}
1223
1224
1225// Add a branch to a node. Split the node if necessary.
1226// Returns 0 if node not split. Old node updated.
1227// Returns 1 if node split, sets *new_node to address of new node.
1228// Old node updated, becomes one of two.
1229RTREE_TEMPLATE
1230bool RTREE_QUAL::AddBranch( Branch* a_branch, Node* a_node, Node** a_newNode )
1231{
1232 ASSERT( a_branch );
1233 ASSERT( a_node );
1234
1235 if( a_node->m_count < MAXNODES ) // Split won't be necessary
1236 {
1237 a_node->m_branch[a_node->m_count] = *a_branch;
1238 ++a_node->m_count;
1239
1240 return false;
1241 }
1242 else
1243 {
1244 ASSERT( a_newNode );
1245
1246 SplitNode( a_node, a_branch, a_newNode );
1247 return true;
1248 }
1249}
1250
1251
1252// Disconnect a dependent node.
1253// Caller must return (or stop using iteration index) after this as count has changed
1254RTREE_TEMPLATE
1255void RTREE_QUAL::DisconnectBranch( Node* a_node, int a_index )
1256{
1257 ASSERT( a_node && (a_index >= 0) && (a_index < MAXNODES) );
1258 ASSERT( a_node->m_count > 0 );
1259
1260 // Remove element by swapping with the last element to prevent gaps in array
1261 a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
1262
1263 --a_node->m_count;
1264}
1265
1266
1267// Pick a branch. Pick the one that will need the smallest increase
1268// in area to accomodate the new rectangle. This will result in the
1269// least total area for the covering rectangles in the current node.
1270// In case of a tie, pick the one which was smaller before, to get
1271// the best resolution when searching.
1272RTREE_TEMPLATE
1273int RTREE_QUAL::PickBranch( Rect* a_rect, Node* a_node )
1274{
1275 ASSERT( a_rect && a_node );
1276
1277 bool firstTime = true;
1278 ELEMTYPEREAL increase;
1279 ELEMTYPEREAL bestIncr = (ELEMTYPEREAL) -1;
1280 ELEMTYPEREAL area;
1281 ELEMTYPEREAL bestArea = 0;
1282 int best = 0;
1283 Rect tempRect;
1284
1285 for( int index = 0; index < a_node->m_count; ++index )
1286 {
1287 Rect* curRect = &a_node->m_branch[index].m_rect;
1288 area = CalcRectVolume( curRect );
1289 tempRect = CombineRect( a_rect, curRect );
1290 increase = CalcRectVolume( &tempRect ) - area;
1291
1292 if( (increase < bestIncr) || firstTime )
1293 {
1294 best = index;
1295 bestArea = area;
1296 bestIncr = increase;
1297 firstTime = false;
1298 }
1299 else if( (increase == bestIncr) && (area < bestArea) )
1300 {
1301 best = index;
1302 bestArea = area;
1303 bestIncr = increase;
1304 }
1305 }
1306
1307 return best;
1308}
1309
1310
1311// Combine two rectangles into larger one containing both
1312RTREE_TEMPLATE
1313typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect( Rect* a_rectA, Rect* a_rectB )
1314{
1315 ASSERT( a_rectA && a_rectB );
1316
1317 Rect newRect;
1318
1319 for( int index = 0; index < NUMDIMS; ++index )
1320 {
1321 newRect.m_min[index] = std::min( a_rectA->m_min[index], a_rectB->m_min[index] );
1322 newRect.m_max[index] = std::max( a_rectA->m_max[index], a_rectB->m_max[index] );
1323 }
1324
1325 return newRect;
1326}
1327
1328
1329// Split a node.
1330// Divides the nodes branches and the extra one between two nodes.
1331// Old node is one of the new ones, and one really new one is created.
1332// Tries more than one method for choosing a partition, uses best result.
1333RTREE_TEMPLATE
1334void RTREE_QUAL::SplitNode( Node* a_node, Branch* a_branch, Node** a_newNode )
1335{
1336 ASSERT( a_node );
1337 ASSERT( a_branch );
1338
1339 // Could just use local here, but member or external is faster since it is reused
1340 PartitionVars localVars;
1341 PartitionVars* parVars = &localVars;
1342 int level;
1343
1344 // Load all the branches into a buffer, initialize old node
1345 level = a_node->m_level;
1346 GetBranches( a_node, a_branch, parVars );
1347
1348 // Find partition
1349 ChoosePartition( parVars, MINNODES );
1350
1351 // Put branches from buffer into 2 nodes according to chosen partition
1352 *a_newNode = AllocNode();
1353 (*a_newNode)->m_level = a_node->m_level = level;
1354 LoadNodes( a_node, *a_newNode, parVars );
1355
1356 ASSERT( (a_node->m_count + (*a_newNode)->m_count) == parVars->m_total );
1357}
1358
1359
1360// Calculate the n-dimensional volume of a rectangle
1361RTREE_TEMPLATE
1362ELEMTYPEREAL RTREE_QUAL::RectVolume( Rect* a_rect )
1363{
1364 ASSERT( a_rect );
1365
1366 ELEMTYPEREAL volume = (ELEMTYPEREAL) 1;
1367
1368 for( int index = 0; index<NUMDIMS; ++index )
1369 {
1370 volume *= a_rect->m_max[index] - a_rect->m_min[index];
1371 }
1372
1373 ASSERT( volume >= (ELEMTYPEREAL) 0 );
1374
1375 return volume;
1376}
1377
1378
1379// The exact volume of the bounding sphere for the given Rect
1380RTREE_TEMPLATE
1381ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume( Rect* a_rect )
1382{
1383 ASSERT( a_rect );
1384
1385 ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL) 0;
1386 ELEMTYPEREAL radius;
1387
1388 for( int index = 0; index < NUMDIMS; ++index )
1389 {
1390 ELEMTYPEREAL halfExtent =
1391 ( (ELEMTYPEREAL) a_rect->m_max[index] - (ELEMTYPEREAL) a_rect->m_min[index] ) * 0.5f;
1392 sumOfSquares += halfExtent * halfExtent;
1393 }
1394
1395 radius = (ELEMTYPEREAL) sqrt( sumOfSquares );
1396
1397 // Pow maybe slow, so test for common dims like 2,3 and just use x*x, x*x*x.
1398 if( NUMDIMS == 3 )
1399 {
1400 return radius * radius * radius * m_unitSphereVolume;
1401 }
1402 else if( NUMDIMS == 2 )
1403 {
1404 return radius * radius * m_unitSphereVolume;
1405 }
1406 else
1407 {
1408 return (ELEMTYPEREAL) (pow( radius, NUMDIMS ) * m_unitSphereVolume);
1409 }
1410}
1411
1412
1413// Use one of the methods to calculate retangle volume
1414RTREE_TEMPLATE
1415ELEMTYPEREAL RTREE_QUAL::CalcRectVolume( Rect* a_rect )
1416{
1417#ifdef RTREE_USE_SPHERICAL_VOLUME
1418 return RectSphericalVolume( a_rect ); // Slower but helps certain merge cases
1419#else // RTREE_USE_SPHERICAL_VOLUME
1420 return RectVolume( a_rect ); // Faster but can cause poor merges
1421#endif // RTREE_USE_SPHERICAL_VOLUME
1422}
1423
1424
1425// Load branch buffer with branches from full node plus the extra branch.
1426RTREE_TEMPLATE
1427void RTREE_QUAL::GetBranches( Node* a_node, Branch* a_branch, PartitionVars* a_parVars )
1428{
1429 ASSERT( a_node );
1430 ASSERT( a_branch );
1431
1432 ASSERT( a_node->m_count == MAXNODES );
1433
1434 // Load the branch buffer
1435 for( int index = 0; index < MAXNODES; ++index )
1436 {
1437 a_parVars->m_branchBuf[index] = a_node->m_branch[index];
1438 }
1439
1440 a_parVars->m_branchBuf[MAXNODES] = *a_branch;
1441 a_parVars->m_branchCount = MAXNODES + 1;
1442
1443 // Calculate rect containing all in the set
1444 a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
1445
1446 for( int index = 1; index < MAXNODES + 1; ++index )
1447 {
1448 a_parVars->m_coverSplit =
1449 CombineRect( &a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect );
1450 }
1451
1452 a_parVars->m_coverSplitArea = CalcRectVolume( &a_parVars->m_coverSplit );
1453
1454 InitNode( a_node );
1455}
1456
1457
1458// Method #0 for choosing a partition:
1459// As the seeds for the two groups, pick the two rects that would waste the
1460// most area if covered by a single rectangle, i.e. evidently the worst pair
1461// to have in the same group.
1462// Of the remaining, one at a time is chosen to be put in one of the two groups.
1463// The one chosen is the one with the greatest difference in area expansion
1464// depending on which group - the rect most strongly attracted to one group
1465// and repelled from the other.
1466// If one group gets too full (more would force other group to violate min
1467// fill requirement) then other group gets the rest.
1468// These last are the ones that can go in either group most easily.
1469RTREE_TEMPLATE
1470void RTREE_QUAL::ChoosePartition( PartitionVars* a_parVars, int a_minFill )
1471{
1472 ASSERT( a_parVars );
1473
1474 ELEMTYPEREAL biggestDiff;
1475 int group, chosen = 0, betterGroup = 0;
1476
1477 InitParVars( a_parVars, a_parVars->m_branchCount, a_minFill );
1478 PickSeeds( a_parVars );
1479
1480 while( ( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1481 && ( a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill) )
1482 && ( a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill) ) )
1483 {
1484 biggestDiff = (ELEMTYPEREAL) -1;
1485
1486 for( int index = 0; index<a_parVars->m_total; ++index )
1487 {
1488 if( !a_parVars->m_taken[index] )
1489 {
1490 Rect* curRect = &a_parVars->m_branchBuf[index].m_rect;
1491 Rect rect0 = CombineRect( curRect, &a_parVars->m_cover[0] );
1492 Rect rect1 = CombineRect( curRect, &a_parVars->m_cover[1] );
1493 ELEMTYPEREAL growth0 = CalcRectVolume( &rect0 ) - a_parVars->m_area[0];
1494 ELEMTYPEREAL growth1 = CalcRectVolume( &rect1 ) - a_parVars->m_area[1];
1495 ELEMTYPEREAL diff = growth1 - growth0;
1496
1497 if( diff >= 0 )
1498 {
1499 group = 0;
1500 }
1501 else
1502 {
1503 group = 1;
1504 diff = -diff;
1505 }
1506
1507 if( diff > biggestDiff )
1508 {
1509 biggestDiff = diff;
1510 chosen = index;
1511 betterGroup = group;
1512 }
1513 else if( (diff == biggestDiff)
1514 && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]) )
1515 {
1516 chosen = index;
1517 betterGroup = group;
1518 }
1519 }
1520 }
1521
1522 Classify( chosen, betterGroup, a_parVars );
1523 }
1524
1525 // If one group too full, put remaining rects in the other
1526 if( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1527 {
1528 if( a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill )
1529 {
1530 group = 1;
1531 }
1532 else
1533 {
1534 group = 0;
1535 }
1536
1537 for( int index = 0; index<a_parVars->m_total; ++index )
1538 {
1539 if( !a_parVars->m_taken[index] )
1540 {
1541 Classify( index, group, a_parVars );
1542 }
1543 }
1544 }
1545
1546 ASSERT( (a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total );
1547 ASSERT( (a_parVars->m_count[0] >= a_parVars->m_minFill)
1548 && (a_parVars->m_count[1] >= a_parVars->m_minFill) );
1549}
1550
1551
1552// Copy branches from the buffer into two nodes according to the partition.
1553RTREE_TEMPLATE
1554void RTREE_QUAL::LoadNodes( Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars )
1555{
1556 ASSERT( a_nodeA );
1557 ASSERT( a_nodeB );
1558 ASSERT( a_parVars );
1559
1560 for( int index = 0; index < a_parVars->m_total; ++index )
1561 {
1562 ASSERT( a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1 );
1563
1564 if( a_parVars->m_partition[index] == 0 )
1565 {
1566 AddBranch( &a_parVars->m_branchBuf[index], a_nodeA, NULL );
1567 }
1568 else if( a_parVars->m_partition[index] == 1 )
1569 {
1570 AddBranch( &a_parVars->m_branchBuf[index], a_nodeB, NULL );
1571 }
1572 }
1573}
1574
1575
1576// Initialize a PartitionVars structure.
1577RTREE_TEMPLATE
1578void RTREE_QUAL::InitParVars( PartitionVars* a_parVars, int a_maxRects, int a_minFill )
1579{
1580 ASSERT( a_parVars );
1581
1582 a_parVars->m_count[0] = a_parVars->m_count[1] = 0;
1583 a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL) 0;
1584 a_parVars->m_total = a_maxRects;
1585 a_parVars->m_minFill = a_minFill;
1586
1587 for( int index = 0; index < a_maxRects; ++index )
1588 {
1589 a_parVars->m_taken[index] = false;
1590 a_parVars->m_partition[index] = -1;
1591 }
1592}
1593
1594
1595RTREE_TEMPLATE
1596void RTREE_QUAL::PickSeeds( PartitionVars* a_parVars )
1597{
1598 int seed0 = 0, seed1 = 0;
1599 ELEMTYPEREAL worst, waste;
1600 ELEMTYPEREAL area[MAXNODES + 1];
1601
1602 for( int index = 0; index<a_parVars->m_total; ++index )
1603 {
1604 area[index] = CalcRectVolume( &a_parVars->m_branchBuf[index].m_rect );
1605 }
1606
1607 worst = -a_parVars->m_coverSplitArea - 1;
1608
1609 for( int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA )
1610 {
1611 for( int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB )
1612 {
1613 Rect oneRect = CombineRect( &a_parVars->m_branchBuf[indexA].m_rect,
1614 &a_parVars->m_branchBuf[indexB].m_rect );
1615 waste = CalcRectVolume( &oneRect ) - area[indexA] - area[indexB];
1616
1617 if( waste >= worst )
1618 {
1619 worst = waste;
1620 seed0 = indexA;
1621 seed1 = indexB;
1622 }
1623 }
1624 }
1625
1626 Classify( seed0, 0, a_parVars );
1627 Classify( seed1, 1, a_parVars );
1628}
1629
1630
1631// Put a branch in one of the groups.
1632RTREE_TEMPLATE
1633void RTREE_QUAL::Classify( int a_index, int a_group, PartitionVars* a_parVars )
1634{
1635 ASSERT( a_parVars );
1636 ASSERT( !a_parVars->m_taken[a_index] );
1637
1638 a_parVars->m_partition[a_index] = a_group;
1639 a_parVars->m_taken[a_index] = true;
1640
1641 if( a_parVars->m_count[a_group] == 0 )
1642 {
1643 a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
1644 }
1645 else
1646 {
1647 a_parVars->m_cover[a_group] = CombineRect( &a_parVars->m_branchBuf[a_index].m_rect,
1648 &a_parVars->m_cover[a_group] );
1649 }
1650
1651 a_parVars->m_area[a_group] = CalcRectVolume( &a_parVars->m_cover[a_group] );
1652 ++a_parVars->m_count[a_group];
1653}
1654
1655
1656// Delete a data rectangle from an index structure.
1657// Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root node.
1658// Returns 1 if record not found, 0 if success.
1659// RemoveRect provides for eliminating the root.
1660RTREE_TEMPLATE
1661bool RTREE_QUAL::RemoveRect( Rect* a_rect, const DATATYPE& a_id, Node** a_root )
1662{
1663 ASSERT( a_rect && a_root );
1664 ASSERT( *a_root );
1665
1666 Node* tempNode;
1667 ListNode* reInsertList = NULL;
1668
1669 if( !RemoveRectRec( a_rect, a_id, *a_root, &reInsertList ) )
1670 {
1671 // Found and deleted a data item
1672 // Reinsert any branches from eliminated nodes
1673 while( reInsertList )
1674 {
1675 tempNode = reInsertList->m_node;
1676
1677 for( int index = 0; index < tempNode->m_count; ++index )
1678 {
1679 InsertRect( &(tempNode->m_branch[index].m_rect),
1680 tempNode->m_branch[index].m_data,
1681 a_root,
1682 tempNode->m_level );
1683 }
1684
1685 ListNode* remLNode = reInsertList;
1686 reInsertList = reInsertList->m_next;
1687
1688 FreeNode( remLNode->m_node );
1689 FreeListNode( remLNode );
1690 }
1691
1692 // Check for redundant root (not leaf, 1 child) and eliminate
1693 if( (*a_root)->m_count == 1 && (*a_root)->IsInternalNode() )
1694 {
1695 tempNode = (*a_root)->m_branch[0].m_child;
1696
1697 ASSERT( tempNode );
1698 FreeNode( *a_root );
1699 *a_root = tempNode;
1700 }
1701
1702 return false;
1703 }
1704 else
1705 {
1706 return true;
1707 }
1708}
1709
1710
1711// Delete a rectangle from non-root part of an index structure.
1712// Called by RemoveRect. Descends tree recursively,
1713// merges branches on the way back up.
1714// Returns 1 if record not found, 0 if success.
1715RTREE_TEMPLATE
1716bool RTREE_QUAL::RemoveRectRec( Rect* a_rect,
1717 const DATATYPE& a_id,
1718 Node* a_node,
1719 ListNode** a_listNode )
1720{
1721 ASSERT( a_rect && a_node && a_listNode );
1722 ASSERT( a_node->m_level >= 0 );
1723
1724 if( a_node->IsInternalNode() ) // not a leaf node
1725 {
1726 for( int index = 0; index < a_node->m_count; ++index )
1727 {
1728 if( Overlap( a_rect, &(a_node->m_branch[index].m_rect) ) )
1729 {
1730 if( !RemoveRectRec( a_rect, a_id, a_node->m_branch[index].m_child, a_listNode ) )
1731 {
1732 if( a_node->m_branch[index].m_child->m_count >= MINNODES )
1733 {
1734 // child removed, just resize parent rect
1735 a_node->m_branch[index].m_rect =
1736 NodeCover( a_node->m_branch[index].m_child );
1737 }
1738 else
1739 {
1740 // child removed, not enough entries in node, eliminate node
1741 ReInsert( a_node->m_branch[index].m_child, a_listNode );
1742 DisconnectBranch( a_node, index ); // Must return after this call as count has changed
1743 }
1744
1745 return false;
1746 }
1747 }
1748 }
1749
1750 return true;
1751 }
1752 else // A leaf node
1753 {
1754 for( int index = 0; index < a_node->m_count; ++index )
1755 {
1756 if( a_node->m_branch[index].m_child == (Node*) a_id )
1757 {
1758 DisconnectBranch( a_node, index ); // Must return after this call as count has changed
1759 return false;
1760 }
1761 }
1762
1763 return true;
1764 }
1765}
1766
1767
1768// Decide whether two rectangles overlap.
1769RTREE_TEMPLATE
1770bool RTREE_QUAL::Overlap( Rect* a_rectA, Rect* a_rectB ) const
1771{
1772 ASSERT( a_rectA && a_rectB );
1773
1774 for( int index = 0; index < NUMDIMS; ++index )
1775 {
1776 if( a_rectA->m_min[index] > a_rectB->m_max[index]
1777 || a_rectB->m_min[index] > a_rectA->m_max[index] )
1778 {
1779 return false;
1780 }
1781 }
1782
1783 return true;
1784}
1785
1786
1787// Add a node to the reinsertion list. All its branches will later
1788// be reinserted into the index structure.
1789RTREE_TEMPLATE
1790void RTREE_QUAL::ReInsert( Node* a_node, ListNode** a_listNode )
1791{
1792 ListNode* newListNode;
1793
1794 newListNode = AllocListNode();
1795 newListNode->m_node = a_node;
1796 newListNode->m_next = *a_listNode;
1797 *a_listNode = newListNode;
1798}
1799
1800
1801// Search in an index tree or subtree for all data retangles that overlap the argument rectangle.
1802RTREE_TEMPLATE
1803bool RTREE_QUAL::Search( Node* a_node, Rect* a_rect, int& a_foundCount,
1804 std::function<bool (const DATATYPE&)> a_callback ) const
1805{
1806 ASSERT( a_node );
1807 ASSERT( a_node->m_level >= 0 );
1808 ASSERT( a_rect );
1809
1810 if( a_node->IsInternalNode() ) // This is an internal node in the tree
1811 {
1812 for( int index = 0; index < a_node->m_count; ++index )
1813 {
1814 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1815 {
1816 if( !Search( a_node->m_branch[index].m_child, a_rect, a_foundCount, a_callback ) )
1817 {
1818 return false; // Don't continue searching
1819 }
1820 }
1821 }
1822 }
1823 else // This is a leaf node
1824 {
1825 for( int index = 0; index < a_node->m_count; ++index )
1826 {
1827 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1828 {
1829 DATATYPE& id = a_node->m_branch[index].m_data;
1830 ++a_foundCount;
1831
1832 if( a_callback && !a_callback( id ) )
1833 {
1834 return false; // Don't continue searching
1835 }
1836 }
1837 }
1838 }
1839
1840 return true; // Continue searching
1841}
1842
1843
1844//calculate the minimum distance between a point and a rectangle as defined by Manolopoulos et al.
1845//it uses the square distance to avoid the use of ELEMTYPEREAL values, which are slower.
1846RTREE_TEMPLATE
1847ELEMTYPE RTREE_QUAL::MinDist( const ELEMTYPE a_point[NUMDIMS], Rect* a_rect )
1848{
1849 ELEMTYPE *q, *s, *t;
1850 q = (ELEMTYPE*) a_point;
1851 s = a_rect->m_min;
1852 t = a_rect->m_max;
1853 int minDist = 0;
1854 for( int index = 0; index < NUMDIMS; index++ )
1855 {
1856 int r = q[index];
1857 if( q[index] < s[index] )
1858 {
1859 r = s[index];
1860 }
1861 else if( q[index] >t[index] )
1862 {
1863 r = t[index];
1864 }
1865 int addend = q[index] - r;
1866 minDist += addend * addend;
1867 }
1868 return minDist;
1869}
1870
1871
1872//insert a NNNode in a list sorted by its minDist (desc.)
1873RTREE_TEMPLATE
1874void RTREE_QUAL::InsertNNListSorted( std::vector<NNNode*>* nodeList, NNNode* newNode )
1875{
1876 typedef typename std::vector<NNNode*>::iterator iterator;
1877 iterator iter = nodeList->begin();
1878 while( iter != nodeList->end() && (*iter)->minDist > newNode->minDist )
1879 {
1880 ++iter;
1881 }
1882 nodeList->insert(iter, newNode);
1883}
1884
1885
1886#undef RTREE_TEMPLATE
1887#undef RTREE_QUAL
1888#undef RTREE_SEARCH_TEMPLATE
1889#undef RTREE_SEARCH_QUAL
1890
1891#endif // RTREE_H
Definition: rtree.h:525
Iterator is not remove safe.
Definition: rtree.h:202
bool IsNull()
Is iterator invalid.
Definition: rtree.h:219
void GetBounds(ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS])
Get the bounds for this node.
Definition: rtree.h:244
const DATATYPE & operator*() const
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:233
bool IsNotNull()
Is iterator pointing to valid data.
Definition: rtree.h:222
DATATYPE & operator*()
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:225
bool operator++()
Find the next data element.
Definition: rtree.h:241
Implementation of RTree, a multidimensional bounding rectangle tree.
Definition: rtree.h:77
int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], std::function< bool(const DATATYPE &)> a_callback) const
Find all within search rectangle.
void RemoveAll()
Remove all entries from tree.
void GetNext(Iterator &a_it)
Get Next for iteration.
Definition: rtree.h:359
Node * m_root
Root of tree.
Definition: rtree.h:517
bool Save(const char *a_fileName)
Save tree contents to file.
bool Save(RTFileStream &a_stream)
Save tree contents to stream.
Statistics CalcStats()
Calculate Statistics.
@ MINNODES
Min elements in node.
Definition: rtree.h:87
@ MAXNODES
Max elements in node.
Definition: rtree.h:86
int Count()
Count the data elements in this container. This is slow as no internal counter is maintained.
bool IsNull(Iterator &a_it)
Is iterator NULL, or at end?
Definition: rtree.h:362
DATATYPE & GetAt(Iterator &a_it)
Get object at iterator position.
Definition: rtree.h:365
void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Insert entry.
DATATYPE NearestNeighbor(const ELEMTYPE a_point[NUMDIMS], ELEMTYPE a_squareDistanceCallback(const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data), ELEMTYPE *a_squareDistance)
Find the nearest neighbor of a specific point.
DATATYPE NearestNeighbor(const ELEMTYPE a_point[NUMDIMS])
Find the nearest neighbor of a specific point.
void GetFirst(Iterator &a_it)
Get 'first' for iteration.
Definition: rtree.h:333
bool Load(RTFileStream &a_stream)
Load tree contents from stream.
bool Load(const char *a_fileName)
Load tree contents from file.
ELEMTYPEREAL m_unitSphereVolume
Unit sphere constant for required number of dimensions.
Definition: rtree.h:518
bool Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Remove entry.
May be data or may be another subtree The parents level determines this.
Definition: rtree.h:379
Rect m_rect
Bounds.
Definition: rtree.h:380
Node * m_child
Child node.
Definition: rtree.h:383
DATATYPE m_data
Data Id or Ptr.
Definition: rtree.h:384
A link list of nodes for reinsertion after a delete operation.
Definition: rtree.h:401
ListNode * m_next
Next in list.
Definition: rtree.h:402
Node * m_node
Node.
Definition: rtree.h:403
Data structure used for Nearest Neighbor search implementation.
Definition: rtree.h:425
Node for each branch level.
Definition: rtree.h:390
int m_level
Leaf is zero, others positive.
Definition: rtree.h:395
int m_count
Count.
Definition: rtree.h:394
Branch m_branch[MAXNODES]
Branch.
Definition: rtree.h:396
Variables for finding a split partition.
Definition: rtree.h:408
Minimal bounding rectangle (n-dimensional)
Definition: rtree.h:370
ELEMTYPE m_min[NUMDIMS]
Min dimensions of bounding box.
Definition: rtree.h:371
ELEMTYPE m_max[NUMDIMS]
Max dimensions of bounding box.
Definition: rtree.h:372
Definition: rtree.h:90