42 #define RTREE_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
43 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES>
44 #define RTREE_SEARCH_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
45 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES, class VISITOR>
46 #define RTREE_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
48 #define RTREE_SEARCH_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
51 #define RTREE_DONT_USE_MEMPOOLS
52 #define RTREE_USE_SPHERICAL_VOLUME
74 template <
class DATATYPE,
class ELEMTYPE,
int NUMDIMS,
75 class ELEMTYPEREAL = ELEMTYPE,
int TMAXNODES = 8,
int TMINNODES = TMAXNODES / 2>
108 void Insert(
const ELEMTYPE a_min[NUMDIMS],
109 const ELEMTYPE a_max[NUMDIMS],
110 const DATATYPE& a_dataId );
117 bool Remove(
const ELEMTYPE a_min[NUMDIMS],
118 const ELEMTYPE a_max[NUMDIMS],
119 const DATATYPE& a_dataId );
126 int Search(
const ELEMTYPE a_min[NUMDIMS],
127 const ELEMTYPE a_max[NUMDIMS],
128 std::function<
bool (
const DATATYPE&)> a_callback )
const;
130 template <
class VISITOR>
131 int Search(
const ELEMTYPE a_min[NUMDIMS],
const ELEMTYPE a_max[NUMDIMS], VISITOR& a_visitor )
135 for(
int index = 0; index<NUMDIMS; ++index )
137 ASSERT( a_min[index] <= a_max[index] );
144 for(
int axis = 0; axis<NUMDIMS; ++axis )
146 rect.m_min[axis] = a_min[axis];
147 rect.m_max[axis] = a_max[axis];
170 bool Load(
const char* a_fileName );
177 bool Save(
const char* a_fileName );
197 ELEMTYPE a_squareDistanceCallback(
const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
198 ELEMTYPE* a_squareDistance );
205 enum { MAX_STACK = 32 };
228 StackElement& curTos = m_stack[m_tos - 1];
229 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
236 StackElement& curTos = m_stack[m_tos - 1];
237 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
244 void GetBounds( ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS] )
247 StackElement& curTos = m_stack[m_tos - 1];
248 Branch& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex];
250 for(
int index = 0; index < NUMDIMS; ++index )
260 void Init() { m_tos = 0; }
272 StackElement curTos = Pop();
274 if( curTos.m_node->IsLeaf() )
277 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
280 Push( curTos.m_node, curTos.m_branchIndex + 1 );
288 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
292 Push( curTos.m_node, curTos.m_branchIndex + 1 );
296 Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
297 Push( nextLevelnode, 0 );
300 if( nextLevelnode->IsLeaf() )
309 void Push( Node* a_node,
int a_branchIndex )
311 m_stack[m_tos].m_node = a_node;
312 m_stack[m_tos].m_branchIndex = a_branchIndex;
314 ASSERT( m_tos <= MAX_STACK );
322 return m_stack[m_tos];
325 StackElement m_stack[MAX_STACK];
340 if( first->IsInternalNode() && first->
m_count > 1 )
342 a_it.Push( first, 1 );
344 else if( first->IsLeaf() )
348 a_it.Push( first, 0 );
391 bool IsInternalNode() {
return m_level > 0; }
392 bool IsLeaf() {
return m_level == 0; }
415 ELEMTYPEREAL m_area[2];
420 ELEMTYPEREAL m_coverSplitArea;
432 void FreeNode(
Node* a_node );
433 void InitNode(
Node* a_node );
434 void InitRect(
Rect* a_rect );
435 bool InsertRectRec(
Rect* a_rect,
436 const DATATYPE& a_id,
440 bool InsertRect(
Rect* a_rect,
const DATATYPE& a_id,
Node** a_root,
int a_level );
442 bool AddBranch(
Branch* a_branch,
Node* a_node,
Node** a_newNode );
443 void DisconnectBranch(
Node* a_node,
int a_index );
444 int PickBranch(
Rect* a_rect,
Node* a_node );
446 void SplitNode(
Node* a_node,
Branch* a_branch,
Node** a_newNode );
447 ELEMTYPEREAL RectSphericalVolume(
Rect* a_rect );
448 ELEMTYPEREAL RectVolume(
Rect* a_rect );
449 ELEMTYPEREAL CalcRectVolume(
Rect* a_rect );
451 void ChoosePartition(
PartitionVars* a_parVars,
int a_minFill );
453 void InitParVars(
PartitionVars* a_parVars,
int a_maxRects,
int a_minFill );
455 void Classify(
int a_index,
int a_group,
PartitionVars* a_parVars );
456 bool RemoveRect(
Rect* a_rect,
const DATATYPE& a_id,
Node** a_root );
457 bool RemoveRectRec(
Rect* a_rect,
458 const DATATYPE& a_id,
462 void FreeListNode(
ListNode* a_listNode );
463 bool Overlap(
Rect* a_rectA,
Rect* a_rectB )
const;
465 ELEMTYPE MinDist(
const ELEMTYPE a_point[NUMDIMS],
Rect* a_rect );
466 void InsertNNListSorted( std::vector<NNNode*>* nodeList,
NNNode* newNode );
469 std::function<
bool (
const DATATYPE&)> a_callback )
const;
471 template <
class VISITOR>
472 bool Search(
Node* a_node,
Rect* a_rect, VISITOR& a_visitor,
int& a_foundCount )
475 ASSERT( a_node->
m_level >= 0 );
478 if( a_node->IsInternalNode() )
480 for(
int index = 0; index < a_node->
m_count; ++index )
493 for(
int index = 0; index < a_node->
m_count; ++index )
499 if( !a_visitor(
id ) )
510 void RemoveAllRec( Node* a_node );
512 void CountRec( Node* a_node,
int& a_count );
540 bool OpenRead(
const char* a_fileName )
542 m_file = fopen( a_fileName,
"rb" );
552 bool OpenWrite(
const char* a_fileName )
554 m_file = fopen( a_fileName,
"wb" );
573 template <
typename TYPE>
574 size_t Write(
const TYPE& a_value )
577 return fwrite( (
void*) &a_value,
sizeof(a_value), 1, m_file );
580 template <
typename TYPE>
581 size_t WriteArray(
const TYPE* a_array,
int a_count )
584 return fwrite( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
587 template <
typename TYPE>
588 size_t Read( TYPE& a_value )
591 return fread( (
void*) &a_value,
sizeof(a_value), 1, m_file );
594 template <
typename TYPE>
595 size_t ReadArray( TYPE* a_array,
int a_count )
598 return fread( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
603 RTREE_TEMPLATE RTREE_QUAL::RTree()
605 ASSERT( MAXNODES > MINNODES );
606 ASSERT( MINNODES > 0 );
611 ASSERT(
sizeof(DATATYPE) ==
sizeof(
void*) ||
sizeof(DATATYPE) ==
sizeof(
int) );
614 const float UNIT_SPHERE_VOLUMES[] =
616 0.000000f, 2.000000f, 3.141593f,
617 4.188790f, 4.934802f, 5.263789f,
618 5.167713f, 4.724766f, 4.058712f,
619 3.298509f, 2.550164f, 1.884104f,
620 1.335263f, 0.910629f, 0.599265f,
621 0.381443f, 0.235331f, 0.140981f,
622 0.082146f, 0.046622f, 0.025807f,
625 m_root = AllocNode();
627 m_unitSphereVolume = (ELEMTYPEREAL) UNIT_SPHERE_VOLUMES[NUMDIMS];
632 RTREE_QUAL::~RTree() {
638 void RTREE_QUAL::Insert(
const ELEMTYPE a_min[NUMDIMS],
639 const ELEMTYPE a_max[NUMDIMS],
640 const DATATYPE& a_dataId )
644 for(
int index = 0; index<NUMDIMS; ++index )
646 ASSERT( a_min[index] <= a_max[index] );
653 for(
int axis = 0; axis<NUMDIMS; ++axis )
655 rect.m_min[axis] = a_min[axis];
656 rect.m_max[axis] = a_max[axis];
659 InsertRect( &rect, a_dataId, &m_root, 0 );
664 bool RTREE_QUAL::Remove(
const ELEMTYPE a_min[NUMDIMS],
665 const ELEMTYPE a_max[NUMDIMS],
666 const DATATYPE& a_dataId )
670 for(
int index = 0; index<NUMDIMS; ++index )
672 ASSERT( a_min[index] <= a_max[index] );
679 for(
int axis = 0; axis<NUMDIMS; ++axis )
681 rect.m_min[axis] = a_min[axis];
682 rect.m_max[axis] = a_max[axis];
685 return RemoveRect( &rect, a_dataId, &m_root );
690 int RTREE_QUAL::Search(
const ELEMTYPE a_min[NUMDIMS],
691 const ELEMTYPE a_max[NUMDIMS],
692 std::function<
bool (
const DATATYPE&)> a_callback )
const
696 for(
int index = 0; index<NUMDIMS; ++index )
698 ASSERT( a_min[index] <= a_max[index] );
705 for(
int axis = 0; axis<NUMDIMS; ++axis )
707 rect.m_min[axis] = a_min[axis];
708 rect.m_max[axis] = a_max[axis];
714 Search( m_root, &rect, foundCount, a_callback );
720 DATATYPE RTREE_QUAL::NearestNeighbor(
const ELEMTYPE a_point[NUMDIMS] )
722 return this->NearestNeighbor( a_point, 0, 0 );
727 DATATYPE RTREE_QUAL::NearestNeighbor(
const ELEMTYPE a_point[NUMDIMS],
728 ELEMTYPE a_squareDistanceCallback(
const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
729 ELEMTYPE* a_squareDistance )
731 typedef typename std::vector<NNNode*>::iterator iterator;
732 std::vector<NNNode*> nodeList;
734 NNNode* closestNode = 0;
735 while( !closestNode || !closestNode->isLeaf )
738 for(
int index = 0; index < node->m_count; ++index )
740 NNNode* newNode =
new NNNode;
741 newNode->isLeaf = node->IsLeaf();
742 newNode->m_branch = node->m_branch[index];
743 if( newNode->isLeaf && a_squareDistanceCallback )
744 newNode->minDist = a_squareDistanceCallback( a_point, newNode->m_branch.m_data );
746 newNode->minDist = this->MinDist( a_point, &(node->m_branch[index].m_rect) );
749 this->InsertNNListSorted( &nodeList, newNode );
751 if( nodeList.size() == 0 )
755 closestNode = nodeList.back();
756 node = closestNode->m_branch.m_child;
762 for( iterator iter = nodeList.begin(); iter != nodeList.end(); ++iter )
764 NNNode* nnode = *iter;
768 *a_squareDistance = closestNode->minDist;
769 return closestNode->m_branch.m_data;
774 int RTREE_QUAL::Count()
778 CountRec( m_root, count );
785 void RTREE_QUAL::CountRec( Node* a_node,
int& a_count )
787 if( a_node->IsInternalNode() )
789 for(
int index = 0; index < a_node->m_count; ++index )
791 CountRec( a_node->m_branch[index].m_child, a_count );
796 a_count += a_node->m_count;
802 bool RTREE_QUAL::Load(
const char* a_fileName )
808 if( !stream.OpenRead( a_fileName ) )
813 bool result = Load( stream );
825 int _dataFileId = (
'R' << 0) | (
'T' << 8) | (
'R' << 16) | (
'E' << 24);
826 int _dataSize =
sizeof(DATATYPE);
827 int _dataNumDims = NUMDIMS;
828 int _dataElemSize =
sizeof(ELEMTYPE);
829 int _dataElemRealSize =
sizeof(ELEMTYPEREAL);
830 int _dataMaxNodes = TMAXNODES;
831 int _dataMinNodes = TMINNODES;
836 int dataElemSize = 0;
837 int dataElemRealSize = 0;
838 int dataMaxNodes = 0;
839 int dataMinNodes = 0;
841 a_stream.Read( dataFileId );
842 a_stream.Read( dataSize );
843 a_stream.Read( dataNumDims );
844 a_stream.Read( dataElemSize );
845 a_stream.Read( dataElemRealSize );
846 a_stream.Read( dataMaxNodes );
847 a_stream.Read( dataMinNodes );
852 if( (dataFileId == _dataFileId)
853 && (dataSize == _dataSize)
854 && (dataNumDims == _dataNumDims)
855 && (dataElemSize == _dataElemSize)
856 && (dataElemRealSize == _dataElemRealSize)
857 && (dataMaxNodes == _dataMaxNodes)
858 && (dataMinNodes == _dataMinNodes)
862 result = LoadRec( m_root, a_stream );
870 bool RTREE_QUAL::LoadRec( Node* a_node,
RTFileStream& a_stream )
872 a_stream.Read( a_node->m_level );
873 a_stream.Read( a_node->m_count );
875 if( a_node->IsInternalNode() )
877 for(
int index = 0; index < a_node->m_count; ++index )
879 Branch* curBranch = &a_node->m_branch[index];
881 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
882 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
884 curBranch->m_child = AllocNode();
885 LoadRec( curBranch->m_child, a_stream );
890 for(
int index = 0; index < a_node->m_count; ++index )
892 Branch* curBranch = &a_node->m_branch[index];
894 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
895 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
897 a_stream.Read( curBranch->m_data );
906 bool RTREE_QUAL::Save(
const char* a_fileName )
910 if( !stream.OpenWrite( a_fileName ) )
915 bool result = Save( stream );
927 int dataFileId = (
'R' << 0) | (
'T' << 8) | (
'R' << 16) | (
'E' << 24);
928 int dataSize =
sizeof(DATATYPE);
929 int dataNumDims = NUMDIMS;
930 int dataElemSize =
sizeof(ELEMTYPE);
931 int dataElemRealSize =
sizeof(ELEMTYPEREAL);
932 int dataMaxNodes = TMAXNODES;
933 int dataMinNodes = TMINNODES;
935 a_stream.Write( dataFileId );
936 a_stream.Write( dataSize );
937 a_stream.Write( dataNumDims );
938 a_stream.Write( dataElemSize );
939 a_stream.Write( dataElemRealSize );
940 a_stream.Write( dataMaxNodes );
941 a_stream.Write( dataMinNodes );
944 bool result = SaveRec( m_root, a_stream );
951 bool RTREE_QUAL::SaveRec( Node* a_node,
RTFileStream& a_stream )
953 a_stream.Write( a_node->m_level );
954 a_stream.Write( a_node->m_count );
956 if( a_node->IsInternalNode() )
958 for(
int index = 0; index < a_node->m_count; ++index )
960 Branch* curBranch = &a_node->m_branch[index];
962 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
963 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
965 SaveRec( curBranch->m_child, a_stream );
970 for(
int index = 0; index < a_node->m_count; ++index )
972 Branch* curBranch = &a_node->m_branch[index];
974 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
975 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
977 a_stream.Write( curBranch->m_data );
986 void RTREE_QUAL::RemoveAll()
991 m_root = AllocNode();
997 void RTREE_QUAL::Reset()
999 #ifdef RTREE_DONT_USE_MEMPOOLS
1001 RemoveAllRec( m_root );
1010 void RTREE_QUAL::RemoveAllRec( Node* a_node )
1013 ASSERT( a_node->m_level >= 0 );
1015 if( a_node->IsInternalNode() )
1017 for(
int index = 0; index < a_node->m_count; ++index )
1019 RemoveAllRec( a_node->m_branch[index].m_child );
1028 typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
1032 #ifdef RTREE_DONT_USE_MEMPOOLS
1037 InitNode( newNode );
1043 void RTREE_QUAL::FreeNode( Node* a_node )
1047 #ifdef RTREE_DONT_USE_MEMPOOLS
1058 typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
1060 #ifdef RTREE_DONT_USE_MEMPOOLS
1061 return new ListNode;
1069 void RTREE_QUAL::FreeListNode( ListNode* a_listNode )
1071 #ifdef RTREE_DONT_USE_MEMPOOLS
1080 void RTREE_QUAL::InitNode( Node* a_node )
1082 a_node->m_count = 0;
1083 a_node->m_level = -1;
1088 void RTREE_QUAL::InitRect( Rect* a_rect )
1090 for(
int index = 0; index < NUMDIMS; ++index )
1092 a_rect->m_min[index] = (ELEMTYPE) 0;
1093 a_rect->m_max[index] = (ELEMTYPE) 0;
1106 bool RTREE_QUAL::InsertRectRec( Rect* a_rect,
1107 const DATATYPE& a_id,
1112 ASSERT( a_rect && a_node && a_newNode );
1113 ASSERT( a_level >= 0 && a_level <= a_node->m_level );
1120 if( a_node->m_level > a_level )
1122 index = PickBranch( a_rect, a_node );
1124 if( !InsertRectRec( a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level ) )
1127 a_node->m_branch[index].m_rect =
1128 CombineRect( a_rect, &(a_node->m_branch[index].m_rect) );
1133 a_node->m_branch[index].m_rect = NodeCover( a_node->m_branch[index].m_child );
1134 branch.m_child = otherNode;
1135 branch.m_rect = NodeCover( otherNode );
1136 return AddBranch( &branch, a_node, a_newNode );
1139 else if( a_node->m_level == a_level )
1141 branch.m_rect = *a_rect;
1142 branch.m_child = (Node*) a_id;
1144 return AddBranch( &branch, a_node, a_newNode );
1163 bool RTREE_QUAL::InsertRect( Rect* a_rect,
const DATATYPE& a_id, Node** a_root,
int a_level )
1165 ASSERT( a_rect && a_root );
1166 ASSERT( a_level >= 0 && a_level <= (*a_root)->m_level );
1169 for(
int index = 0; index < NUMDIMS; ++index )
1171 ASSERT( a_rect->m_min[index] <= a_rect->m_max[index] );
1180 if( InsertRectRec( a_rect, a_id, *a_root, &newNode, a_level ) )
1182 newRoot = AllocNode();
1183 newRoot->m_level = (*a_root)->m_level + 1;
1184 branch.m_rect = NodeCover( *a_root );
1185 branch.m_child = *a_root;
1186 AddBranch( &branch, newRoot, NULL );
1187 branch.m_rect = NodeCover( newNode );
1188 branch.m_child = newNode;
1189 AddBranch( &branch, newRoot, NULL );
1200 typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover( Node* a_node )
1204 int firstTime =
true;
1208 for(
int index = 0; index < a_node->m_count; ++index )
1212 rect = a_node->m_branch[index].m_rect;
1217 rect = CombineRect( &rect, &(a_node->m_branch[index].m_rect) );
1230 bool RTREE_QUAL::AddBranch( Branch* a_branch, Node* a_node, Node** a_newNode )
1235 if( a_node->m_count < MAXNODES )
1237 a_node->m_branch[a_node->m_count] = *a_branch;
1244 ASSERT( a_newNode );
1246 SplitNode( a_node, a_branch, a_newNode );
1255 void RTREE_QUAL::DisconnectBranch( Node* a_node,
int a_index )
1257 ASSERT( a_node && (a_index >= 0) && (a_index < MAXNODES) );
1258 ASSERT( a_node->m_count > 0 );
1261 a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
1273 int RTREE_QUAL::PickBranch( Rect* a_rect, Node* a_node )
1275 ASSERT( a_rect && a_node );
1277 bool firstTime =
true;
1278 ELEMTYPEREAL increase;
1279 ELEMTYPEREAL bestIncr = (ELEMTYPEREAL) -1;
1281 ELEMTYPEREAL bestArea = 0;
1285 for(
int index = 0; index < a_node->m_count; ++index )
1287 Rect* curRect = &a_node->m_branch[index].m_rect;
1288 area = CalcRectVolume( curRect );
1289 tempRect = CombineRect( a_rect, curRect );
1290 increase = CalcRectVolume( &tempRect ) - area;
1292 if( (increase < bestIncr) || firstTime )
1296 bestIncr = increase;
1299 else if( (increase == bestIncr) && (area < bestArea) )
1303 bestIncr = increase;
1313 typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect( Rect* a_rectA, Rect* a_rectB )
1315 ASSERT( a_rectA && a_rectB );
1319 for(
int index = 0; index < NUMDIMS; ++index )
1321 newRect.m_min[index] = std::min( a_rectA->m_min[index], a_rectB->m_min[index] );
1322 newRect.m_max[index] = std::max( a_rectA->m_max[index], a_rectB->m_max[index] );
1334 void RTREE_QUAL::SplitNode( Node* a_node, Branch* a_branch, Node** a_newNode )
1340 PartitionVars localVars;
1341 PartitionVars* parVars = &localVars;
1345 level = a_node->m_level;
1346 GetBranches( a_node, a_branch, parVars );
1349 ChoosePartition( parVars, MINNODES );
1352 *a_newNode = AllocNode();
1353 (*a_newNode)->m_level = a_node->m_level = level;
1354 LoadNodes( a_node, *a_newNode, parVars );
1356 ASSERT( (a_node->m_count + (*a_newNode)->m_count) == parVars->m_total );
1362 ELEMTYPEREAL RTREE_QUAL::RectVolume( Rect* a_rect )
1366 ELEMTYPEREAL volume = (ELEMTYPEREAL) 1;
1368 for(
int index = 0; index<NUMDIMS; ++index )
1370 volume *= a_rect->m_max[index] - a_rect->m_min[index];
1373 ASSERT( volume >= (ELEMTYPEREAL) 0 );
1381 ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume( Rect* a_rect )
1385 ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL) 0;
1386 ELEMTYPEREAL radius;
1388 for(
int index = 0; index < NUMDIMS; ++index )
1390 ELEMTYPEREAL halfExtent =
1391 ( (ELEMTYPEREAL) a_rect->m_max[index] - (ELEMTYPEREAL) a_rect->m_min[index] ) * 0.5f;
1392 sumOfSquares += halfExtent * halfExtent;
1395 radius = (ELEMTYPEREAL) sqrt( sumOfSquares );
1400 return radius * radius * radius * m_unitSphereVolume;
1402 else if( NUMDIMS == 2 )
1404 return radius * radius * m_unitSphereVolume;
1408 return (ELEMTYPEREAL) (pow( radius, NUMDIMS ) * m_unitSphereVolume);
1415 ELEMTYPEREAL RTREE_QUAL::CalcRectVolume( Rect* a_rect )
1417 #ifdef RTREE_USE_SPHERICAL_VOLUME
1418 return RectSphericalVolume( a_rect );
1420 return RectVolume( a_rect );
1427 void RTREE_QUAL::GetBranches( Node* a_node, Branch* a_branch, PartitionVars* a_parVars )
1432 ASSERT( a_node->m_count == MAXNODES );
1435 for(
int index = 0; index < MAXNODES; ++index )
1437 a_parVars->m_branchBuf[index] = a_node->m_branch[index];
1440 a_parVars->m_branchBuf[MAXNODES] = *a_branch;
1441 a_parVars->m_branchCount = MAXNODES + 1;
1444 a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
1446 for(
int index = 1; index < MAXNODES + 1; ++index )
1448 a_parVars->m_coverSplit =
1449 CombineRect( &a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect );
1452 a_parVars->m_coverSplitArea = CalcRectVolume( &a_parVars->m_coverSplit );
1470 void RTREE_QUAL::ChoosePartition( PartitionVars* a_parVars,
int a_minFill )
1472 ASSERT( a_parVars );
1474 ELEMTYPEREAL biggestDiff;
1475 int group, chosen = 0, betterGroup = 0;
1477 InitParVars( a_parVars, a_parVars->m_branchCount, a_minFill );
1478 PickSeeds( a_parVars );
1480 while( ( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1481 && ( a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill) )
1482 && ( a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill) ) )
1484 biggestDiff = (ELEMTYPEREAL) -1;
1486 for(
int index = 0; index<a_parVars->m_total; ++index )
1488 if( !a_parVars->m_taken[index] )
1490 Rect* curRect = &a_parVars->m_branchBuf[index].m_rect;
1491 Rect rect0 = CombineRect( curRect, &a_parVars->m_cover[0] );
1492 Rect rect1 = CombineRect( curRect, &a_parVars->m_cover[1] );
1493 ELEMTYPEREAL growth0 = CalcRectVolume( &rect0 ) - a_parVars->m_area[0];
1494 ELEMTYPEREAL growth1 = CalcRectVolume( &rect1 ) - a_parVars->m_area[1];
1495 ELEMTYPEREAL diff = growth1 - growth0;
1507 if( diff > biggestDiff )
1511 betterGroup = group;
1513 else if( (diff == biggestDiff)
1514 && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]) )
1517 betterGroup = group;
1522 Classify( chosen, betterGroup, a_parVars );
1526 if( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1528 if( a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill )
1537 for(
int index = 0; index<a_parVars->m_total; ++index )
1539 if( !a_parVars->m_taken[index] )
1541 Classify( index, group, a_parVars );
1546 ASSERT( (a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total );
1547 ASSERT( (a_parVars->m_count[0] >= a_parVars->m_minFill)
1548 && (a_parVars->m_count[1] >= a_parVars->m_minFill) );
1554 void RTREE_QUAL::LoadNodes( Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars )
1558 ASSERT( a_parVars );
1560 for(
int index = 0; index < a_parVars->m_total; ++index )
1562 ASSERT( a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1 );
1564 if( a_parVars->m_partition[index] == 0 )
1566 AddBranch( &a_parVars->m_branchBuf[index], a_nodeA, NULL );
1568 else if( a_parVars->m_partition[index] == 1 )
1570 AddBranch( &a_parVars->m_branchBuf[index], a_nodeB, NULL );
1578 void RTREE_QUAL::InitParVars( PartitionVars* a_parVars,
int a_maxRects,
int a_minFill )
1580 ASSERT( a_parVars );
1582 a_parVars->m_count[0] = a_parVars->m_count[1] = 0;
1583 a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL) 0;
1584 a_parVars->m_total = a_maxRects;
1585 a_parVars->m_minFill = a_minFill;
1587 for(
int index = 0; index < a_maxRects; ++index )
1589 a_parVars->m_taken[index] =
false;
1590 a_parVars->m_partition[index] = -1;
1596 void RTREE_QUAL::PickSeeds( PartitionVars* a_parVars )
1598 int seed0 = 0, seed1 = 0;
1599 ELEMTYPEREAL worst, waste;
1600 ELEMTYPEREAL area[MAXNODES + 1];
1602 for(
int index = 0; index<a_parVars->m_total; ++index )
1604 area[index] = CalcRectVolume( &a_parVars->m_branchBuf[index].m_rect );
1607 worst = -a_parVars->m_coverSplitArea - 1;
1609 for(
int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA )
1611 for(
int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB )
1613 Rect oneRect = CombineRect( &a_parVars->m_branchBuf[indexA].m_rect,
1614 &a_parVars->m_branchBuf[indexB].m_rect );
1615 waste = CalcRectVolume( &oneRect ) - area[indexA] - area[indexB];
1617 if( waste >= worst )
1626 Classify( seed0, 0, a_parVars );
1627 Classify( seed1, 1, a_parVars );
1633 void RTREE_QUAL::Classify(
int a_index,
int a_group, PartitionVars* a_parVars )
1635 ASSERT( a_parVars );
1636 ASSERT( !a_parVars->m_taken[a_index] );
1638 a_parVars->m_partition[a_index] = a_group;
1639 a_parVars->m_taken[a_index] =
true;
1641 if( a_parVars->m_count[a_group] == 0 )
1643 a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
1647 a_parVars->m_cover[a_group] = CombineRect( &a_parVars->m_branchBuf[a_index].m_rect,
1648 &a_parVars->m_cover[a_group] );
1651 a_parVars->m_area[a_group] = CalcRectVolume( &a_parVars->m_cover[a_group] );
1652 ++a_parVars->m_count[a_group];
1661 bool RTREE_QUAL::RemoveRect( Rect* a_rect,
const DATATYPE& a_id, Node** a_root )
1663 ASSERT( a_rect && a_root );
1667 ListNode* reInsertList = NULL;
1669 if( !RemoveRectRec( a_rect, a_id, *a_root, &reInsertList ) )
1673 while( reInsertList )
1675 tempNode = reInsertList->m_node;
1677 for(
int index = 0; index < tempNode->m_count; ++index )
1679 InsertRect( &(tempNode->m_branch[index].m_rect),
1680 tempNode->m_branch[index].m_data,
1682 tempNode->m_level );
1685 ListNode* remLNode = reInsertList;
1686 reInsertList = reInsertList->m_next;
1688 FreeNode( remLNode->m_node );
1689 FreeListNode( remLNode );
1693 if( (*a_root)->m_count == 1 && (*a_root)->IsInternalNode() )
1695 tempNode = (*a_root)->m_branch[0].m_child;
1698 FreeNode( *a_root );
1716 bool RTREE_QUAL::RemoveRectRec( Rect* a_rect,
1717 const DATATYPE& a_id,
1719 ListNode** a_listNode )
1721 ASSERT( a_rect && a_node && a_listNode );
1722 ASSERT( a_node->m_level >= 0 );
1724 if( a_node->IsInternalNode() )
1726 for(
int index = 0; index < a_node->m_count; ++index )
1728 if( Overlap( a_rect, &(a_node->m_branch[index].m_rect) ) )
1730 if( !RemoveRectRec( a_rect, a_id, a_node->m_branch[index].m_child, a_listNode ) )
1732 if( a_node->m_branch[index].m_child->m_count >= MINNODES )
1735 a_node->m_branch[index].m_rect =
1736 NodeCover( a_node->m_branch[index].m_child );
1741 ReInsert( a_node->m_branch[index].m_child, a_listNode );
1742 DisconnectBranch( a_node, index );
1754 for(
int index = 0; index < a_node->m_count; ++index )
1756 if( a_node->m_branch[index].m_child == (Node*) a_id )
1758 DisconnectBranch( a_node, index );
1770 bool RTREE_QUAL::Overlap( Rect* a_rectA, Rect* a_rectB )
const
1772 ASSERT( a_rectA && a_rectB );
1774 for(
int index = 0; index < NUMDIMS; ++index )
1776 if( a_rectA->m_min[index] > a_rectB->m_max[index]
1777 || a_rectB->m_min[index] > a_rectA->m_max[index] )
1790 void RTREE_QUAL::ReInsert( Node* a_node, ListNode** a_listNode )
1792 ListNode* newListNode;
1794 newListNode = AllocListNode();
1795 newListNode->m_node = a_node;
1796 newListNode->m_next = *a_listNode;
1797 *a_listNode = newListNode;
1803 bool RTREE_QUAL::Search( Node* a_node, Rect* a_rect,
int& a_foundCount,
1804 std::function<
bool (
const DATATYPE&)> a_callback )
const
1807 ASSERT( a_node->m_level >= 0 );
1810 if( a_node->IsInternalNode() )
1812 for(
int index = 0; index < a_node->m_count; ++index )
1814 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1816 if( !Search( a_node->m_branch[index].m_child, a_rect, a_foundCount, a_callback ) )
1825 for(
int index = 0; index < a_node->m_count; ++index )
1827 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1829 DATATYPE&
id = a_node->m_branch[index].m_data;
1832 if( a_callback && !a_callback(
id ) )
1847 ELEMTYPE RTREE_QUAL::MinDist(
const ELEMTYPE a_point[NUMDIMS], Rect* a_rect )
1849 ELEMTYPE *q, *s, *t;
1850 q = (ELEMTYPE*) a_point;
1854 for(
int index = 0; index < NUMDIMS; index++ )
1857 if( q[index] < s[index] )
1861 else if( q[index] >t[index] )
1865 int addend = q[index] - r;
1866 minDist += addend * addend;
1874 void RTREE_QUAL::InsertNNListSorted( std::vector<NNNode*>* nodeList, NNNode* newNode )
1876 typedef typename std::vector<NNNode*>::iterator iterator;
1877 iterator iter = nodeList->begin();
1878 while( iter != nodeList->end() && (*iter)->minDist > newNode->minDist )
1882 nodeList->insert(iter, newNode);
1886 #undef RTREE_TEMPLATE
1888 #undef RTREE_SEARCH_TEMPLATE
1889 #undef RTREE_SEARCH_QUAL
Iterator is not remove safe.
Definition: rtree.h:202
const DATATYPE & operator*() const
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:233
bool IsNull()
Is iterator invalid.
Definition: rtree.h:219
void GetBounds(ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS])
Get the bounds for this node.
Definition: rtree.h:244
bool IsNotNull()
Is iterator pointing to valid data.
Definition: rtree.h:222
DATATYPE & operator*()
Access the current data element. Caller must be sure iterator is not NULL first.
Definition: rtree.h:225
bool operator++()
Find the next data element.
Definition: rtree.h:241
Implementation of RTree, a multidimensional bounding rectangle tree.
Definition: rtree.h:77
int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], std::function< bool(const DATATYPE &)> a_callback) const
Find all within search rectangle.
void RemoveAll()
Remove all entries from tree.
void GetNext(Iterator &a_it)
Get Next for iteration.
Definition: rtree.h:359
Node * m_root
Root of tree.
Definition: rtree.h:517
bool Save(const char *a_fileName)
Save tree contents to file.
@ MINNODES
Min elements in node.
Definition: rtree.h:87
@ MAXNODES
Max elements in node.
Definition: rtree.h:86
bool Save(RTFileStream &a_stream)
Save tree contents to stream.
Statistics CalcStats()
Calculate Statistics.
int Count()
Count the data elements in this container. This is slow as no internal counter is maintained.
bool IsNull(Iterator &a_it)
Is iterator NULL, or at end?
Definition: rtree.h:362
void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Insert entry.
DATATYPE & GetAt(Iterator &a_it)
Get object at iterator position.
Definition: rtree.h:365
DATATYPE NearestNeighbor(const ELEMTYPE a_point[NUMDIMS], ELEMTYPE a_squareDistanceCallback(const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data), ELEMTYPE *a_squareDistance)
Find the nearest neighbor of a specific point.
DATATYPE NearestNeighbor(const ELEMTYPE a_point[NUMDIMS])
Find the nearest neighbor of a specific point.
void GetFirst(Iterator &a_it)
Get 'first' for iteration.
Definition: rtree.h:333
bool Load(RTFileStream &a_stream)
Load tree contents from stream.
bool Load(const char *a_fileName)
Load tree contents from file.
ELEMTYPEREAL m_unitSphereVolume
Unit sphere constant for required number of dimensions.
Definition: rtree.h:518
bool Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE &a_dataId)
Remove entry.
May be data or may be another subtree The parents level determines this.
Definition: rtree.h:379
Rect m_rect
Bounds.
Definition: rtree.h:380
Node * m_child
Child node.
Definition: rtree.h:383
DATATYPE m_data
Data Id or Ptr.
Definition: rtree.h:384
A link list of nodes for reinsertion after a delete operation.
Definition: rtree.h:401
ListNode * m_next
Next in list.
Definition: rtree.h:402
Node * m_node
Node.
Definition: rtree.h:403
Data structure used for Nearest Neighbor search implementation.
Definition: rtree.h:425
Node for each branch level.
Definition: rtree.h:390
int m_level
Leaf is zero, others positive.
Definition: rtree.h:395
int m_count
Count.
Definition: rtree.h:394
Branch m_branch[MAXNODES]
Branch.
Definition: rtree.h:396
Variables for finding a split partition.
Definition: rtree.h:408
Minimal bounding rectangle (n-dimensional)
Definition: rtree.h:370
ELEMTYPE m_min[NUMDIMS]
Min dimensions of bounding box.
Definition: rtree.h:371
ELEMTYPE m_max[NUMDIMS]
Max dimensions of bounding box.
Definition: rtree.h:372