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