diff --git a/src/rtree.h b/src/rtree.h index 37e730e1..b308dcda 100644 --- a/src/rtree.h +++ b/src/rtree.h @@ -13,55 +13,54 @@ #define Min(a,b) \ (((a) < (b)) ? (a) : (b)) -// -// RTree.h -// +#define RTREE_TEMPLATE template +#define RTREE_QUAL RTree -#define RTREE_TEMPLATE template -#define RTREE_QUAL RTree - -#define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one. -#define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems - -// Fwd decl -class RTFileStream; // File I/O helper class, look below for implementation and notes. +// This version does not contain a fixed memory allocator, fill in lines with +// EXAMPLE to implement one. +#define RTREE_DONT_USE_MEMPOOLS +// Better split classification, may be slower on some systems +#define RTREE_USE_SPHERICAL_VOLUME /// \class RTree +/// /// Implementation of RTree, a multidimensional bounding rectangle tree. /// Example usage: For a 3-dimensional tree use RTree myTree; /// -/// This modified, templated C++ version by Greg Douglas at Auran (http://www.auran.com) +/// This modified, templated C++ version by Greg Douglas at Auran +/// (http://www.auran.com) /// -/// DATATYPE Referenced data, should be int, void*, obj* etc. no larger than sizeof and simple type -/// ELEMTYPE Type of element such as int or float -/// NUMDIMS Number of dimensions such as 2 or 3 -/// ELEMTYPEREAL Type of element that allows fractional and large values such as float or double, for use in volume calcs +/// \c DATATYPE Referenced data, should be int, void*, obj* etc. no larger than +/// sizeof and simple type +/// \c ELEMTYPE Type of element such as int or float +/// \c NUMDIMS Number of dimensions such as 2 or 3 +/// \c ELEMTYPEREAL Type of element that allows fractional and large values such +/// as float or double, for use in volume calcs /// -/// NOTES: Inserting and removing data requires the knowledge of its constant Minimal Bounding Rectangle. -/// This version uses new/delete for nodes, I recommend using a fixed size allocator for efficiency. -/// Instead of using a callback function for returned results, I recommend and efficient pre-sized, grow-only memory -/// array similar to MFC CArray or STL Vector for returning search query result. +/// NOTES: Inserting and removing data requires the knowledge of its constant +/// Minimal Bounding Rectangle. This version uses new/delete for nodes, +/// I recommend using a fixed size allocator for efficiency. Instead of using +/// a callback function for returned results, I recommend and efficient +/// pre-sized, grow-only memory array similar to MFC CArray or STL Vector for +/// returning search query result. /// -template class RTree { protected: - struct Node; // Fwd decl. Used by other internal structs and iterator - + public: - - // These constant must be declared after Branch and before Node struct - // Stuck up here for MSVC 6 compiler. NSVC .NET 2003 is much happier. enum { - MAXNODES = TMAXNODES, ///< Max elements in node - MINNODES = TMINNODES, ///< Min elements in node + MAXNODES = TMAXNODES, ///< Max elements in node + MINNODES = TMINNODES, ///< Min elements in node }; - - + public: RTree(); @@ -70,27 +69,35 @@ public: /// Insert entry /// \param a_min Min of bounding rect /// \param a_max Max of bounding rect - /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed. - void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId); + /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers + /// not allowed. + void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], + const DATATYPE& a_dataId); /// Remove entry /// \param a_min Min of bounding rect /// \param a_max Max of bounding rect - /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed. - void Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId); + /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers + /// not allowed. + void Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], + const DATATYPE& a_dataId); /// Find all within search rectangle /// \param a_min Min of search bounding rect /// \param a_max Max of search bounding rect - /// \param a_resultCallback Callback function to return result. Callback should return 'true' to continue searching + /// \param a_resultCallback Callback function to return result. Callback + /// should return 'true' to continue searching /// \param a_context User context to pass as parameter to a_resultCallback /// \return Returns the number of entries found - int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], bool a_resultCallback(DATATYPE a_data, void* a_context), void* a_context) const; + int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], + bool a_resultCallback(DATATYPE a_data, void* a_context), + void* a_context) const; /// Remove all entries from tree void RemoveAll(); - /// Count the data elements in this container. This is slow as no internal counter is maintained. + /// Count the data elements in this container. This is slow as no internal + /// counter is maintained. int Count(); @@ -98,109 +105,102 @@ public: class Iterator { private: - - enum { MAX_STACK = 32 }; // Max stack size. Allows almost n^32 where n is number of branches in node - + // Max stack size. Allows almost n^32 where n is number of branches in + // node + enum { MAX_STACK = 32 }; + struct StackElement { Node* m_node; int m_branchIndex; }; - + public: - - Iterator() { Init(); } - - ~Iterator() { } - + Iterator() { Init(); } + + ~Iterator() { } + /// Is iterator invalid - bool IsNull() { return (m_tos <= 0); } - + bool IsNull() { return (m_tos <= 0); } + /// Is iterator pointing to valid data - bool IsNotNull() { return (m_tos > 0); } - - /// Access the current data element. Caller must be sure iterator is not NULL first. + bool IsNotNull() { return (m_tos > 0); } + + /// Access the current data element. DATATYPE& operator*() { ASSERT(IsNotNull()); StackElement& curTos = m_stack[m_tos - 1]; return curTos.m_node->m_branch[curTos.m_branchIndex].m_data; } - - /// Access the current data element. Caller must be sure iterator is not NULL first. + + /// Access the current data element. const DATATYPE& operator*() const { ASSERT(IsNotNull()); StackElement& curTos = m_stack[m_tos - 1]; return curTos.m_node->m_branch[curTos.m_branchIndex].m_data; } - + /// Find the next data element - bool operator++() { return FindNextData(); } - + bool operator++() { return FindNextData(); } + /// Get the bounds for this node void GetBounds(ELEMTYPE a_min[NUMDIMS], ELEMTYPE a_max[NUMDIMS]) { ASSERT(IsNotNull()); StackElement& curTos = m_stack[m_tos - 1]; Branch& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex]; - - for(int index = 0; index < NUMDIMS; ++index) - { + + for (int index = 0; index < NUMDIMS; ++index) { a_min[index] = curBranch.m_rect.m_min[index]; a_max[index] = curBranch.m_rect.m_max[index]; } } - + private: - - /// Reset iterator - void Init() { m_tos = 0; } - - /// Find the next data element in the tree (For internal use only) + // Reset iterator + void Init() { m_tos = 0; } + + // Find the next data element in the tree (For internal use only) bool FindNextData() { - for(;;) - { - if(m_tos <= 0) - { + for (;;) { + if (m_tos <= 0) return false; - } - StackElement curTos = Pop(); // Copy stack top cause it may change as we use it - - if(curTos.m_node->IsLeaf()) - { + + // Copy stack top cause it may change as we use it + StackElement curTos = Pop(); + + if (curTos.m_node->IsLeaf()) { // Keep walking through data while we can - if(curTos.m_branchIndex+1 < curTos.m_node->m_count) - { + if (curTos.m_branchIndex+1 < curTos.m_node->m_count) { // There is more data, just point to the next one Push(curTos.m_node, curTos.m_branchIndex + 1); return true; } // No more data, so it will fall back to previous level - } - else - { - if(curTos.m_branchIndex+1 < curTos.m_node->m_count) - { + } else { + if (curTos.m_branchIndex+1 < curTos.m_node->m_count) { // Push sibling on for future tree walk - // This is the 'fall back' node when we finish with the current level + // This is the 'fall back' node when we finish with + // the current level Push(curTos.m_node, curTos.m_branchIndex + 1); } - // Since cur node is not a leaf, push first of next level to get deeper into the tree + // Since cur node is not a leaf, push first of next level to + // get deeper into the tree Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child; Push(nextLevelnode, 0); - // If we pushed on a new leaf, exit as the data is ready at TOS - if(nextLevelnode->IsLeaf()) - { + // If we pushed on a new leaf, exit as the data is ready at + // TOS + if (nextLevelnode->IsLeaf()) return true; - } } } } - - /// Push node and branch onto iteration stack (For internal use only) + + // Push node and branch onto iteration stack void Push(Node* a_node, int a_branchIndex) { m_stack[m_tos].m_node = a_node; @@ -208,91 +208,90 @@ public: ++m_tos; ASSERT(m_tos <= MAX_STACK); } - - /// Pop element off iteration stack (For internal use only) + + // Pop element off iteration stack StackElement& Pop() { ASSERT(m_tos > 0); --m_tos; return m_stack[m_tos]; } - - StackElement m_stack[MAX_STACK]; ///< Stack as we are doing iteration instead of recursion - int m_tos; ///< Top Of Stack index + + // Stack as we are doing iteration instead of recursion + StackElement m_stack[MAX_STACK]; + // Top Of Stack index + int m_tos; }; - - /// Get 'first' for iteration + + // Get 'first' for iteration void GetFirst(Iterator& a_it) { a_it.Init(); Node* first = m_root; - while(first) - { - if(first->IsInternalNode() && first->m_count > 1) - { + + while (first) { + if (first->IsInternalNode() && first->m_count > 1) { a_it.Push(first, 1); // Descend sibling branch later - } - else if(first->IsLeaf()) - { - if(first->m_count) - { + } else if(first->IsLeaf()) { + if(first->m_count) { a_it.Push(first, 0); } break; } first = first->m_branch[0].m_child; } - } - - /// Get Next for iteration - void GetNext(Iterator& a_it) { ++a_it; } - - /// Is iterator NULL, or at end? - bool IsNull(Iterator& a_it) { return a_it.IsNull(); } - - /// Get object at iterator position - DATATYPE& GetAt(Iterator& a_it) { return *a_it; } - + } + + // Get Next for iteration + void GetNext(Iterator& a_it) { ++a_it; } + + // Is iterator NULL, or at end? + bool IsNull(Iterator& a_it) { return a_it.IsNull(); } + + // Get object at iterator position + DATATYPE& GetAt(Iterator& a_it) { return *a_it; } + protected: - - /// Minimal bounding rectangle (n-dimensional) + // Minimal bounding rectangle (n-dimensional) struct Rect { - ELEMTYPE m_min[NUMDIMS]; ///< Min dimensions of bounding box - ELEMTYPE m_max[NUMDIMS]; ///< Max dimensions of bounding box + ELEMTYPE m_min[NUMDIMS]; ///< Min dimensions of bounding box + ELEMTYPE m_max[NUMDIMS]; ///< Max dimensions of bounding box }; - + /// May be data or may be another subtree /// The parents level determines this. /// If the parents level is 0, then this is data struct Branch { - Rect m_rect; ///< Bounds + Rect m_rect; ///< Bounds union { - Node* m_child; ///< Child node - DATATYPE m_data; ///< Data Id or Ptr + Node* m_child; ///< Child node + DATATYPE m_data; ///< Data Id or Ptr }; }; - + /// Node for each branch level struct Node { - bool IsInternalNode() { return (m_level > 0); } // Not a leaf, but a internal node - bool IsLeaf() { return (m_level == 0); } // A leaf, contains data - - int m_count; ///< Count - int m_level; ///< Leaf is zero, others positive - Branch m_branch[MAXNODES]; ///< Branch + // Not a leaf, but a internal node + bool IsInternalNode() { return (m_level > 0); } + // A leaf, contains data + bool IsLeaf() { return (m_level == 0); } + + int m_count; ///< Count + int m_level; ///< Leaf is zero, others positive + Branch m_branch[MAXNODES]; ///< Branch }; - + /// A link list of nodes for reinsertion after a delete operation struct ListNode { - ListNode* m_next; ///< Next in list - Node* m_node; ///< Node + ListNode* m_next; ///< Next in list + Node* m_node; ///< Node }; - + /// Variables for finding a split partition struct PartitionVars { @@ -303,19 +302,21 @@ protected: int m_count[2]; Rect m_cover[2]; ELEMTYPEREAL m_area[2]; - + Branch m_branchBuf[MAXNODES+1]; int m_branchCount; Rect m_coverSplit; ELEMTYPEREAL m_coverSplitArea; }; - + Node* AllocNode(); void FreeNode(Node* a_node); void InitNode(Node* a_node); void InitRect(Rect* a_rect); - bool InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, Node** a_newNode, int a_level); - bool InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level); + bool InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, + Node** a_newNode, int a_level); + bool InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, + int a_level); Rect NodeCover(Node* a_node); bool AddBranch(Branch* a_branch, Node* a_node, Node** a_newNode); void DisconnectBranch(Node* a_node, int a_index); @@ -332,18 +333,23 @@ protected: void PickSeeds(PartitionVars* a_parVars); void Classify(int a_index, int a_group, PartitionVars* a_parVars); bool RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root); - bool RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode); + bool RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, + ListNode** a_listNode); ListNode* AllocListNode(); void FreeListNode(ListNode* a_listNode); bool Overlap(Rect* a_rectA, Rect* a_rectB) const; void ReInsert(Node* a_node, ListNode** a_listNode); - bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, bool a_resultCallback(DATATYPE a_data, void* a_context), void* a_context) const; + bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, + bool a_resultCallback(DATATYPE a_data, void* a_context), + void* a_context) const; void RemoveAllRec(Node* a_node); void Reset(); void CountRec(Node* a_node, int& a_count); - Node* m_root; ///< Root of tree - ELEMTYPEREAL m_unitSphereVolume; ///< Unit sphere constant for required number of dimensions + /// Root of tree + Node* m_root; + /// Unit sphere constant for required number of dimensions + ELEMTYPEREAL m_unitSphereVolume; }; @@ -352,12 +358,11 @@ RTREE_QUAL::RTree() { ASSERT(MAXNODES > MINNODES); ASSERT(MINNODES > 0); - - - // We only support machine word size simple data type eg. integer index or object pointer. - // Since we are storing as union with non data branch + + // We only support machine word size simple data type eg. integer index or + // object pointer. Since we are storing as union with non data branch ASSERT(sizeof(DATATYPE) == sizeof(void*) || sizeof(DATATYPE) == sizeof(int)); - + // Precomputed volumes of the unit spheres for the first few dimensions const float UNIT_SPHERE_VOLUMES[] = { 0.000000f, 2.000000f, 3.141593f, // Dimension 0,1,2 @@ -368,7 +373,7 @@ RTREE_QUAL::RTree() 0.381443f, 0.235331f, 0.140981f, // Dimension 15,16,17 0.082146f, 0.046622f, 0.025807f, // Dimension 18,19,20 }; - + m_root = AllocNode(); m_root->m_level = 0; m_unitSphereVolume = (ELEMTYPEREAL)UNIT_SPHERE_VOLUMES[NUMDIMS]; @@ -383,72 +388,67 @@ RTREE_QUAL::~RTree() RTREE_TEMPLATE -void RTREE_QUAL::Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId) +void RTREE_QUAL::Insert(const ELEMTYPE a_min[NUMDIMS], + const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId) { #ifdef _DEBUG - for(int index=0; indexIsInternalNode()) // not a leaf node - { - for(int index = 0; index < a_node->m_count; ++index) - { + if (a_node->IsInternalNode()) { // not a leaf node + for (int index = 0; index < a_node->m_count; ++index) CountRec(a_node->m_branch[index].m_child, a_count); - } - } - else // A leaf node - { + } else { // A leaf node a_count += a_node->m_count; } } @@ -485,7 +480,7 @@ void RTREE_QUAL::RemoveAll() { // Delete all existing nodes Reset(); - + m_root = AllocNode(); m_root->m_level = 0; } @@ -509,13 +504,10 @@ void RTREE_QUAL::RemoveAllRec(Node* a_node) { ASSERT(a_node); ASSERT(a_node->m_level >= 0); - - if(a_node->IsInternalNode()) // This is an internal node in the tree - { - for(int index=0; index < a_node->m_count; ++index) - { + + if (a_node->IsInternalNode()) { // This is an internal node in the tree + for (int index=0; index < a_node->m_count; ++index) RemoveAllRec(a_node->m_branch[index].m_child); - } } FreeNode(a_node); } @@ -539,7 +531,7 @@ RTREE_TEMPLATE void RTREE_QUAL::FreeNode(Node* a_node) { ASSERT(a_node); - + #ifdef RTREE_DONT_USE_MEMPOOLS delete a_node; #else // RTREE_DONT_USE_MEMPOOLS @@ -583,8 +575,7 @@ void RTREE_QUAL::InitNode(Node* a_node) RTREE_TEMPLATE void RTREE_QUAL::InitRect(Rect* a_rect) { - for(int index = 0; index < NUMDIMS; ++index) - { + for (int index = 0; index < NUMDIMS; ++index) { a_rect->m_min[index] = (ELEMTYPE)0; a_rect->m_max[index] = (ELEMTYPE)0; } @@ -599,42 +590,36 @@ void RTREE_QUAL::InitRect(Rect* a_rect) // The level argument specifies the number of steps up from the leaf // level to insert; e.g. a data rectangle goes in at level = 0. RTREE_TEMPLATE -bool RTREE_QUAL::InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, Node** a_newNode, int a_level) +bool RTREE_QUAL::InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, + Node** a_newNode, int a_level) { ASSERT(a_rect && a_node && a_newNode); ASSERT(a_level >= 0 && a_level <= a_node->m_level); - + int index; Branch branch; Node* otherNode; - + // Still above level for insertion, go down tree recursively - if(a_node->m_level > a_level) - { + if (a_node->m_level > a_level) { index = PickBranch(a_rect, a_node); - if (!InsertRectRec(a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level)) - { + if (!InsertRectRec(a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level)) { // Child was not split a_node->m_branch[index].m_rect = CombineRect(a_rect, &(a_node->m_branch[index].m_rect)); return false; - } - else // Child was split - { + } else { // Child was split a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child); branch.m_child = otherNode; branch.m_rect = NodeCover(otherNode); return AddBranch(&branch, a_node, a_newNode); } - } - else if(a_node->m_level == a_level) // Have reached level for insertion. Add rect, split if necessary - { + // Have reached level for insertion. Add rect, split if necessary + } else if (a_node->m_level == a_level) { branch.m_rect = *a_rect; branch.m_child = (Node*) a_id; // Child field of leaves contains id of data record return AddBranch(&branch, a_node, a_newNode); - } - else - { + } else { // Should never occur ASSERT(0); return false; @@ -650,23 +635,22 @@ bool RTREE_QUAL::InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, // InsertRect2 does the recursion. // RTREE_TEMPLATE -bool RTREE_QUAL::InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level) +bool RTREE_QUAL::InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, + int a_level) { ASSERT(a_rect && a_root); ASSERT(a_level >= 0 && a_level <= (*a_root)->m_level); #ifdef _DEBUG - for(int index=0; index < NUMDIMS; ++index) - { + for (int index=0; index < NUMDIMS; ++index) ASSERT(a_rect->m_min[index] <= a_rect->m_max[index]); - } #endif //_DEBUG - + Node* newRoot; Node* newNode; Branch branch; - - if(InsertRectRec(a_rect, a_id, *a_root, &newNode, a_level)) // Root split - { + + // Root split + if (InsertRectRec(a_rect, a_id, *a_root, &newNode, a_level)) { newRoot = AllocNode(); // Grow tree taller and new root newRoot->m_level = (*a_root)->m_level + 1; branch.m_rect = NodeCover(*a_root); @@ -678,34 +662,31 @@ bool RTREE_QUAL::InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, i *a_root = newRoot; return true; } - + return false; } -// Find the smallest rectangle that includes all rectangles in branches of a node. +// Find the smallest rectangle that includes all rectangles in branches of +// a node. RTREE_TEMPLATE typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover(Node* a_node) { ASSERT(a_node); - + int firstTime = true; Rect rect; InitRect(&rect); - - for(int index = 0; index < a_node->m_count; ++index) - { - if(firstTime) - { + + for (int index = 0; index < a_node->m_count; ++index) { + if (firstTime) { rect = a_node->m_branch[index].m_rect; firstTime = false; - } - else - { + } else { rect = CombineRect(&rect, &(a_node->m_branch[index].m_rect)); } } - + return rect; } @@ -719,18 +700,15 @@ bool RTREE_QUAL::AddBranch(Branch* a_branch, Node* a_node, Node** a_newNode) { ASSERT(a_branch); ASSERT(a_node); - - if(a_node->m_count < MAXNODES) // Split won't be necessary - { + + if (a_node->m_count < MAXNODES) { // Split won't be necessary a_node->m_branch[a_node->m_count] = *a_branch; ++a_node->m_count; - + return false; - } - else - { + } else { ASSERT(a_newNode); - + SplitNode(a_node, a_branch, a_newNode); return true; } @@ -738,16 +716,17 @@ bool RTREE_QUAL::AddBranch(Branch* a_branch, Node* a_node, Node** a_newNode) // Disconnect a dependent node. -// Caller must return (or stop using iteration index) after this as count has changed +// Caller must return (or stop using iteration index) after this as count has +// changed RTREE_TEMPLATE void RTREE_QUAL::DisconnectBranch(Node* a_node, int a_index) { ASSERT(a_node && (a_index >= 0) && (a_index < MAXNODES)); ASSERT(a_node->m_count > 0); - + // Remove element by swapping with the last element to prevent gaps in array a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1]; - + --a_node->m_count; } @@ -761,30 +740,27 @@ RTREE_TEMPLATE int RTREE_QUAL::PickBranch(Rect* a_rect, Node* a_node) { ASSERT(a_rect && a_node); - + bool firstTime = true; ELEMTYPEREAL increase; ELEMTYPEREAL bestIncr = (ELEMTYPEREAL)-1; ELEMTYPEREAL area; ELEMTYPEREAL bestArea; - int best; + int best = 0; Rect tempRect; - - for(int index=0; index < a_node->m_count; ++index) - { + + for (int index=0; index < a_node->m_count; ++index) { Rect* curRect = &a_node->m_branch[index].m_rect; area = CalcRectVolume(curRect); tempRect = CombineRect(a_rect, curRect); increase = CalcRectVolume(&tempRect) - area; - if((increase < bestIncr) || firstTime) - { + + if ((increase < bestIncr) || firstTime) { best = index; bestArea = area; bestIncr = increase; firstTime = false; - } - else if((increase == bestIncr) && (area < bestArea)) - { + } else if ((increase == bestIncr) && (area < bestArea)) { best = index; bestArea = area; bestIncr = increase; @@ -799,15 +775,14 @@ RTREE_TEMPLATE typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect(Rect* a_rectA, Rect* a_rectB) { ASSERT(a_rectA && a_rectB); - + Rect newRect; - - for(int index = 0; index < NUMDIMS; ++index) - { + + for (int index = 0; index < NUMDIMS; ++index) { newRect.m_min[index] = Min(a_rectA->m_min[index], a_rectB->m_min[index]); newRect.m_max[index] = Max(a_rectA->m_max[index], a_rectB->m_max[index]); } - + return newRect; } @@ -822,24 +797,25 @@ void RTREE_QUAL::SplitNode(Node* a_node, Branch* a_branch, Node** a_newNode) { ASSERT(a_node); ASSERT(a_branch); - - // Could just use local here, but member or external is faster since it is reused + + // Could just use local here, but member or external is faster since it is + // reused PartitionVars localVars; PartitionVars* parVars = &localVars; int level; - + // Load all the branches into a buffer, initialize old node level = a_node->m_level; GetBranches(a_node, a_branch, parVars); - + // Find partition ChoosePartition(parVars, MINNODES); - + // Put branches from buffer into 2 nodes according to chosen partition *a_newNode = AllocNode(); (*a_newNode)->m_level = a_node->m_level = level; LoadNodes(a_node, *a_newNode, parVars); - + ASSERT((a_node->m_count + (*a_newNode)->m_count) == parVars->m_total); } @@ -849,16 +825,14 @@ RTREE_TEMPLATE ELEMTYPEREAL RTREE_QUAL::RectVolume(Rect* a_rect) { ASSERT(a_rect); - + ELEMTYPEREAL volume = (ELEMTYPEREAL)1; - - for(int index=0; indexm_max[index] - a_rect->m_min[index]; - } - + ASSERT(volume >= (ELEMTYPEREAL)0); - + return volume; } @@ -868,31 +842,25 @@ RTREE_TEMPLATE ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume(Rect* a_rect) { ASSERT(a_rect); - + ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL)0; ELEMTYPEREAL radius; - - for(int index=0; index < NUMDIMS; ++index) - { - ELEMTYPEREAL halfExtent = ((ELEMTYPEREAL)a_rect->m_max[index] - (ELEMTYPEREAL)a_rect->m_min[index]) * 0.5f; + + for (int index=0; index < NUMDIMS; ++index) { + ELEMTYPEREAL halfExtent = ((ELEMTYPEREAL)a_rect->m_max[index] + - (ELEMTYPEREAL)a_rect->m_min[index]) * 0.5f; sumOfSquares += halfExtent * halfExtent; } - + radius = (ELEMTYPEREAL)sqrt(sumOfSquares); - + // Pow maybe slow, so test for common dims like 2,3 and just use x*x, x*x*x. - if(NUMDIMS == 3) - { + if (NUMDIMS == 3) return (radius * radius * radius * m_unitSphereVolume); - } - else if(NUMDIMS == 2) - { + else if (NUMDIMS == 2) return (radius * radius * m_unitSphereVolume); - } else - { return (ELEMTYPEREAL)(pow(radius, NUMDIMS) * m_unitSphereVolume); - } } @@ -910,29 +878,27 @@ ELEMTYPEREAL RTREE_QUAL::CalcRectVolume(Rect* a_rect) // Load branch buffer with branches from full node plus the extra branch. RTREE_TEMPLATE -void RTREE_QUAL::GetBranches(Node* a_node, Branch* a_branch, PartitionVars* a_parVars) +void RTREE_QUAL::GetBranches(Node* a_node, Branch* a_branch, + PartitionVars* a_parVars) { ASSERT(a_node); ASSERT(a_branch); - + ASSERT(a_node->m_count == MAXNODES); - + // Load the branch buffer - for(int index=0; index < MAXNODES; ++index) - { + for (int index=0; index < MAXNODES; ++index) a_parVars->m_branchBuf[index] = a_node->m_branch[index]; - } a_parVars->m_branchBuf[MAXNODES] = *a_branch; a_parVars->m_branchCount = MAXNODES + 1; - + // Calculate rect containing all in the set a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect; - for(int index=1; index < MAXNODES+1; ++index) - { - a_parVars->m_coverSplit = CombineRect(&a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect); - } + for (int index=1; index < MAXNODES+1; ++index) + a_parVars->m_coverSplit = CombineRect(&a_parVars->m_coverSplit, + &a_parVars->m_branchBuf[index].m_rect); a_parVars->m_coverSplitArea = CalcRectVolume(&a_parVars->m_coverSplit); - + InitNode(a_node); } @@ -952,46 +918,40 @@ RTREE_TEMPLATE void RTREE_QUAL::ChoosePartition(PartitionVars* a_parVars, int a_minFill) { ASSERT(a_parVars); - + ELEMTYPEREAL biggestDiff; - int group, chosen, betterGroup; - + int group, chosen = 0, betterGroup = 0; + InitParVars(a_parVars, a_parVars->m_branchCount, a_minFill); PickSeeds(a_parVars); - + while (((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) && (a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill)) - && (a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill))) - { + && (a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill))) { biggestDiff = (ELEMTYPEREAL) -1; - for(int index=0; indexm_total; ++index) - { - if(!a_parVars->m_taken[index]) - { + + for (int index=0; indexm_total; ++index) { + if (!a_parVars->m_taken[index]) { Rect* curRect = &a_parVars->m_branchBuf[index].m_rect; Rect rect0 = CombineRect(curRect, &a_parVars->m_cover[0]); Rect rect1 = CombineRect(curRect, &a_parVars->m_cover[1]); ELEMTYPEREAL growth0 = CalcRectVolume(&rect0) - a_parVars->m_area[0]; ELEMTYPEREAL growth1 = CalcRectVolume(&rect1) - a_parVars->m_area[1]; ELEMTYPEREAL diff = growth1 - growth0; - if(diff >= 0) - { + + if (diff >= 0) { group = 0; - } - else - { + } else { group = 1; diff = -diff; } - - if(diff > biggestDiff) - { + + if (diff > biggestDiff) { biggestDiff = diff; chosen = index; betterGroup = group; - } - else if((diff == biggestDiff) && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup])) - { + } else if ((diff == biggestDiff) && (a_parVars->m_count[group] + < a_parVars->m_count[betterGroup])) { chosen = index; betterGroup = group; } @@ -999,27 +959,20 @@ void RTREE_QUAL::ChoosePartition(PartitionVars* a_parVars, int a_minFill) } Classify(chosen, betterGroup, a_parVars); } - + // If one group too full, put remaining rects in the other - if((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) - { - if(a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill) - { + if ((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) { + if (a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill) group = 1; - } else - { group = 0; - } - for(int index=0; indexm_total; ++index) - { - if(!a_parVars->m_taken[index]) - { + + for (int index=0; indexm_total; ++index) { + if (!a_parVars->m_taken[index]) Classify(index, group, a_parVars); - } } } - + ASSERT((a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total); ASSERT((a_parVars->m_count[0] >= a_parVars->m_minFill) && (a_parVars->m_count[1] >= a_parVars->m_minFill)); @@ -1033,35 +986,31 @@ void RTREE_QUAL::LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVar ASSERT(a_nodeA); ASSERT(a_nodeB); ASSERT(a_parVars); - - for(int index=0; index < a_parVars->m_total; ++index) - { + + for (int index=0; index < a_parVars->m_total; ++index) { ASSERT(a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1); - - if(a_parVars->m_partition[index] == 0) - { + + if (a_parVars->m_partition[index] == 0) AddBranch(&a_parVars->m_branchBuf[index], a_nodeA, NULL); - } - else if(a_parVars->m_partition[index] == 1) - { + else if (a_parVars->m_partition[index] == 1) AddBranch(&a_parVars->m_branchBuf[index], a_nodeB, NULL); - } } } // Initialize a PartitionVars structure. RTREE_TEMPLATE -void RTREE_QUAL::InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill) +void RTREE_QUAL::InitParVars(PartitionVars* a_parVars, int a_maxRects, + int a_minFill) { ASSERT(a_parVars); - + a_parVars->m_count[0] = a_parVars->m_count[1] = 0; a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL)0; a_parVars->m_total = a_maxRects; a_parVars->m_minFill = a_minFill; - for(int index=0; index < a_maxRects; ++index) - { + + for (int index=0; index < a_maxRects; ++index) { a_parVars->m_taken[index] = false; a_parVars->m_partition[index] = -1; } @@ -1071,24 +1020,20 @@ void RTREE_QUAL::InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_min RTREE_TEMPLATE void RTREE_QUAL::PickSeeds(PartitionVars* a_parVars) { - int seed0, seed1; + int seed0 = 0, seed1 = 0; ELEMTYPEREAL worst, waste; ELEMTYPEREAL area[MAXNODES+1]; - - for(int index=0; indexm_total; ++index) - { + + for (int index=0; indexm_total; ++index) area[index] = CalcRectVolume(&a_parVars->m_branchBuf[index].m_rect); - } - + worst = -a_parVars->m_coverSplitArea - 1; - for(int indexA=0; indexA < a_parVars->m_total-1; ++indexA) - { - for(int indexB = indexA+1; indexB < a_parVars->m_total; ++indexB) - { - Rect oneRect = CombineRect(&a_parVars->m_branchBuf[indexA].m_rect, &a_parVars->m_branchBuf[indexB].m_rect); + for (int indexA=0; indexA < a_parVars->m_total-1; ++indexA) { + for (int indexB = indexA+1; indexB < a_parVars->m_total; ++indexB) { + Rect oneRect = CombineRect(&a_parVars->m_branchBuf[indexA].m_rect, + &a_parVars->m_branchBuf[indexB].m_rect); waste = CalcRectVolume(&oneRect) - area[indexA] - area[indexB]; - if(waste > worst) - { + if (waste > worst) { worst = waste; seed0 = indexA; seed1 = indexB; @@ -1106,18 +1051,16 @@ void RTREE_QUAL::Classify(int a_index, int a_group, PartitionVars* a_parVars) { ASSERT(a_parVars); ASSERT(!a_parVars->m_taken[a_index]); - + a_parVars->m_partition[a_index] = a_group; a_parVars->m_taken[a_index] = true; - + if (a_parVars->m_count[a_group] == 0) - { a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect; - } else - { - a_parVars->m_cover[a_group] = CombineRect(&a_parVars->m_branchBuf[a_index].m_rect, &a_parVars->m_cover[a_group]); - } + a_parVars->m_cover[a_group] = CombineRect( + &a_parVars->m_branchBuf[a_index].m_rect, &a_parVars->m_cover[a_group]); + a_parVars->m_area[a_group] = CalcRectVolume(&a_parVars->m_cover[a_group]); ++a_parVars->m_count[a_group]; } @@ -1132,46 +1075,37 @@ bool RTREE_QUAL::RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root) { ASSERT(a_rect && a_root); ASSERT(*a_root); - + Node* tempNode; ListNode* reInsertList = NULL; - - if(!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList)) - { + + if (!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList)) { // Found and deleted a data item // Reinsert any branches from eliminated nodes - while(reInsertList) - { + while (reInsertList) { tempNode = reInsertList->m_node; - - for(int index = 0; index < tempNode->m_count; ++index) - { + + for (int index = 0; index < tempNode->m_count; ++index) InsertRect(&(tempNode->m_branch[index].m_rect), - tempNode->m_branch[index].m_data, - a_root, - tempNode->m_level); - } - + tempNode->m_branch[index].m_data, a_root, tempNode->m_level); + ListNode* remLNode = reInsertList; reInsertList = reInsertList->m_next; - + FreeNode(remLNode->m_node); FreeListNode(remLNode); } - + // Check for redundant root (not leaf, 1 child) and eliminate - if((*a_root)->m_count == 1 && (*a_root)->IsInternalNode()) - { + if ((*a_root)->m_count == 1 && (*a_root)->IsInternalNode()) { tempNode = (*a_root)->m_branch[0].m_child; - + ASSERT(tempNode); FreeNode(*a_root); *a_root = tempNode; } return false; - } - else - { + } else { return true; } } @@ -1182,43 +1116,38 @@ bool RTREE_QUAL::RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root) // merges branches on the way back up. // Returns 1 if record not found, 0 if success. RTREE_TEMPLATE -bool RTREE_QUAL::RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode) +bool RTREE_QUAL::RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, + ListNode** a_listNode) { ASSERT(a_rect && a_node && a_listNode); ASSERT(a_node->m_level >= 0); - - if(a_node->IsInternalNode()) // not a leaf node - { - for(int index = 0; index < a_node->m_count; ++index) - { - if(Overlap(a_rect, &(a_node->m_branch[index].m_rect))) - { - if(!RemoveRectRec(a_rect, a_id, a_node->m_branch[index].m_child, a_listNode)) - { - if(a_node->m_branch[index].m_child->m_count >= MINNODES) - { + + if (a_node->IsInternalNode()) { // not a leaf node + for (int index = 0; index < a_node->m_count; ++index) { + if (Overlap(a_rect, &(a_node->m_branch[index].m_rect))) { + if (!RemoveRectRec(a_rect, a_id, a_node->m_branch[index].m_child, a_listNode)) { + if (a_node->m_branch[index].m_child->m_count >= MINNODES) { // child removed, just resize parent rect a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child); - } - else - { - // child removed, not enough entries in node, eliminate node + } else { + // child removed, not enough entries in node, eliminate + // node ReInsert(a_node->m_branch[index].m_child, a_listNode); - DisconnectBranch(a_node, index); // Must return after this call as count has changed + DisconnectBranch(a_node, index); + // Must return after this call as count has changed } return false; } } } return true; - } - else // A leaf node - { - for(int index = 0; index < a_node->m_count; ++index) + } else { // A leaf node + for (int index = 0; index < a_node->m_count; ++index) { - if(a_node->m_branch[index].m_child == (Node*)a_id) + if (a_node->m_branch[index].m_child == (Node*)a_id) { - DisconnectBranch(a_node, index); // Must return after this call as count has changed + DisconnectBranch(a_node, index); + // Must return after this call as count has changed return false; } } @@ -1232,12 +1161,10 @@ RTREE_TEMPLATE bool RTREE_QUAL::Overlap(Rect* a_rectA, Rect* a_rectB) const { ASSERT(a_rectA && a_rectB); - - for(int index=0; index < NUMDIMS; ++index) - { + + for (int index=0; index < NUMDIMS; ++index) { if (a_rectA->m_min[index] > a_rectB->m_max[index] || - a_rectB->m_min[index] > a_rectA->m_max[index]) - { + a_rectB->m_min[index] > a_rectA->m_max[index]) { return false; } } @@ -1251,7 +1178,7 @@ RTREE_TEMPLATE void RTREE_QUAL::ReInsert(Node* a_node, ListNode** a_listNode) { ListNode* newListNode; - + newListNode = AllocListNode(); newListNode->m_node = a_node; newListNode->m_next = *a_listNode; @@ -1259,48 +1186,42 @@ void RTREE_QUAL::ReInsert(Node* a_node, ListNode** a_listNode) } -// Search in an index tree or subtree for all data retangles that overlap the argument rectangle. +// Search in an index tree or subtree for all data retangles that overlap +// the argument rectangle. RTREE_TEMPLATE -bool RTREE_QUAL::Search(Node* a_node, Rect* a_rect, int& a_foundCount, bool (*a_resultCallback)(DATATYPE a_data, void* a_context), void* a_context) const +bool RTREE_QUAL::Search(Node* a_node, Rect* a_rect, int& a_foundCount, + bool (*a_resultCallback)(DATATYPE a_data, void* a_context), + void* a_context) const { ASSERT(a_node); ASSERT(a_node->m_level >= 0); ASSERT(a_rect); - - if(a_node->IsInternalNode()) // This is an internal node in the tree - { - for(int index=0; index < a_node->m_count; ++index) - { - if(Overlap(a_rect, &a_node->m_branch[index].m_rect)) - { - if(!Search(a_node->m_branch[index].m_child, a_rect, a_foundCount, a_resultCallback, a_context)) - { + + if (a_node->IsInternalNode()) { // This is an internal node in the tree + for (int index=0; index < a_node->m_count; ++index) { + if (Overlap(a_rect, &a_node->m_branch[index].m_rect)) { + if (!Search(a_node->m_branch[index].m_child, a_rect, + a_foundCount, a_resultCallback, a_context)) { return false; // Don't continue searching } } } - } - else // This is a leaf node - { - for(int index=0; index < a_node->m_count; ++index) - { - if(Overlap(a_rect, &a_node->m_branch[index].m_rect)) - { + } else { // This is a leaf node + for (int index=0; index < a_node->m_count; ++index) { + if (Overlap(a_rect, &a_node->m_branch[index].m_rect)) { DATATYPE& id = a_node->m_branch[index].m_data; - - // NOTE: There are different ways to return results. Here's where to modify - if(a_resultCallback) - { + + // NOTE: There are different ways to return results. + // Here's where to modify + if (a_resultCallback) { ++a_foundCount; - if(!a_resultCallback(id, a_context)) - { + if (!a_resultCallback(id, a_context)) return false; // Don't continue searching - } } } } } - + return true; // Continue searching }