33 #define ASSERT assert // RTree uses ASSERT( condition )
45 #define RTREE_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
46 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES>
47 #define RTREE_SEARCH_TEMPLATE template <class DATATYPE, class ELEMTYPE, int NUMDIMS, \
48 class ELEMTYPEREAL, int TMAXNODES, int TMINNODES, class VISITOR>
49 #define RTREE_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
51 #define RTREE_SEARCH_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, \
54 #define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one.
55 #define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems
77 template <
class DATATYPE,
class ELEMTYPE,
int NUMDIMS,
78 class ELEMTYPEREAL = ELEMTYPE,
int TMAXNODES = 8,
int TMINNODES = TMAXNODES / 2>
111 void Insert(
const ELEMTYPE a_min[NUMDIMS],
112 const ELEMTYPE a_max[NUMDIMS],
113 const DATATYPE& a_dataId );
120 bool Remove(
const ELEMTYPE a_min[NUMDIMS],
121 const ELEMTYPE a_max[NUMDIMS],
122 const DATATYPE& a_dataId );
130 int Search(
const ELEMTYPE a_min[NUMDIMS],
131 const ELEMTYPE a_max[NUMDIMS],
132 bool a_resultCallback( DATATYPE a_data,
void* a_context ),
135 template <
class VISITOR>
136 int Search(
const ELEMTYPE a_min[NUMDIMS],
const ELEMTYPE a_max[NUMDIMS], VISITOR& a_visitor )
140 for(
int index = 0; index<NUMDIMS; ++index )
142 ASSERT( a_min[index] <= a_max[index] );
149 for(
int axis = 0; axis<NUMDIMS; ++axis )
151 rect.m_min[axis] = a_min[axis];
152 rect.m_max[axis] = a_max[axis];
175 bool Load(
const char* a_fileName );
182 bool Save(
const char* a_fileName );
202 ELEMTYPE a_squareDistanceCallback(
const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
203 ELEMTYPE* a_squareDistance );
210 enum { MAX_STACK = 32 };
233 StackElement& curTos = m_stack[m_tos - 1];
234 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
241 StackElement& curTos = m_stack[m_tos - 1];
242 return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
249 void GetBounds( ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS] )
252 StackElement& curTos = m_stack[m_tos - 1];
253 Branch& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex];
255 for(
int index = 0; index < NUMDIMS; ++index )
265 void Init() { m_tos = 0; }
277 StackElement curTos = Pop();
279 if( curTos.m_node->IsLeaf() )
282 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
285 Push( curTos.m_node, curTos.m_branchIndex + 1 );
293 if( curTos.m_branchIndex + 1 < curTos.m_node->m_count )
297 Push( curTos.m_node, curTos.m_branchIndex + 1 );
301 Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
302 Push( nextLevelnode, 0 );
305 if( nextLevelnode->IsLeaf() )
314 void Push( Node* a_node,
int a_branchIndex )
316 m_stack[m_tos].m_node = a_node;
317 m_stack[m_tos].m_branchIndex = a_branchIndex;
319 ASSERT( m_tos <= MAX_STACK );
327 return m_stack[m_tos];
330 StackElement m_stack[MAX_STACK];
345 if( first->IsInternalNode() && first->m_count > 1 )
347 a_it.Push( first, 1 );
349 else if( first->IsLeaf() )
353 a_it.Push( first, 0 );
367 bool IsNull( Iterator& a_it ) {
return a_it.IsNull(); }
370 DATATYPE&
GetAt( Iterator& a_it ) {
return *a_it; }
396 bool IsInternalNode() {
return m_level > 0; }
397 bool IsLeaf() {
return m_level == 0; }
420 ELEMTYPEREAL m_area[2];
425 ELEMTYPEREAL m_coverSplitArea;
437 void FreeNode(
Node* a_node );
438 void InitNode(
Node* a_node );
439 void InitRect(
Rect* a_rect );
440 bool InsertRectRec(
Rect* a_rect,
441 const DATATYPE& a_id,
445 bool InsertRect(
Rect* a_rect,
const DATATYPE& a_id,
Node** a_root,
int a_level );
447 bool AddBranch(
Branch* a_branch,
Node* a_node,
Node** a_newNode );
448 void DisconnectBranch(
Node* a_node,
int a_index );
449 int PickBranch(
Rect* a_rect,
Node* a_node );
451 void SplitNode(
Node* a_node,
Branch* a_branch,
Node** a_newNode );
452 ELEMTYPEREAL RectSphericalVolume(
Rect* a_rect );
453 ELEMTYPEREAL RectVolume(
Rect* a_rect );
454 ELEMTYPEREAL CalcRectVolume(
Rect* a_rect );
456 void ChoosePartition(
PartitionVars* a_parVars,
int a_minFill );
458 void InitParVars(
PartitionVars* a_parVars,
int a_maxRects,
int a_minFill );
460 void Classify(
int a_index,
int a_group,
PartitionVars* a_parVars );
461 bool RemoveRect(
Rect* a_rect,
const DATATYPE& a_id,
Node** a_root );
462 bool RemoveRectRec(
Rect* a_rect,
463 const DATATYPE& a_id,
467 void FreeListNode(
ListNode* a_listNode );
468 bool Overlap(
Rect* a_rectA,
Rect* a_rectB );
470 ELEMTYPE MinDist(
const ELEMTYPE a_point[NUMDIMS],
Rect* a_rect );
471 void InsertNNListSorted( std::vector<NNNode*>* nodeList,
NNNode* newNode );
473 bool Search(
Node * a_node,
Rect * a_rect,
int& a_foundCount,
bool a_resultCallback(
475 void* a_context ),
void* a_context );
477 template <
class VISITOR>
478 bool Search(
Node* a_node,
Rect* a_rect, VISITOR& a_visitor,
int& a_foundCount )
481 ASSERT( a_node->
m_level >= 0 );
484 if( a_node->IsInternalNode() )
486 for(
int index = 0; index < a_node->
m_count; ++index )
499 for(
int index = 0; index < a_node->
m_count; ++index )
505 if( !a_visitor(
id ) )
516 void RemoveAllRec( Node* a_node );
518 void CountRec( Node* a_node,
int& a_count );
546 bool OpenRead(
const char* a_fileName )
548 m_file = fopen( a_fileName,
"rb" );
558 bool OpenWrite(
const char* a_fileName )
560 m_file = fopen( a_fileName,
"wb" );
579 template <
typename TYPE>
580 size_t Write(
const TYPE& a_value )
583 return fwrite( (
void*) &a_value,
sizeof(a_value), 1, m_file );
586 template <
typename TYPE>
587 size_t WriteArray(
const TYPE* a_array,
int a_count )
590 return fwrite( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
593 template <
typename TYPE>
594 size_t Read( TYPE& a_value )
597 return fread( (
void*) &a_value,
sizeof(a_value), 1, m_file );
600 template <
typename TYPE>
601 size_t ReadArray( TYPE* a_array,
int a_count )
604 return fread( (
void*) a_array,
sizeof(TYPE) * a_count, 1, m_file );
609 RTREE_TEMPLATE RTREE_QUAL::RTree()
611 ASSERT( MAXNODES > MINNODES );
612 ASSERT( MINNODES > 0 );
617 ASSERT(
sizeof(DATATYPE) ==
sizeof(
void*) ||
sizeof(DATATYPE) ==
sizeof(
int) );
620 const float UNIT_SPHERE_VOLUMES[] =
622 0.000000f, 2.000000f, 3.141593f,
623 4.188790f, 4.934802f, 5.263789f,
624 5.167713f, 4.724766f, 4.058712f,
625 3.298509f, 2.550164f, 1.884104f,
626 1.335263f, 0.910629f, 0.599265f,
627 0.381443f, 0.235331f, 0.140981f,
628 0.082146f, 0.046622f, 0.025807f,
631 m_root = AllocNode();
633 m_unitSphereVolume = (ELEMTYPEREAL) UNIT_SPHERE_VOLUMES[NUMDIMS];
638 RTREE_QUAL::~RTree() {
644 void RTREE_QUAL::Insert(
const ELEMTYPE a_min[NUMDIMS],
645 const ELEMTYPE a_max[NUMDIMS],
646 const DATATYPE& a_dataId )
650 for(
int index = 0; index<NUMDIMS; ++index )
652 ASSERT( a_min[index] <= a_max[index] );
659 for(
int axis = 0; axis<NUMDIMS; ++axis )
661 rect.m_min[axis] = a_min[axis];
662 rect.m_max[axis] = a_max[axis];
665 InsertRect( &rect, a_dataId, &m_root, 0 );
670 bool RTREE_QUAL::Remove(
const ELEMTYPE a_min[NUMDIMS],
671 const ELEMTYPE a_max[NUMDIMS],
672 const DATATYPE& a_dataId )
676 for(
int index = 0; index<NUMDIMS; ++index )
678 ASSERT( a_min[index] <= a_max[index] );
685 for(
int axis = 0; axis<NUMDIMS; ++axis )
687 rect.m_min[axis] = a_min[axis];
688 rect.m_max[axis] = a_max[axis];
691 return RemoveRect( &rect, a_dataId, &m_root );
696 int RTREE_QUAL::Search(
const ELEMTYPE a_min[NUMDIMS],
697 const ELEMTYPE a_max[NUMDIMS],
698 bool a_resultCallback( DATATYPE a_data,
void* a_context ),
703 for(
int index = 0; index<NUMDIMS; ++index )
705 ASSERT( a_min[index] <= a_max[index] );
712 for(
int axis = 0; axis<NUMDIMS; ++axis )
714 rect.m_min[axis] = a_min[axis];
715 rect.m_max[axis] = a_max[axis];
721 Search( m_root, &rect, foundCount, a_resultCallback, a_context );
727 DATATYPE RTREE_QUAL::NearestNeighbor(
const ELEMTYPE a_point[NUMDIMS] )
729 return this->NearestNeighbor( a_point, 0, 0 );
734 DATATYPE RTREE_QUAL::NearestNeighbor(
const ELEMTYPE a_point[NUMDIMS],
735 ELEMTYPE a_squareDistanceCallback(
const ELEMTYPE a_point[NUMDIMS], DATATYPE a_data ),
736 ELEMTYPE* a_squareDistance )
738 typedef typename std::vector<NNNode*>::iterator iterator;
739 std::vector<NNNode*> nodeList;
741 NNNode* closestNode = 0;
742 while( !closestNode || !closestNode->isLeaf )
745 for(
int index = 0; index < node->m_count; ++index )
747 NNNode* newNode =
new NNNode;
748 newNode->isLeaf = node->IsLeaf();
749 newNode->m_branch = node->m_branch[index];
750 if( newNode->isLeaf && a_squareDistanceCallback )
751 newNode->minDist = a_squareDistanceCallback( a_point, newNode->m_branch.m_data );
753 newNode->minDist = this->MinDist( a_point, &(node->m_branch[index].m_rect) );
756 this->InsertNNListSorted( &nodeList, newNode );
758 if( nodeList.size() == 0 )
762 closestNode = nodeList.back();
763 node = closestNode->m_branch.m_child;
769 for( iterator iter = nodeList.begin(); iter != nodeList.end(); ++iter )
771 NNNode* nnode = *iter;
775 *a_squareDistance = closestNode->minDist;
776 return closestNode->m_branch.m_data;
781 int RTREE_QUAL::Count()
785 CountRec( m_root, count );
792 void RTREE_QUAL::CountRec( Node* a_node,
int& a_count )
794 if( a_node->IsInternalNode() )
796 for(
int index = 0; index < a_node->m_count; ++index )
798 CountRec( a_node->m_branch[index].m_child, a_count );
803 a_count += a_node->m_count;
809 bool RTREE_QUAL::Load(
const char* a_fileName )
815 if( !stream.OpenRead( a_fileName ) )
820 bool result = Load( stream );
832 int _dataFileId = (
'R' << 0) | (
'T' << 8) | (
'R' << 16) | (
'E' << 24);
833 int _dataSize =
sizeof(DATATYPE);
834 int _dataNumDims = NUMDIMS;
835 int _dataElemSize =
sizeof(ELEMTYPE);
836 int _dataElemRealSize =
sizeof(ELEMTYPEREAL);
837 int _dataMaxNodes = TMAXNODES;
838 int _dataMinNodes = TMINNODES;
843 int dataElemSize = 0;
844 int dataElemRealSize = 0;
845 int dataMaxNodes = 0;
846 int dataMinNodes = 0;
848 a_stream.Read( dataFileId );
849 a_stream.Read( dataSize );
850 a_stream.Read( dataNumDims );
851 a_stream.Read( dataElemSize );
852 a_stream.Read( dataElemRealSize );
853 a_stream.Read( dataMaxNodes );
854 a_stream.Read( dataMinNodes );
859 if( (dataFileId == _dataFileId)
860 && (dataSize == _dataSize)
861 && (dataNumDims == _dataNumDims)
862 && (dataElemSize == _dataElemSize)
863 && (dataElemRealSize == _dataElemRealSize)
864 && (dataMaxNodes == _dataMaxNodes)
865 && (dataMinNodes == _dataMinNodes)
869 result = LoadRec( m_root, a_stream );
877 bool RTREE_QUAL::LoadRec( Node* a_node,
RTFileStream& a_stream )
879 a_stream.Read( a_node->m_level );
880 a_stream.Read( a_node->m_count );
882 if( a_node->IsInternalNode() )
884 for(
int index = 0; index < a_node->m_count; ++index )
886 Branch* curBranch = &a_node->m_branch[index];
888 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
889 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
891 curBranch->m_child = AllocNode();
892 LoadRec( curBranch->m_child, a_stream );
897 for(
int index = 0; index < a_node->m_count; ++index )
899 Branch* curBranch = &a_node->m_branch[index];
901 a_stream.ReadArray( curBranch->m_rect.m_min, NUMDIMS );
902 a_stream.ReadArray( curBranch->m_rect.m_max, NUMDIMS );
904 a_stream.Read( curBranch->m_data );
913 bool RTREE_QUAL::Save(
const char* a_fileName )
917 if( !stream.OpenWrite( a_fileName ) )
922 bool result = Save( stream );
934 int dataFileId = (
'R' << 0) | (
'T' << 8) | (
'R' << 16) | (
'E' << 24);
935 int dataSize =
sizeof(DATATYPE);
936 int dataNumDims = NUMDIMS;
937 int dataElemSize =
sizeof(ELEMTYPE);
938 int dataElemRealSize =
sizeof(ELEMTYPEREAL);
939 int dataMaxNodes = TMAXNODES;
940 int dataMinNodes = TMINNODES;
942 a_stream.Write( dataFileId );
943 a_stream.Write( dataSize );
944 a_stream.Write( dataNumDims );
945 a_stream.Write( dataElemSize );
946 a_stream.Write( dataElemRealSize );
947 a_stream.Write( dataMaxNodes );
948 a_stream.Write( dataMinNodes );
951 bool result = SaveRec( m_root, a_stream );
958 bool RTREE_QUAL::SaveRec( Node* a_node,
RTFileStream& a_stream )
960 a_stream.Write( a_node->m_level );
961 a_stream.Write( a_node->m_count );
963 if( a_node->IsInternalNode() )
965 for(
int index = 0; index < a_node->m_count; ++index )
967 Branch* curBranch = &a_node->m_branch[index];
969 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
970 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
972 SaveRec( curBranch->m_child, a_stream );
977 for(
int index = 0; index < a_node->m_count; ++index )
979 Branch* curBranch = &a_node->m_branch[index];
981 a_stream.WriteArray( curBranch->m_rect.m_min, NUMDIMS );
982 a_stream.WriteArray( curBranch->m_rect.m_max, NUMDIMS );
984 a_stream.Write( curBranch->m_data );
993 void RTREE_QUAL::RemoveAll()
998 m_root = AllocNode();
1004 void RTREE_QUAL::Reset()
1006 #ifdef RTREE_DONT_USE_MEMPOOLS
1008 RemoveAllRec( m_root );
1009 #else // RTREE_DONT_USE_MEMPOOLS
1012 #endif // RTREE_DONT_USE_MEMPOOLS
1017 void RTREE_QUAL::RemoveAllRec( Node* a_node )
1020 ASSERT( a_node->m_level >= 0 );
1022 if( a_node->IsInternalNode() )
1024 for(
int index = 0; index < a_node->m_count; ++index )
1026 RemoveAllRec( a_node->m_branch[index].m_child );
1035 typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
1039 #ifdef RTREE_DONT_USE_MEMPOOLS
1041 #else // RTREE_DONT_USE_MEMPOOLS
1043 #endif // RTREE_DONT_USE_MEMPOOLS
1044 InitNode( newNode );
1050 void RTREE_QUAL::FreeNode( Node* a_node )
1054 #ifdef RTREE_DONT_USE_MEMPOOLS
1056 #else // RTREE_DONT_USE_MEMPOOLS
1058 #endif // RTREE_DONT_USE_MEMPOOLS
1065 typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
1067 #ifdef RTREE_DONT_USE_MEMPOOLS
1068 return new ListNode;
1069 #else // RTREE_DONT_USE_MEMPOOLS
1071 #endif // RTREE_DONT_USE_MEMPOOLS
1076 void RTREE_QUAL::FreeListNode( ListNode* a_listNode )
1078 #ifdef RTREE_DONT_USE_MEMPOOLS
1080 #else // RTREE_DONT_USE_MEMPOOLS
1082 #endif // RTREE_DONT_USE_MEMPOOLS
1087 void RTREE_QUAL::InitNode( Node* a_node )
1089 a_node->m_count = 0;
1090 a_node->m_level = -1;
1095 void RTREE_QUAL::InitRect( Rect* a_rect )
1097 for(
int index = 0; index < NUMDIMS; ++index )
1099 a_rect->m_min[index] = (ELEMTYPE) 0;
1100 a_rect->m_max[index] = (ELEMTYPE) 0;
1113 bool RTREE_QUAL::InsertRectRec( Rect* a_rect,
1114 const DATATYPE& a_id,
1119 ASSERT( a_rect && a_node && a_newNode );
1120 ASSERT( a_level >= 0 && a_level <= a_node->m_level );
1127 if( a_node->m_level > a_level )
1129 index = PickBranch( a_rect, a_node );
1131 if( !InsertRectRec( a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level ) )
1134 a_node->m_branch[index].m_rect =
1135 CombineRect( a_rect, &(a_node->m_branch[index].m_rect) );
1140 a_node->m_branch[index].m_rect = NodeCover( a_node->m_branch[index].m_child );
1141 branch.m_child = otherNode;
1142 branch.m_rect = NodeCover( otherNode );
1143 return AddBranch( &branch, a_node, a_newNode );
1146 else if( a_node->m_level == a_level )
1148 branch.m_rect = *a_rect;
1149 branch.m_child = (Node*) a_id;
1151 return AddBranch( &branch, a_node, a_newNode );
1170 bool RTREE_QUAL::InsertRect( Rect* a_rect,
const DATATYPE& a_id, Node** a_root,
int a_level )
1172 ASSERT( a_rect && a_root );
1173 ASSERT( a_level >= 0 && a_level <= (*a_root)->m_level );
1176 for(
int index = 0; index < NUMDIMS; ++index )
1178 ASSERT( a_rect->m_min[index] <= a_rect->m_max[index] );
1187 if( InsertRectRec( a_rect, a_id, *a_root, &newNode, a_level ) )
1189 newRoot = AllocNode();
1190 newRoot->m_level = (*a_root)->m_level + 1;
1191 branch.m_rect = NodeCover( *a_root );
1192 branch.m_child = *a_root;
1193 AddBranch( &branch, newRoot, NULL );
1194 branch.m_rect = NodeCover( newNode );
1195 branch.m_child = newNode;
1196 AddBranch( &branch, newRoot, NULL );
1207 typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover( Node* a_node )
1211 int firstTime =
true;
1215 for(
int index = 0; index < a_node->m_count; ++index )
1219 rect = a_node->m_branch[index].m_rect;
1224 rect = CombineRect( &rect, &(a_node->m_branch[index].m_rect) );
1237 bool RTREE_QUAL::AddBranch( Branch* a_branch, Node* a_node, Node** a_newNode )
1242 if( a_node->m_count < MAXNODES )
1244 a_node->m_branch[a_node->m_count] = *a_branch;
1251 ASSERT( a_newNode );
1253 SplitNode( a_node, a_branch, a_newNode );
1262 void RTREE_QUAL::DisconnectBranch( Node* a_node,
int a_index )
1264 ASSERT( a_node && (a_index >= 0) && (a_index < MAXNODES) );
1265 ASSERT( a_node->m_count > 0 );
1268 a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
1280 int RTREE_QUAL::PickBranch( Rect* a_rect, Node* a_node )
1282 ASSERT( a_rect && a_node );
1284 bool firstTime =
true;
1285 ELEMTYPEREAL increase;
1286 ELEMTYPEREAL bestIncr = (ELEMTYPEREAL) -1;
1288 ELEMTYPEREAL bestArea = 0;
1292 for(
int index = 0; index < a_node->m_count; ++index )
1294 Rect* curRect = &a_node->m_branch[index].m_rect;
1295 area = CalcRectVolume( curRect );
1296 tempRect = CombineRect( a_rect, curRect );
1297 increase = CalcRectVolume( &tempRect ) - area;
1299 if( (increase < bestIncr) || firstTime )
1303 bestIncr = increase;
1306 else if( (increase == bestIncr) && (area < bestArea) )
1310 bestIncr = increase;
1320 typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect( Rect* a_rectA, Rect* a_rectB )
1322 ASSERT( a_rectA && a_rectB );
1326 for(
int index = 0; index < NUMDIMS; ++index )
1328 newRect.m_min[index] = rMin( a_rectA->m_min[index], a_rectB->m_min[index] );
1329 newRect.m_max[index] = rMax( a_rectA->m_max[index], a_rectB->m_max[index] );
1341 void RTREE_QUAL::SplitNode( Node* a_node, Branch* a_branch, Node** a_newNode )
1347 PartitionVars localVars;
1348 PartitionVars* parVars = &localVars;
1352 level = a_node->m_level;
1353 GetBranches( a_node, a_branch, parVars );
1356 ChoosePartition( parVars, MINNODES );
1359 *a_newNode = AllocNode();
1360 (*a_newNode)->m_level = a_node->m_level = level;
1361 LoadNodes( a_node, *a_newNode, parVars );
1363 ASSERT( (a_node->m_count + (*a_newNode)->m_count) == parVars->m_total );
1369 ELEMTYPEREAL RTREE_QUAL::RectVolume( Rect* a_rect )
1373 ELEMTYPEREAL volume = (ELEMTYPEREAL) 1;
1375 for(
int index = 0; index<NUMDIMS; ++index )
1377 volume *= a_rect->m_max[index] - a_rect->m_min[index];
1380 ASSERT( volume >= (ELEMTYPEREAL) 0 );
1388 ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume( Rect* a_rect )
1392 ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL) 0;
1393 ELEMTYPEREAL radius;
1395 for(
int index = 0; index < NUMDIMS; ++index )
1397 ELEMTYPEREAL halfExtent =
1398 ( (ELEMTYPEREAL) a_rect->m_max[index] - (ELEMTYPEREAL) a_rect->m_min[index] ) * 0.5f;
1399 sumOfSquares += halfExtent * halfExtent;
1402 radius = (ELEMTYPEREAL) sqrt( sumOfSquares );
1407 return radius * radius * radius * m_unitSphereVolume;
1409 else if( NUMDIMS == 2 )
1411 return radius * radius * m_unitSphereVolume;
1415 return (ELEMTYPEREAL) (pow( radius, NUMDIMS ) * m_unitSphereVolume);
1422 ELEMTYPEREAL RTREE_QUAL::CalcRectVolume( Rect* a_rect )
1424 #ifdef RTREE_USE_SPHERICAL_VOLUME
1425 return RectSphericalVolume( a_rect );
1426 #else // RTREE_USE_SPHERICAL_VOLUME
1427 return RectVolume( a_rect );
1428 #endif // RTREE_USE_SPHERICAL_VOLUME
1434 void RTREE_QUAL::GetBranches( Node* a_node, Branch* a_branch, PartitionVars* a_parVars )
1439 ASSERT( a_node->m_count == MAXNODES );
1442 for(
int index = 0; index < MAXNODES; ++index )
1444 a_parVars->m_branchBuf[index] = a_node->m_branch[index];
1447 a_parVars->m_branchBuf[MAXNODES] = *a_branch;
1448 a_parVars->m_branchCount = MAXNODES + 1;
1451 a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
1453 for(
int index = 1; index < MAXNODES + 1; ++index )
1455 a_parVars->m_coverSplit =
1456 CombineRect( &a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect );
1459 a_parVars->m_coverSplitArea = CalcRectVolume( &a_parVars->m_coverSplit );
1477 void RTREE_QUAL::ChoosePartition( PartitionVars* a_parVars,
int a_minFill )
1479 ASSERT( a_parVars );
1481 ELEMTYPEREAL biggestDiff;
1482 int group, chosen = 0, betterGroup = 0;
1484 InitParVars( a_parVars, a_parVars->m_branchCount, a_minFill );
1485 PickSeeds( a_parVars );
1487 while( ( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1488 && ( a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill) )
1489 && ( a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill) ) )
1491 biggestDiff = (ELEMTYPEREAL) -1;
1493 for(
int index = 0; index<a_parVars->m_total; ++index )
1495 if( !a_parVars->m_taken[index] )
1497 Rect* curRect = &a_parVars->m_branchBuf[index].m_rect;
1498 Rect rect0 = CombineRect( curRect, &a_parVars->m_cover[0] );
1499 Rect rect1 = CombineRect( curRect, &a_parVars->m_cover[1] );
1500 ELEMTYPEREAL growth0 = CalcRectVolume( &rect0 ) - a_parVars->m_area[0];
1501 ELEMTYPEREAL growth1 = CalcRectVolume( &rect1 ) - a_parVars->m_area[1];
1502 ELEMTYPEREAL diff = growth1 - growth0;
1514 if( diff > biggestDiff )
1518 betterGroup = group;
1520 else if( (diff == biggestDiff)
1521 && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]) )
1524 betterGroup = group;
1529 Classify( chosen, betterGroup, a_parVars );
1533 if( (a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total )
1535 if( a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill )
1544 for(
int index = 0; index<a_parVars->m_total; ++index )
1546 if( !a_parVars->m_taken[index] )
1548 Classify( index, group, a_parVars );
1553 ASSERT( (a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total );
1554 ASSERT( (a_parVars->m_count[0] >= a_parVars->m_minFill)
1555 && (a_parVars->m_count[1] >= a_parVars->m_minFill) );
1561 void RTREE_QUAL::LoadNodes( Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars )
1565 ASSERT( a_parVars );
1567 for(
int index = 0; index < a_parVars->m_total; ++index )
1569 ASSERT( a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1 );
1571 if( a_parVars->m_partition[index] == 0 )
1573 AddBranch( &a_parVars->m_branchBuf[index], a_nodeA, NULL );
1575 else if( a_parVars->m_partition[index] == 1 )
1577 AddBranch( &a_parVars->m_branchBuf[index], a_nodeB, NULL );
1585 void RTREE_QUAL::InitParVars( PartitionVars* a_parVars,
int a_maxRects,
int a_minFill )
1587 ASSERT( a_parVars );
1589 a_parVars->m_count[0] = a_parVars->m_count[1] = 0;
1590 a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL) 0;
1591 a_parVars->m_total = a_maxRects;
1592 a_parVars->m_minFill = a_minFill;
1594 for(
int index = 0; index < a_maxRects; ++index )
1596 a_parVars->m_taken[index] =
false;
1597 a_parVars->m_partition[index] = -1;
1603 void RTREE_QUAL::PickSeeds( PartitionVars* a_parVars )
1605 int seed0 = 0, seed1 = 0;
1606 ELEMTYPEREAL worst, waste;
1607 ELEMTYPEREAL area[MAXNODES + 1];
1609 for(
int index = 0; index<a_parVars->m_total; ++index )
1611 area[index] = CalcRectVolume( &a_parVars->m_branchBuf[index].m_rect );
1614 worst = -a_parVars->m_coverSplitArea - 1;
1616 for(
int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA )
1618 for(
int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB )
1620 Rect oneRect = CombineRect( &a_parVars->m_branchBuf[indexA].m_rect,
1621 &a_parVars->m_branchBuf[indexB].m_rect );
1622 waste = CalcRectVolume( &oneRect ) - area[indexA] - area[indexB];
1624 if( waste >= worst )
1633 Classify( seed0, 0, a_parVars );
1634 Classify( seed1, 1, a_parVars );
1640 void RTREE_QUAL::Classify(
int a_index,
int a_group, PartitionVars* a_parVars )
1642 ASSERT( a_parVars );
1643 ASSERT( !a_parVars->m_taken[a_index] );
1645 a_parVars->m_partition[a_index] = a_group;
1646 a_parVars->m_taken[a_index] =
true;
1648 if( a_parVars->m_count[a_group] == 0 )
1650 a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
1654 a_parVars->m_cover[a_group] = CombineRect( &a_parVars->m_branchBuf[a_index].m_rect,
1655 &a_parVars->m_cover[a_group] );
1658 a_parVars->m_area[a_group] = CalcRectVolume( &a_parVars->m_cover[a_group] );
1659 ++a_parVars->m_count[a_group];
1668 bool RTREE_QUAL::RemoveRect( Rect* a_rect,
const DATATYPE& a_id, Node** a_root )
1670 ASSERT( a_rect && a_root );
1674 ListNode* reInsertList = NULL;
1676 if( !RemoveRectRec( a_rect, a_id, *a_root, &reInsertList ) )
1680 while( reInsertList )
1682 tempNode = reInsertList->m_node;
1684 for(
int index = 0; index < tempNode->m_count; ++index )
1686 InsertRect( &(tempNode->m_branch[index].m_rect),
1687 tempNode->m_branch[index].m_data,
1689 tempNode->m_level );
1692 ListNode* remLNode = reInsertList;
1693 reInsertList = reInsertList->m_next;
1695 FreeNode( remLNode->m_node );
1696 FreeListNode( remLNode );
1700 if( (*a_root)->m_count == 1 && (*a_root)->IsInternalNode() )
1702 tempNode = (*a_root)->m_branch[0].m_child;
1705 FreeNode( *a_root );
1723 bool RTREE_QUAL::RemoveRectRec( Rect* a_rect,
1724 const DATATYPE& a_id,
1726 ListNode** a_listNode )
1728 ASSERT( a_rect && a_node && a_listNode );
1729 ASSERT( a_node->m_level >= 0 );
1731 if( a_node->IsInternalNode() )
1733 for(
int index = 0; index < a_node->m_count; ++index )
1735 if( Overlap( a_rect, &(a_node->m_branch[index].m_rect) ) )
1737 if( !RemoveRectRec( a_rect, a_id, a_node->m_branch[index].m_child, a_listNode ) )
1739 if( a_node->m_branch[index].m_child->m_count >= MINNODES )
1742 a_node->m_branch[index].m_rect =
1743 NodeCover( a_node->m_branch[index].m_child );
1748 ReInsert( a_node->m_branch[index].m_child, a_listNode );
1749 DisconnectBranch( a_node, index );
1761 for(
int index = 0; index < a_node->m_count; ++index )
1763 if( a_node->m_branch[index].m_child == (Node*) a_id )
1765 DisconnectBranch( a_node, index );
1777 bool RTREE_QUAL::Overlap( Rect* a_rectA, Rect* a_rectB )
1779 ASSERT( a_rectA && a_rectB );
1781 for(
int index = 0; index < NUMDIMS; ++index )
1783 if( a_rectA->m_min[index] > a_rectB->m_max[index]
1784 || a_rectB->m_min[index] > a_rectA->m_max[index] )
1797 void RTREE_QUAL::ReInsert( Node* a_node, ListNode** a_listNode )
1799 ListNode* newListNode;
1801 newListNode = AllocListNode();
1802 newListNode->m_node = a_node;
1803 newListNode->m_next = *a_listNode;
1804 *a_listNode = newListNode;
1810 bool RTREE_QUAL::Search( Node* a_node, Rect* a_rect,
int& a_foundCount,
bool a_resultCallback(
1812 void* a_context ),
void* a_context )
1815 ASSERT( a_node->m_level >= 0 );
1818 if( a_node->IsInternalNode() )
1820 for(
int index = 0; index < a_node->m_count; ++index )
1822 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1824 if( !Search( a_node->m_branch[index].m_child, a_rect, a_foundCount,
1825 a_resultCallback, a_context ) )
1834 for(
int index = 0; index < a_node->m_count; ++index )
1836 if( Overlap( a_rect, &a_node->m_branch[index].m_rect ) )
1838 DATATYPE&
id = a_node->m_branch[index].m_data;
1841 if( &a_resultCallback )
1845 if( !a_resultCallback(
id, a_context ) )
1861 ELEMTYPE RTREE_QUAL::MinDist(
const ELEMTYPE a_point[NUMDIMS], Rect* a_rect )
1863 ELEMTYPE *q, *s, *t;
1864 q = (ELEMTYPE*) a_point;
1868 for(
int index = 0; index < NUMDIMS; index++ )
1871 if( q[index] < s[index] )
1875 else if( q[index] >t[index] )
1879 int addend = q[index] - r;
1880 minDist += addend * addend;
1888 void RTREE_QUAL::InsertNNListSorted( std::vector<NNNode*>* nodeList, NNNode* newNode )
1890 typedef typename std::vector<NNNode*>::iterator iterator;
1891 iterator iter = nodeList->begin();
1892 while( iter != nodeList->end() && (*iter)->minDist > newNode->minDist )
1896 nodeList->insert(iter, newNode);
1900 #undef RTREE_TEMPLATE
1902 #undef RTREE_SEARCH_TEMPLATE
1903 #undef RTREE_SEARCH_QUAL