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, \
81#define RTREE_SEARCH_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
84#define RTREE_DONT_USE_MEMPOOLS
85#define RTREE_USE_SPHERICAL_VOLUME
107template <
class DATATYPE,
class ELEMTYPE,
int NUMDIMS,
108 class ELEMTYPEREAL = ELEMTYPE,
int TMAXNODES = 8,
int TMINNODES = TMAXNODES / 2>
148 void Insert(
const ELEMTYPE a_min[NUMDIMS],
149 const ELEMTYPE a_max[NUMDIMS],
150 const DATATYPE& a_dataId );
157 bool Remove(
const ELEMTYPE a_min[NUMDIMS],
158 const ELEMTYPE a_max[NUMDIMS],
159 const DATATYPE& a_dataId );
166 int Search(
const ELEMTYPE a_min[NUMDIMS],
167 const ELEMTYPE a_max[NUMDIMS],
168 std::function<
bool (
const DATATYPE&)> a_callback )
const;
176 int Search(
const ELEMTYPE a_min[NUMDIMS],
const ELEMTYPE a_max[NUMDIMS],
177 std::function<
bool(
const DATATYPE& )> a_callback,
bool& aFinished )
const;
179 template <
class VISITOR>
180 int Search(
const ELEMTYPE a_min[NUMDIMS],
const ELEMTYPE a_max[NUMDIMS], VISITOR& a_visitor )
const
184 for(
int index = 0; index < NUMDIMS; ++index )
186 ASSERT( a_min[index] <= a_max[index] );
193 for(
int axis = 0; axis < NUMDIMS; ++axis )
195 rect.m_min[axis] = a_min[axis];
196 rect.m_max[axis] = a_max[axis];
219 bool Load(
const char* a_fileName );
226 bool Save(
const char* a_fileName );
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;
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;
269 Iterator() : m_stack( {} ), m_tos( 0 )
271 for(
int i = 0; i < NUMDIMS; ++i )
273 m_rect.
m_min[i] = std::numeric_limits<ELEMTYPE>::min();
274 m_rect.
m_max[i] = std::numeric_limits<ELEMTYPE>::max();
278 Iterator(
const Rect& aRect ) : m_stack( {} ), m_tos( 0 ), m_rect( aRect )
296 StackElement& curTos = m_stack[m_tos - 1];
297 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
304 StackElement& curTos = m_stack[m_tos - 1];
305 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
308 DATATYPE* operator->()
311 StackElement& curTos = m_stack[m_tos - 1];
312 return &( curTos.m_node->m_branch[curTos.m_branchIndex].m_data );
330 bool operator==(
const Iterator& rhs )
const
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 ) );
338 bool operator!=(
const Iterator& rhs )
const
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 ) );
352 StackElement curTos = Pop();
353 int nextBranch = curTos.m_branchIndex + 1;
355 if( curTos.m_node->IsLeaf() )
358 for(
int i = nextBranch; i < curTos.m_node->m_count; i++ )
360 if( RTree::Overlap( &m_rect, &curTos.m_node->m_branch[i].m_rect ) )
362 Push( curTos.m_node, i );
372 for(
int i = nextBranch; i < curTos.m_node->m_count; i++ )
374 if( RTree::Overlap( &m_rect, &curTos.m_node->m_branch[i].m_rect ) )
376 Push( curTos.m_node, i );
381 Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
385 Push( nextLevelnode, 0 );
390 if( nextLevelnode->IsLeaf()
391 && RTree::Overlap( &m_rect, &nextLevelnode->m_branch[0].m_rect ) )
398 void Push( Node* a_node,
int a_branchIndex )
400 m_stack[m_tos].m_node = a_node;
401 m_stack[m_tos].m_branchIndex = a_branchIndex;
403 ASSERT( m_tos <= MAX_STACK );
411 return m_stack[m_tos];
414 std::array<StackElement, MAX_STACK> m_stack;
422 using iterator = Iterator;
423 using const_iterator =
const Iterator;
425 iterator begin(
const Rect& aRect )
const
427 iterator retval( aRect );
443 iterator begin()
const
447 std::fill_n( full_rect.m_min, NUMDIMS, INT_MIN );
448 std::fill_n( full_rect.m_max, NUMDIMS, INT_MAX );
450 return begin( full_rect );
459 iterator end(
const Rect& aRect )
const
482 constexpr bool IsInternalNode()
const {
return m_level > 0; }
483 constexpr bool IsLeaf()
const {
return m_level == 0; }
506 ELEMTYPEREAL m_area[2];
511 ELEMTYPEREAL m_coverSplitArea;
524 return other.minDist < minDist;
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,
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,
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;
564 bool Search(
const Node* a_node,
const Rect* a_rect,
int& a_foundCount,
565 std::function<
bool (
const DATATYPE&)> a_callback )
const;
567 template <
class VISITOR>
568 bool Search(
const Node* a_node,
const Rect* a_rect, VISITOR& a_visitor,
int& a_foundCount )
const
571 ASSERT( a_node->m_level >= 0 );
574 if( a_node->IsInternalNode() )
576 for(
int index = 0; index < a_node->m_count; ++index )
578 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
580 if( !
Search( a_node->m_branch[index].m_child, a_rect, a_visitor, a_foundCount ) )
589 for(
int index = 0; index < a_node->m_count; ++index )
591 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
593 const DATATYPE&
id = a_node->m_branch[index].m_data;
595 if( !a_visitor(
id ) )
606 void RemoveAllRec( Node* a_node )
const;
608 void CountRec(
const Node* a_node,
int& a_count )
const;
610 bool SaveRec(
const Node* a_node,
RTFileStream& a_stream )
const;
611 bool LoadRec(
const Node* a_node,
RTFileStream& a_stream )
const;
636 bool OpenRead(
const char* a_fileName )
638 m_file = std::fopen( a_fileName,
"rb" );
648 bool OpenWrite(
const char* a_fileName )
650 m_file = std::fopen( a_fileName,
"wb" );
664 std::fclose( m_file );
669 template <
typename TYPE>
670 size_t Write(
const TYPE& a_value )
673 return std::fwrite( (
void*) &a_value,
sizeof(a_value), 1, m_file );
676 template <
typename TYPE>
677 size_t WriteArray(
const TYPE* a_array,
int a_count )
680 return std::fwrite( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
683 template <
typename TYPE>
684 size_t Read( TYPE& a_value )
687 return std::fread( (
void*) &a_value,
sizeof(a_value), 1, m_file );
690 template <
typename TYPE>
691 size_t ReadArray( TYPE* a_array,
int a_count )
694 return std::fread( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
699RTREE_TEMPLATE RTREE_QUAL::RTree()
701 ASSERT( MAXNODES > MINNODES );
702 ASSERT( MINNODES > 0 );
707 ASSERT(
sizeof(DATATYPE) ==
sizeof(
void*) ||
sizeof(DATATYPE) ==
sizeof(
int) );
710 const float UNIT_SPHERE_VOLUMES[] =
712 0.000000f, 2.000000f, 3.141593f,
713 4.188790f, 4.934802f, 5.263789f,
714 5.167713f, 4.724766f, 4.058712f,
715 3.298509f, 2.550164f, 1.884104f,
716 1.335263f, 0.910629f, 0.599265f,
717 0.381443f, 0.235331f, 0.140981f,
718 0.082146f, 0.046622f, 0.025807f,
721 m_root = AllocNode();
723 m_unitSphereVolume = (ELEMTYPEREAL) UNIT_SPHERE_VOLUMES[NUMDIMS];
728RTREE_QUAL::~RTree() {
734void RTREE_QUAL::Insert(
const ELEMTYPE a_min[NUMDIMS],
735 const ELEMTYPE a_max[NUMDIMS],
736 const DATATYPE& a_dataId )
740 for(
int index = 0; index<NUMDIMS; ++index )
742 ASSERT( a_min[index] <= a_max[index] );
749 for(
int axis = 0; axis < NUMDIMS; ++axis )
751 rect.m_min[axis] = a_min[axis];
752 rect.m_max[axis] = a_max[axis];
755 InsertRect( &rect, a_dataId, &m_root, 0 );
760bool RTREE_QUAL::Remove(
const ELEMTYPE a_min[NUMDIMS],
761 const ELEMTYPE a_max[NUMDIMS],
762 const DATATYPE& a_dataId )
766 for(
int index = 0; index<NUMDIMS; ++index )
768 ASSERT( a_min[index] <= a_max[index] );
775 for(
int axis = 0; axis < NUMDIMS; ++axis )
777 rect.m_min[axis] = a_min[axis];
778 rect.m_max[axis] = a_max[axis];
781 return RemoveRect( &rect, a_dataId, &m_root );
786int RTREE_QUAL::Search(
const ELEMTYPE a_min[NUMDIMS],
787 const ELEMTYPE a_max[NUMDIMS],
788 std::function<
bool (
const DATATYPE&)> a_callback )
const
792 for(
int index = 0; index < NUMDIMS; ++index )
794 ASSERT( a_min[index] <= a_max[index] );
801 for(
int axis = 0; axis < NUMDIMS; ++axis )
803 rect.m_min[axis] = a_min[axis];
804 rect.m_max[axis] = a_max[axis];
810 Search( m_root, &rect, foundCount, a_callback );
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
821 for(
int index = 0; index < NUMDIMS; ++index )
823 ASSERT( a_min[index] <= a_max[index] );
830 for(
int axis = 0; axis < NUMDIMS; ++axis )
832 rect.m_min[axis] = a_min[axis];
833 rect.m_max[axis] = a_max[axis];
839 aFinished = Search( m_root, &rect, foundCount, a_callback );
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
851 std::vector<std::pair<ELEMTYPE, DATATYPE>> result;
852 std::priority_queue<NNNode> search_q;
854 for(
int i = 0; i < m_root->m_count; ++i )
856 if( m_root->IsLeaf() )
858 search_q.push( NNNode{ m_root->m_branch[i],
859 aSquaredDist( a_point, m_root->m_branch[i].m_data ),
864 search_q.push( NNNode{ m_root->m_branch[i],
865 MinDist(a_point, m_root->m_branch[i].m_rect),
870 while( !search_q.empty() )
872 const NNNode curNode = search_q.top();
874 if( aTerminate( result.size(), curNode.minDist ) )
881 if( aFilter( curNode.m_branch.m_data ) )
882 result.emplace_back( curNode.minDist, curNode.m_branch.m_data );
886 Node* node = curNode.m_branch.m_child;
888 for(
int i = 0; i < node->m_count; ++i )
891 newNode.isLeaf = node->IsLeaf();
892 newNode.m_branch = node->m_branch[i];
894 newNode.minDist = aSquaredDist( a_point, newNode.m_branch.m_data );
896 newNode.minDist = this->MinDist( a_point, node->m_branch[i].m_rect );
898 search_q.push( newNode );
907int RTREE_QUAL::Count()
const
911 CountRec( m_root, count );
918void RTREE_QUAL::CountRec(
const Node* a_node,
int& a_count )
const
920 if( a_node->IsInternalNode() )
922 for(
int index = 0; index < a_node->m_count; ++index )
924 CountRec( a_node->m_branch[index].m_child, a_count );
929 a_count += a_node->m_count;
935bool RTREE_QUAL::Load(
const char* a_fileName )
941 if( !stream.OpenRead( a_fileName ) )
946 bool result = Load( stream );
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;
969 int dataElemSize = 0;
970 int dataElemRealSize = 0;
971 int dataMaxNodes = 0;
972 int dataMinNodes = 0;
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 );
985 if( (dataFileId == _dataFileId)
986 && (dataSize == _dataSize)
987 && (dataNumDims == _dataNumDims)
988 && (dataElemSize == _dataElemSize)
989 && (dataElemRealSize == _dataElemRealSize)
990 && (dataMaxNodes == _dataMaxNodes)
991 && (dataMinNodes == _dataMinNodes)
995 result = LoadRec( m_root, a_stream );
1003bool RTREE_QUAL::LoadRec(
const Node* a_node,
RTFileStream& a_stream )
const
1005 a_stream.Read( a_node->m_level );
1006 a_stream.Read( a_node->m_count );
1008 if( a_node->IsInternalNode() )
1010 for(
int index = 0; index < a_node->m_count; ++index )
1012 const Branch* curBranch = &a_node->m_branch[index];
1014 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
1015 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
1017 curBranch->m_child = AllocNode();
1018 LoadRec( curBranch->m_child, a_stream );
1023 for(
int index = 0; index < a_node->m_count; ++index )
1025 const Branch* curBranch = &a_node->m_branch[index];
1027 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
1028 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
1030 a_stream.Read( curBranch->m_data );
1039bool RTREE_QUAL::Save(
const char* a_fileName )
1043 if( !stream.OpenWrite( a_fileName ) )
1048 bool result = Save( stream );
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;
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 );
1077 bool result = SaveRec( m_root, a_stream );
1084bool RTREE_QUAL::SaveRec(
const Node* a_node,
RTFileStream& a_stream )
const
1086 a_stream.Write( a_node->m_level );
1087 a_stream.Write( a_node->m_count );
1089 if( a_node->IsInternalNode() )
1091 for(
int index = 0; index < a_node->m_count; ++index )
1093 const Branch* curBranch = &a_node->m_branch[index];
1095 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
1096 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
1098 SaveRec( curBranch->m_child, a_stream );
1103 for(
int index = 0; index < a_node->m_count; ++index )
1105 const Branch* curBranch = &a_node->m_branch[index];
1107 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
1108 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
1110 a_stream.Write( curBranch->m_data );
1119void RTREE_QUAL::RemoveAll()
1124 m_root = AllocNode();
1125 m_root->m_level = 0;
1130void RTREE_QUAL::Reset()
const
1132#ifdef RTREE_DONT_USE_MEMPOOLS
1134 RemoveAllRec( m_root );
1143void RTREE_QUAL::RemoveAllRec( Node* a_node )
const
1146 ASSERT( a_node->m_level >= 0 );
1148 if( a_node->IsInternalNode() )
1150 for(
int index = 0; index < a_node->m_count; ++index )
1152 RemoveAllRec( a_node->m_branch[index].m_child );
1161typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
const
1165#ifdef RTREE_DONT_USE_MEMPOOLS
1170 InitNode( newNode );
1176void RTREE_QUAL::FreeNode( Node* a_node )
const
1180#ifdef RTREE_DONT_USE_MEMPOOLS
1191typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
const
1193#ifdef RTREE_DONT_USE_MEMPOOLS
1194 return new ListNode;
1202void RTREE_QUAL::FreeListNode( ListNode* a_listNode )
const
1204#ifdef RTREE_DONT_USE_MEMPOOLS
1213void RTREE_QUAL::InitNode( Node* a_node )
const
1215 a_node->m_count = 0;
1216 a_node->m_level = -1;
1221void RTREE_QUAL::InitRect( Rect* a_rect )
const
1223 for(
int index = 0; index < NUMDIMS; ++index )
1225 a_rect->m_min[index] = (ELEMTYPE) 0;
1226 a_rect->m_max[index] = (ELEMTYPE) 0;
1239bool RTREE_QUAL::InsertRectRec(
const Rect* a_rect,
1240 const DATATYPE& a_id,
1245 ASSERT( a_rect && a_node && a_newNode );
1246 ASSERT( a_level >= 0 && a_level <= a_node->m_level );
1253 if( a_node->m_level > a_level )
1255 index = PickBranch( a_rect, a_node );
1257 if( !InsertRectRec( a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level ) )
1260 a_node->m_branch[index].m_rect =
1261 CombineRect( a_rect, &(a_node->m_branch[index].m_rect) );
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 );
1272 else if( a_node->m_level == a_level )
1274 branch.m_rect = *a_rect;
1275 branch.m_child = (Node*) a_id;
1277 return AddBranch( &branch, a_node, a_newNode );
1296bool RTREE_QUAL::InsertRect(
const Rect* a_rect,
const DATATYPE& a_id, Node** a_root,
int a_level )
const
1298 ASSERT( a_rect && a_root );
1299 ASSERT( a_level >= 0 && a_level <= (*a_root)->m_level );
1302 for(
int index = 0; index < NUMDIMS; ++index )
1304 ASSERT( a_rect->m_min[index] <= a_rect->m_max[index] );
1313 if( InsertRectRec( a_rect, a_id, *a_root, &newNode, a_level ) )
1315 newRoot = AllocNode();
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 );
1333typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover( Node* a_node )
const
1337 bool firstTime =
true;
1341 for(
int index = 0; index < a_node->m_count; ++index )
1345 rect = a_node->m_branch[index].m_rect;
1350 rect = CombineRect( &rect, &(a_node->m_branch[index].m_rect) );
1363bool RTREE_QUAL::AddBranch(
const Branch* a_branch, Node* a_node, Node** a_newNode )
const
1368 if( a_node->m_count < MAXNODES )
1370 a_node->m_branch[a_node->m_count] = *a_branch;
1377 ASSERT( a_newNode );
1379 SplitNode( a_node, a_branch, a_newNode );
1388void RTREE_QUAL::DisconnectBranch( Node* a_node,
int a_index )
const
1390 ASSERT( a_node && (a_index >= 0) && (a_index < MAXNODES) );
1391 ASSERT( a_node->m_count > 0 );
1394 a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
1406int RTREE_QUAL::PickBranch(
const Rect* a_rect, Node* a_node )
const
1408 ASSERT( a_rect && a_node );
1410 bool firstTime =
true;
1411 ELEMTYPEREAL increase;
1412 ELEMTYPEREAL bestIncr = (ELEMTYPEREAL) -1;
1414 ELEMTYPEREAL bestArea = 0;
1418 for(
int index = 0; index < a_node->m_count; ++index )
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;
1425 if( (increase < bestIncr) || firstTime )
1429 bestIncr = increase;
1432 else if( (increase == bestIncr) && (area < bestArea) )
1436 bestIncr = increase;
1446typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect(
const Rect* a_rectA,
const Rect* a_rectB )
const
1448 ASSERT( a_rectA && a_rectB );
1452 for(
int index = 0; index < NUMDIMS; ++index )
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] );
1467void RTREE_QUAL::SplitNode( Node* a_node,
const Branch* a_branch, Node** a_newNode )
const
1473 PartitionVars localVars;
1474 PartitionVars* parVars = &localVars;
1478 level = a_node->m_level;
1479 GetBranches( a_node, a_branch, parVars );
1482 ChoosePartition( parVars, MINNODES );
1485 *a_newNode = AllocNode();
1486 (*a_newNode)->m_level = a_node->m_level = level;
1487 LoadNodes( a_node, *a_newNode, parVars );
1489 ASSERT( (a_node->m_count + (*a_newNode)->m_count) == parVars->m_total );
1495ELEMTYPEREAL RTREE_QUAL::RectVolume(
const Rect* a_rect )
const
1499 ELEMTYPEREAL volume = (ELEMTYPEREAL) 1;
1501 for(
int index = 0; index<NUMDIMS; ++index )
1503 volume *= a_rect->m_max[index] - a_rect->m_min[index];
1506 ASSERT( volume >= (ELEMTYPEREAL) 0 );
1514ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume(
const Rect* a_rect )
const
1518 ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL) 0;
1519 ELEMTYPEREAL radius;
1521 for(
int index = 0; index < NUMDIMS; ++index )
1523 ELEMTYPEREAL halfExtent =
1524 ( (ELEMTYPEREAL) a_rect->m_max[index] - (ELEMTYPEREAL) a_rect->m_min[index] ) * 0.5f;
1525 sumOfSquares += halfExtent * halfExtent;
1528 radius = (ELEMTYPEREAL) std::sqrt( sumOfSquares );
1533 return radius * radius * radius * m_unitSphereVolume;
1535 else if( NUMDIMS == 2 )
1537 return radius * radius * m_unitSphereVolume;
1541 return (ELEMTYPEREAL) (std::pow( radius, NUMDIMS ) * m_unitSphereVolume);
1548ELEMTYPEREAL RTREE_QUAL::CalcRectVolume(
const Rect* a_rect )
const
1550#ifdef RTREE_USE_SPHERICAL_VOLUME
1551 return RectSphericalVolume( a_rect );
1553 return RectVolume( a_rect );
1560void RTREE_QUAL::GetBranches( Node* a_node,
const Branch* a_branch, PartitionVars* a_parVars )
const
1565 ASSERT( a_node->m_count == MAXNODES );
1568 for(
int index = 0; index < MAXNODES; ++index )
1570 a_parVars->m_branchBuf[index] = a_node->m_branch[index];
1573 a_parVars->m_branchBuf[MAXNODES] = *a_branch;
1574 a_parVars->m_branchCount = MAXNODES + 1;
1577 a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
1579 for(
int index = 1; index < MAXNODES + 1; ++index )
1581 a_parVars->m_coverSplit =
1582 CombineRect( &a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect );
1585 a_parVars->m_coverSplitArea = CalcRectVolume( &a_parVars->m_coverSplit );
1603void RTREE_QUAL::ChoosePartition( PartitionVars* a_parVars,
int a_minFill )
const
1605 ASSERT( a_parVars );
1607 ELEMTYPEREAL biggestDiff;
1608 int group, chosen = 0, betterGroup = 0;
1610 InitParVars( a_parVars, a_parVars->m_branchCount, a_minFill );
1611 PickSeeds( a_parVars );
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) ) )
1617 biggestDiff = (ELEMTYPEREAL) -1;
1619 for(
int index = 0; index<a_parVars->m_total; ++index )
1621 if( !a_parVars->m_taken[index] )
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;
1640 if( diff > biggestDiff )
1644 betterGroup = group;
1646 else if( (diff == biggestDiff)
1647 && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]) )
1650 betterGroup = group;
1655 Classify( chosen, betterGroup, a_parVars );
1659 if( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1661 if( a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill )
1670 for(
int index = 0; index<a_parVars->m_total; ++index )
1672 if( !a_parVars->m_taken[index] )
1674 Classify( index, group, a_parVars );
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) );
1687void RTREE_QUAL::LoadNodes( Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars )
const
1691 ASSERT( a_parVars );
1693 for(
int index = 0; index < a_parVars->m_total; ++index )
1695 ASSERT( a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1 );
1697 if( a_parVars->m_partition[index] == 0 )
1699 AddBranch( &a_parVars->m_branchBuf[index], a_nodeA, NULL );
1701 else if( a_parVars->m_partition[index] == 1 )
1703 AddBranch( &a_parVars->m_branchBuf[index], a_nodeB, NULL );
1711void RTREE_QUAL::InitParVars( PartitionVars* a_parVars,
int a_maxRects,
int a_minFill )
const
1713 ASSERT( a_parVars );
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;
1720 for(
int index = 0; index < a_maxRects; ++index )
1722 a_parVars->m_taken[index] =
false;
1723 a_parVars->m_partition[index] = -1;
1729void RTREE_QUAL::PickSeeds( PartitionVars* a_parVars )
const
1731 int seed0 = 0, seed1 = 0;
1732 ELEMTYPEREAL worst, waste;
1733 ELEMTYPEREAL area[MAXNODES + 1];
1735 for(
int index = 0; index<a_parVars->m_total; ++index )
1737 area[index] = CalcRectVolume( &a_parVars->m_branchBuf[index].m_rect );
1740 worst = -a_parVars->m_coverSplitArea - 1;
1742 for(
int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA )
1744 for(
int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB )
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];
1750 if( waste >= worst )
1759 Classify( seed0, 0, a_parVars );
1760 Classify( seed1, 1, a_parVars );
1766void RTREE_QUAL::Classify(
int a_index,
int a_group, PartitionVars* a_parVars )
const
1768 ASSERT( a_parVars );
1769 ASSERT( !a_parVars->m_taken[a_index] );
1771 a_parVars->m_partition[a_index] = a_group;
1772 a_parVars->m_taken[a_index] =
true;
1774 if( a_parVars->m_count[a_group] == 0 )
1776 a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
1780 a_parVars->m_cover[a_group] = CombineRect( &a_parVars->m_branchBuf[a_index].m_rect,
1781 &a_parVars->m_cover[a_group] );
1784 a_parVars->m_area[a_group] = CalcRectVolume( &a_parVars->m_cover[a_group] );
1785 ++a_parVars->m_count[a_group];
1794bool RTREE_QUAL::RemoveRect(
const Rect* a_rect,
const DATATYPE& a_id, Node** a_root )
const
1796 ASSERT( a_rect && a_root );
1800 ListNode* reInsertList = NULL;
1802 if( !RemoveRectRec( a_rect, a_id, *a_root, &reInsertList ) )
1806 while( reInsertList )
1808 tempNode = reInsertList->m_node;
1810 for(
int index = 0; index < tempNode->m_count; ++index )
1812 InsertRect( &(tempNode->m_branch[index].m_rect),
1813 tempNode->m_branch[index].m_data,
1815 tempNode->m_level );
1818 ListNode* remLNode = reInsertList;
1819 reInsertList = reInsertList->m_next;
1821 FreeNode( remLNode->m_node );
1822 FreeListNode( remLNode );
1826 if( (*a_root)->m_count == 1 && (*a_root)->IsInternalNode() )
1828 tempNode = (*a_root)->m_branch[0].m_child;
1831 FreeNode( *a_root );
1849bool RTREE_QUAL::RemoveRectRec(
const Rect* a_rect,
1850 const DATATYPE& a_id,
1852 ListNode** a_listNode )
const
1854 ASSERT( a_rect && a_node && a_listNode );
1855 ASSERT( a_node->m_level >= 0 );
1857 if( a_node->IsInternalNode() )
1859 for(
int index = 0; index < a_node->m_count; ++index )
1861 if( Overlap( a_rect, &(a_node->m_branch[index].m_rect) ) )
1863 if( !RemoveRectRec( a_rect, a_id, a_node->m_branch[index].m_child, a_listNode ) )
1865 if( a_node->m_branch[index].m_child->m_count >= MINNODES )
1868 a_node->m_branch[index].m_rect =
1869 NodeCover( a_node->m_branch[index].m_child );
1874 ReInsert( a_node->m_branch[index].m_child, a_listNode );
1875 DisconnectBranch( a_node, index );
1887 for(
int index = 0; index < a_node->m_count; ++index )
1889 if( a_node->m_branch[index].m_child == (Node*) a_id )
1891 DisconnectBranch( a_node, index );
1903bool RTREE_QUAL::Overlap(
const Rect* a_rectA,
const Rect* a_rectB )
1905 ASSERT( a_rectA && a_rectB );
1907 for(
int index = 0; index < NUMDIMS; ++index )
1909 if( a_rectA->m_min[index] > a_rectB->m_max[index]
1910 || a_rectB->m_min[index] > a_rectA->m_max[index] )
1923void RTREE_QUAL::ReInsert( Node* a_node, ListNode** a_listNode )
const
1925 ListNode* newListNode;
1927 newListNode = AllocListNode();
1928 newListNode->m_node = a_node;
1929 newListNode->m_next = *a_listNode;
1930 *a_listNode = newListNode;
1936bool RTREE_QUAL::Search(
const Node* a_node,
const Rect* a_rect,
int& a_foundCount,
1937 std::function<
bool (
const DATATYPE&)> a_callback )
const
1940 ASSERT( a_node->m_level >= 0 );
1943 if( a_node->IsInternalNode() )
1945 for(
int index = 0; index < a_node->m_count; ++index )
1947 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1949 if( !Search( a_node->m_branch[index].m_child, a_rect, a_foundCount, a_callback ) )
1958 for(
int index = 0; index < a_node->m_count; ++index )
1960 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1962 DATATYPE&
id = a_node->m_branch[index].m_data;
1965 if( a_callback && !a_callback(
id ) )
1980ELEMTYPE RTREE_QUAL::MinDist(
const ELEMTYPE a_point[NUMDIMS],
const Rect& a_rect )
const
1982 const ELEMTYPE *q, *s, *t;
1986 double minDist = 0.0;
1988 for(
int index = 0; index < NUMDIMS; index++ )
1992 if( q[index] < s[index] )
1996 else if( q[index] > t[index] )
2001 double addend = q[index] - r;
2002 minDist += addend * addend;
2005 return std::lround( std::sqrt( minDist ) );
2009#undef RTREE_TEMPLATE
2011#undef RTREE_SEARCH_TEMPLATE
2012#undef RTREE_SEARCH_QUAL
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