KiCad PCB EDA Suite
rtree.h
Go to the documentation of this file.
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
55 class RTFileStream; // File I/O helper class, look below for implementation and notes.
56 
57 
74 template <class DATATYPE, class ELEMTYPE, int NUMDIMS,
75  class ELEMTYPEREAL = ELEMTYPE, int TMAXNODES = 8, int TMINNODES = TMAXNODES / 2>
76 class RTree
77 {
78 protected:
79 
80  struct Node; // Fwd decl. Used by other internal structs and iterator
81 public:
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 
97  };
98 
99 public:
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 
161  Statistics CalcStats();
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 
201  class Iterator
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 
208  {
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 
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 
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; }
366 protected:
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 
394  int m_count;
395  int m_level;
396  Branch m_branch[MAXNODES];
397  };
398 
400  struct ListNode
401  {
404  };
405 
408  {
409  int m_partition[MAXNODES + 1];
410  int m_total;
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];
420  ELEMTYPEREAL m_coverSplitArea;
421  };
422 
424  struct NNNode
425  {
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 );
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 
517  Node* m_root;
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;
527 public:
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 
603 RTREE_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 
632 RTREE_QUAL::~RTree() {
633  Reset(); // Free, or reset node memory
634 }
635 
636 
638 void 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 
664 bool 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 
690 int 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 
720 DATATYPE RTREE_QUAL::NearestNeighbor( const ELEMTYPE a_point[NUMDIMS] )
721 {
722  return this->NearestNeighbor( a_point, 0, 0 );
723 }
724 
725 
727 DATATYPE 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 
774 int RTREE_QUAL::Count()
775 {
776  int count = 0;
777 
778  CountRec( m_root, count );
779 
780  return count;
781 }
782 
783 
785 void 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 
802 bool 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 
822 bool 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 
870 bool 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 
906 bool 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 
924 bool 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 
951 bool 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 
986 void 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 
997 void 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 
1010 void 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 
1028 typename 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 
1043 void 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.
1058 typename 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 
1069 void 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 
1080 void RTREE_QUAL::InitNode( Node* a_node )
1081 {
1082  a_node->m_count = 0;
1083  a_node->m_level = -1;
1084 }
1085 
1086 
1088 void 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.
1106 bool 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 //
1163 bool 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.
1200 typename 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.
1230 bool 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
1255 void 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.
1273 int 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
1313 typename 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.
1334 void 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
1362 ELEMTYPEREAL 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
1381 ELEMTYPEREAL 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
1415 ELEMTYPEREAL 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.
1427 void 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.
1470 void 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.
1554 void 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.
1578 void 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 
1596 void 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.
1633 void 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.
1661 bool 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.
1716 bool 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.
1770 bool 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.
1790 void 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.
1803 bool 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.
1847 ELEMTYPE 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.)
1874 void 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
int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], VISITOR &a_visitor)
Definition: rtree.h:131
bool Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Remove entry.
int PickBranch(Rect *a_rect, Node *a_node)
Min elements in node.
Definition: rtree.h:87
#define RTREE_TEMPLATE
Definition: rtree.h:42
bool isLeaf
Definition: rtree.h:428
void GetNext(Iterator &a_it)
Get Next for iteration.
Definition: rtree.h:359
DATATYPE m_data
Data Id or Ptr.
Definition: rtree.h:384
bool OpenWrite(const char *a_fileName)
Definition: rtree.h:552
bool InsertRectRec(Rect *a_rect, const DATATYPE &a_id, Node *a_node, Node **a_newNode, int a_level)
void PickSeeds(PartitionVars *a_parVars)
FILE * m_file
Definition: rtree.h:526
StackElement & Pop()
Pop element off iteration stack (For internal use only)
Definition: rtree.h:318
bool OpenRead(const char *a_fileName)
Definition: rtree.h:540
void Close()
Definition: rtree.h:564
bool RemoveRect(Rect *a_rect, const DATATYPE &a_id, Node **a_root)
May be data or may be another subtree The parents level determines this.
Definition: rtree.h:378
int maxNodeLoad
Definition: rtree.h:94
Node for each branch level.
Definition: rtree.h:389
ELEMTYPEREAL RectVolume(Rect *a_rect)
const DATATYPE & operator*() const
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:233
int Count()
Count the data elements in this container. This is slow as no internal counter is maintained...
bool Search(Node *a_node, Rect *a_rect, VISITOR &a_visitor, int &a_foundCount)
Definition: rtree.h:472
Minimal bounding rectangle (n-dimensional)
Definition: rtree.h:369
ELEMTYPE m_min[NUMDIMS]
Min dimensions of bounding box.
Definition: rtree.h:371
Node * m_root
Root of tree.
Definition: rtree.h:517
ELEMTYPE MinDist(const ELEMTYPE a_point[NUMDIMS], Rect *a_rect)
size_t Read(TYPE &a_value)
Definition: rtree.h:588
void ChoosePartition(PartitionVars *a_parVars, int a_minFill)
ELEMTYPEREAL RectSphericalVolume(Rect *a_rect)
virtual ~RTree()
Statistics CalcStats()
Calculate Statistics.
void RemoveAll()
Remove all entries from tree.
bool Overlap(Rect *a_rectA, Rect *a_rectB) const
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 FreeListNode(ListNode *a_listNode)
void Init()
Reset iterator.
Definition: rtree.h:260
void InitParVars(PartitionVars *a_parVars, int a_maxRects, int a_minFill)
ELEMTYPE m_max[NUMDIMS]
Max dimensions of bounding box.
Definition: rtree.h:372
bool IsLeaf()
Definition: rtree.h:392
static struct PcbQueue * Save
Definition: queue.cpp:56
Rect NodeCover(Node *a_node)
Node * m_node
Node.
Definition: rtree.h:403
DATATYPE NearestNeighbor(const ELEMTYPE a_point[NUMDIMS])
Find the nearest neighbor of a specific point.
Node * m_child
Child node.
Definition: rtree.h:383
void SplitNode(Node *a_node, Branch *a_branch, Node **a_newNode)
A link list of nodes for reinsertion after a delete operation.
Definition: rtree.h:400
int totalItems
Definition: rtree.h:96
int m_count
Count.
Definition: rtree.h:394
ListNode * m_next
Next in list.
Definition: rtree.h:402
Rect m_rect
Bounds.
Definition: rtree.h:380
int avgNodeLoad
Definition: rtree.h:95
bool InsertRect(Rect *a_rect, const DATATYPE &a_id, Node **a_root, int a_level)
void LoadNodes(Node *a_nodeA, Node *a_nodeB, PartitionVars *a_parVars)
void DisconnectBranch(Node *a_node, int a_index)
void Reset()
bool SaveRec(Node *a_node, RTFileStream &a_stream)
bool IsNull()
Is iterator invalid.
Definition: rtree.h:219
ELEMTYPEREAL m_unitSphereVolume
Unit sphere constant for required number of dimensions.
Definition: rtree.h:518
bool RemoveRectRec(Rect *a_rect, const DATATYPE &a_id, Node *a_node, ListNode **a_listNode)
bool IsNotNull()
Is iterator pointing to valid data.
Definition: rtree.h:222
ELEMTYPEREAL CalcRectVolume(Rect *a_rect)
void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Insert entry.
DATATYPE & operator*()
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:225
int m_level
Leaf is zero, others positive.
Definition: rtree.h:395
Implementation of RTree, a multidimensional bounding rectangle tree.
Definition: rtree.h:76
Node * AllocNode()
void GetFirst(Iterator &a_it)
Get &#39;first&#39; for iteration.
Definition: rtree.h:333
bool IsNull(Iterator &a_it)
Is iterator NULL, or at end?
Definition: rtree.h:362
bool operator++()
Find the next data element.
Definition: rtree.h:241
DATATYPE & GetAt(Iterator &a_it)
Get object at iterator position.
Definition: rtree.h:365
Variables for finding a split partition.
Definition: rtree.h:407
void CountRec(Node *a_node, int &a_count)
Rect CombineRect(Rect *a_rectA, Rect *a_rectB)
Branch m_branch[MAXNODES]
Branch.
Definition: rtree.h:396
bool Save(const char *a_fileName)
Save tree contents to file.
void ReInsert(Node *a_node, ListNode **a_listNode)
void RemoveAllRec(Node *a_node)
void GetBounds(ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS])
Get the bounds for this node.
Definition: rtree.h:244
#define max(a, b)
Definition: auxiliary.h:86
void InsertNNListSorted(std::vector< NNNode * > *nodeList, NNNode *newNode)
Max elements in node.
Definition: rtree.h:86
ListNode * AllocListNode()
void FreeNode(Node *a_node)
bool Load(const char *a_fileName)
Load tree contents from file.
ELEMTYPE minDist
Definition: rtree.h:427
bool IsInternalNode()
Definition: rtree.h:391
void InitNode(Node *a_node)
void Push(Node *a_node, int a_branchIndex)
Push node and branch onto iteration stack (For internal use only)
Definition: rtree.h:309
KICAD_PLUGIN_EXPORT SCENEGRAPH * Load(char const *aFileName)
reads a model file and creates a generic display structure
Data structure used for Nearest Neighbor search implementation.
Definition: rtree.h:424
void Classify(int a_index, int a_group, PartitionVars *a_parVars)
~RTFileStream()
Definition: rtree.h:535
bool FindNextData()
Find the next data element in the tree (For internal use only)
Definition: rtree.h:263
bool LoadRec(Node *a_node, RTFileStream &a_stream)
size_t ReadArray(TYPE *a_array, int a_count)
Definition: rtree.h:595
#define ASSERT
Definition: rtree.h:36
size_t WriteArray(const TYPE *a_array, int a_count)
Definition: rtree.h:581
size_t Write(const TYPE &a_value)
Definition: rtree.h:574
Branch m_branch
Definition: rtree.h:426
bool AddBranch(Branch *a_branch, Node *a_node, Node **a_newNode)
Iterator is not remove safe.
Definition: rtree.h:201
void GetBranches(Node *a_node, Branch *a_branch, PartitionVars *a_parVars)
int m_tos
Top Of Stack index.
Definition: rtree.h:326
void InitRect(Rect *a_rect)
ELEMTYPEREAL m_coverSplitArea
Definition: rtree.h:420
RTFileStream()
Definition: rtree.h:530
#define min(a, b)
Definition: auxiliary.h:85