Horizon
Loading...
Searching...
No Matches
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
88class RTFileStream; // File I/O helper class, look below for implementation and notes.
89
90
107template <class DATATYPE, class ELEMTYPE, int NUMDIMS,
108 class ELEMTYPEREAL = ELEMTYPE, int TMAXNODES = 8, int TMINNODES = TMAXNODES / 2>
109class RTree
110{
111protected:
112
113 struct Node; // Fwd decl. Used by other internal structs and iterator
114
115public:
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
139public:
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
245public:
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
465protected:
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
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;
623public:
624
625
627 {
628 m_file = NULL;
629 }
630
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
699RTREE_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
727RTREE_TEMPLATE
728RTREE_QUAL::~RTree() {
729 Reset(); // Free, or reset node memory
730}
731
732
733RTREE_TEMPLATE
734void 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
759RTREE_TEMPLATE
760bool 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
785RTREE_TEMPLATE
786int 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
815RTREE_TEMPLATE
816int 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
844RTREE_TEMPLATE
845std::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
906RTREE_TEMPLATE
907int RTREE_QUAL::Count() const
908{
909 int count = 0;
910
911 CountRec( m_root, count );
912
913 return count;
914}
915
916
917RTREE_TEMPLATE
918void 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
934RTREE_TEMPLATE
935bool 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
954RTREE_TEMPLATE
955bool 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
1002RTREE_TEMPLATE
1003bool 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
1038RTREE_TEMPLATE
1039bool 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
1056RTREE_TEMPLATE
1057bool 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
1083RTREE_TEMPLATE
1084bool 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
1118RTREE_TEMPLATE
1119void 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
1129RTREE_TEMPLATE
1130void 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
1142RTREE_TEMPLATE
1143void 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
1160RTREE_TEMPLATE
1161typename 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
1175RTREE_TEMPLATE
1176void 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.
1190RTREE_TEMPLATE
1191typename 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
1201RTREE_TEMPLATE
1202void 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
1212RTREE_TEMPLATE
1213void RTREE_QUAL::InitNode( Node* a_node ) const
1214{
1215 a_node->m_count = 0;
1216 a_node->m_level = -1;
1217}
1218
1219
1220RTREE_TEMPLATE
1221void 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.
1238RTREE_TEMPLATE
1239bool 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//
1295RTREE_TEMPLATE
1296bool 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.
1332RTREE_TEMPLATE
1333typename 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.
1362RTREE_TEMPLATE
1363bool 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
1387RTREE_TEMPLATE
1388void 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.
1405RTREE_TEMPLATE
1406int 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
1445RTREE_TEMPLATE
1446typename 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.
1466RTREE_TEMPLATE
1467void 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
1494RTREE_TEMPLATE
1495ELEMTYPEREAL 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
1513RTREE_TEMPLATE
1514ELEMTYPEREAL 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
1547RTREE_TEMPLATE
1548ELEMTYPEREAL 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.
1559RTREE_TEMPLATE
1560void 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.
1602RTREE_TEMPLATE
1603void 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.
1686RTREE_TEMPLATE
1687void 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.
1710RTREE_TEMPLATE
1711void 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
1728RTREE_TEMPLATE
1729void 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.
1765RTREE_TEMPLATE
1766void 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.
1793RTREE_TEMPLATE
1794bool 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.
1848RTREE_TEMPLATE
1849bool 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.
1902RTREE_TEMPLATE
1903bool 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.
1922RTREE_TEMPLATE
1923void 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.
1935RTREE_TEMPLATE
1936bool 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
1979RTREE_TEMPLATE
1980ELEMTYPE 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
Iterator operator++(int)
Postfix ++ operator.
Definition rtree.h:323
const DATATYPE & operator*() const
Access the current data element. Caller must be sure iterator is not NULL first.
Definition rtree.h:301
DATATYPE & operator*()
Access the current data element. Caller must be sure iterator is not NULL first.
Definition rtree.h:293
Iterator & operator++()
Prefix ++ operator.
Definition rtree.h:316
constexpr bool IsNotNull() const
Is iterator pointing to valid data.
Definition rtree.h:287
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.
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.
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 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.
_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