RTree.h

Go to the documentation of this file.

// SPDX-License-Identifier: LGPL-2.1-or-later OR GPL-2.0-or-later OR GPL-3.0-or-later OR LicenseRef-ACF-Commercial
#ifndef RTREE_H
#define RTREE_H
 
// NOTE This file compiles under MSVC 6 SP5 and MSVC .Net 2003 it may not work on other compilers without modification.
 
// NOTE These next few lines may be win32 specific, you may need to modify them to compile on other platform
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <stdlib.h>
 
#include <algorithm>
#include <functional>
 
//
// RTree.h
//
 
#define RTREE_TEMPLATE template<class DATATYPE, class ELEMTYPE, int NUMDIMS, class ELEMTYPEREAL, int TMAXNODES, int TMINNODES>
#define RTREE_QUAL RTree<DATATYPE, ELEMTYPE, NUMDIMS, ELEMTYPEREAL, TMAXNODES, TMINNODES>
 
#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.
 
 
template<class DATATYPE, class ELEMTYPE, int NUMDIMS,
       class ELEMTYPEREAL = ELEMTYPE, int TMAXNODES = 8, int TMINNODES = TMAXNODES / 2>
       class RTree
{
       static_assert(std::numeric_limits<ELEMTYPEREAL>::is_iec559, "'ELEMTYPEREAL' accepts floating-point types only");
 
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,                         
              MINNODES = TMINNODES,                         
       };
 
public:
 
       RTree();
       RTree(const RTree& other);
       virtual ~RTree();
 
       void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId);
 
       void Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId);
 
    template<typename Func>
       int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], Func&& callback) const;
 
       void RemoveAll();
 
       int Count();
 
       bool Load(const char* a_fileName);
       bool Load(RTFileStream& a_stream);
 
 
       bool Save(const char* a_fileName);
       bool Save(RTFileStream& a_stream);
 
       class Iterator
       {
       private:
 
              enum { MAX_STACK = 32 }; //  Max stack size. Allows almost n^32 where n is number of branches in node
 
              struct StackElement
              {
                     Node* m_node;
                     int m_branchIndex;
              };
 
       public:
 
              Iterator() { Init(); }
 
              ~Iterator() { }
 
              bool IsNull() { return (m_tos <= 0); }
 
              bool IsNotNull() { return (m_tos > 0); }
 
              DATATYPE& operator*()
              {
                     assert(IsNotNull());
                     StackElement& curTos = m_stack[m_tos - 1];
                     return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
              }
 
              const DATATYPE& operator*() const
              {
                     assert(IsNotNull());
                     StackElement& curTos = m_stack[m_tos - 1];
                     return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
              }
 
              bool operator++() { return FindNextData(); }
 
              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)
                     {
                            a_min[index] = curBranch.m_rect.m_min[index];
                            a_max[index] = curBranch.m_rect.m_max[index];
                     }
              }
 
       private:
 
              void Init() { m_tos = 0; }
 
              bool FindNextData()
              {
                     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())
                            {
                                   // Keep walking through data while we can
                                   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)
                                   {
                                          // Push sibling on for future tree walk
                                          // 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
                                   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())
                                   {
                                          return true;
                                   }
                            }
                     }
              }
 
              void Push(Node* a_node, int a_branchIndex)
              {
                     m_stack[m_tos].m_node = a_node;
                     m_stack[m_tos].m_branchIndex = a_branchIndex;
                     ++m_tos;
                     assert(m_tos <= MAX_STACK);
              }
 
              StackElement& Pop()
              {
                     assert(m_tos > 0);
                     --m_tos;
                     return m_stack[m_tos];
              }
 
              StackElement m_stack[MAX_STACK];              
              int m_tos;                                    
 
              friend class RTree; // Allow hiding of non-public functions while allowing manipulation by logical owner
       };
 
       void GetFirst(Iterator& a_it)
       {
              a_it.Init();
              Node* first = m_root;
              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)
                            {
                                   a_it.Push(first, 0);
                            }
                            break;
                     }
                     first = first->m_branch[0].m_child;
              }
       }
 
       void GetNext(Iterator& a_it) { ++a_it; }
 
       bool IsNull(Iterator& a_it) { return a_it.IsNull(); }
 
       DATATYPE& GetAt(Iterator& a_it) { return *a_it; }
 
protected:
 
       struct Rect
       {
              ELEMTYPE m_min[NUMDIMS];                      
              ELEMTYPE m_max[NUMDIMS];                      
       };
 
       struct Branch
       {
              Rect m_rect;                                  
              Node* m_child;                                
              DATATYPE m_data;                              
       };
 
       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;                                  
              int m_level;                                  
              Branch m_branch[MAXNODES];                    
       };
 
       struct ListNode
       {
              ListNode* m_next;                             
              Node* m_node;                                 
       };
 
       struct PartitionVars
       {
              enum { NOT_TAKEN = -1 }; // indicates that position
 
              int m_partition[MAXNODES + 1];
              int m_total;
              int m_minFill;
              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(const Branch& a_branch, Node* a_node, Node** a_newNode, int a_level);
       bool InsertRect(const Branch& a_branch, Node** a_root, int a_level);
       Rect NodeCover(Node* a_node);
       bool AddBranch(const Branch* a_branch, Node* a_node, Node** a_newNode);
       void DisconnectBranch(Node* a_node, int a_index);
       int PickBranch(const Rect* a_rect, Node* a_node);
       Rect CombineRect(const Rect* a_rectA, const Rect* a_rectB);
       void SplitNode(Node* a_node, const Branch* a_branch, Node** a_newNode);
       ELEMTYPEREAL RectSphericalVolume(Rect* a_rect);
       ELEMTYPEREAL RectVolume(Rect* a_rect);
       ELEMTYPEREAL CalcRectVolume(Rect* a_rect);
       void GetBranches(Node* a_node, const Branch* a_branch, PartitionVars* a_parVars);
       void ChoosePartition(PartitionVars* a_parVars, int a_minFill);
       void LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars);
       void InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill);
       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);
       ListNode* AllocListNode();
       void FreeListNode(ListNode* a_listNode);
       bool Overlap(Rect* a_rectA, Rect* a_rectB) const;
       void ReInsert(Node* a_node, ListNode** a_listNode);
 
    template<typename Func>
       bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, Func&& callback) const;
 
       void RemoveAllRec(Node* a_node);
       void Reset();
       void CountRec(Node* a_node, int& a_count);
 
       bool SaveRec(Node* a_node, RTFileStream& a_stream);
       bool LoadRec(Node* a_node, RTFileStream& a_stream);
       void CopyRec(Node* current, Node* other);
 
       Node* m_root;                                    
       ELEMTYPEREAL m_unitSphereVolume;                 
};
 
 
// Because there is not stream support, this is a quick and dirty file I/O helper.
// Users will likely replace its usage with a Stream implementation from their favorite API.
class RTFileStream
{
       FILE* m_file;
 
public:
 
 
       RTFileStream()
       {
              m_file = NULL;
       }
 
       ~RTFileStream()
       {
              Close();
       }
 
       bool Open(const char* a_fileName, const char* mode)
       {
#if defined(_WIN32) && defined(__STDC_WANT_SECURE_LIB__)
              return fopen_s(&m_file, a_fileName, mode) == 0;
#else
              m_file = fopen(a_fileName, mode);
              return m_file != nullptr;
#endif
       }
 
       bool OpenRead(const char* a_fileName)
       {
              return this->Open(a_fileName, "rb");
       }
 
       bool OpenWrite(const char* a_fileName)
       {
              return this->Open(a_fileName, "wb");
       }
 
       void Close()
       {
              if (m_file)
              {
                     fclose(m_file);
                     m_file = NULL;
              }
       }
 
       template< typename TYPE >
       size_t Write(const TYPE& a_value)
       {
              assert(m_file);
              return fwrite((void*)&a_value, sizeof(a_value), 1, m_file);
       }
 
       template< typename TYPE >
       size_t WriteArray(const TYPE* a_array, int a_count)
       {
              assert(m_file);
              return fwrite((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
       }
 
       template< typename TYPE >
       size_t Read(TYPE& a_value)
       {
              assert(m_file);
              return fread((void*)&a_value, sizeof(a_value), 1, m_file);
       }
 
       template< typename TYPE >
       size_t ReadArray(TYPE* a_array, int a_count)
       {
              assert(m_file);
              return fread((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
       }
};
 
 
RTREE_TEMPLATE
RTREE_QUAL::RTree()
{
       assert(MAXNODES > MINNODES);
       assert(MINNODES > 0);
 
       // 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
         4.188790f, 4.934802f, 5.263789f, // Dimension  3,4,5
         5.167713f, 4.724766f, 4.058712f, // Dimension  6,7,8
         3.298509f, 2.550164f, 1.884104f, // Dimension  9,10,11
         1.335263f, 0.910629f, 0.599265f, // Dimension  12,13,14
         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];
}
 
 
RTREE_TEMPLATE
RTREE_QUAL::RTree(const RTree& other) : RTree()
{
       CopyRec(m_root, other.m_root);
}
 
 
RTREE_TEMPLATE
RTREE_QUAL::~RTree()
{
       Reset(); // Free, or reset node memory
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId)
{
#ifdef _DEBUG
       for (int index = 0; index < NUMDIMS; ++index)
       {
              assert(a_min[index] <= a_max[index]);
       }
#endif //_DEBUG
 
       Branch branch;
       branch.m_data = a_dataId;
       branch.m_child = NULL;
 
       for (int axis = 0; axis < NUMDIMS; ++axis)
       {
              branch.m_rect.m_min[axis] = a_min[axis];
              branch.m_rect.m_max[axis] = a_max[axis];
       }
 
       InsertRect(branch, &m_root, 0);
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId)
{
#ifdef _DEBUG
       for (int index = 0; index < NUMDIMS; ++index)
       {
              assert(a_min[index] <= a_max[index]);
       }
#endif //_DEBUG
 
       Rect rect;
 
       for (int axis = 0; axis < NUMDIMS; ++axis)
       {
              rect.m_min[axis] = a_min[axis];
              rect.m_max[axis] = a_max[axis];
       }
 
       RemoveRect(&rect, a_dataId, &m_root);
}
 
 
RTREE_TEMPLATE
template<typename Func>
int RTREE_QUAL::Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], Func&& callback) const
{
#ifdef _DEBUG
       for (int index = 0; index < NUMDIMS; ++index)
       {
              assert(a_min[index] <= a_max[index]);
       }
#endif //_DEBUG
 
       Rect rect;
 
       for (int axis = 0; axis < NUMDIMS; ++axis)
       {
              rect.m_min[axis] = a_min[axis];
              rect.m_max[axis] = a_max[axis];
       }
 
       // NOTE: May want to return search result another way, perhaps returning the number of found elements here.
 
       int foundCount = 0;
       Search(m_root, &rect, foundCount, std::forward<Func>(callback));
 
       return foundCount;
}
 
 
RTREE_TEMPLATE
int RTREE_QUAL::Count()
{
       int count = 0;
       CountRec(m_root, count);
 
       return count;
}
 
 
 
RTREE_TEMPLATE
void RTREE_QUAL::CountRec(Node* a_node, int& a_count)
{
       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
       {
              a_count += a_node->m_count;
       }
}
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::Load(const char* a_fileName)
{
       RemoveAll(); // Clear existing tree
 
       RTFileStream stream;
       if (!stream.OpenRead(a_fileName))
       {
              return false;
       }
 
       bool result = Load(stream);
 
       stream.Close();
 
       return result;
}
 
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::Load(RTFileStream& a_stream)
{
       // Write some kind of header
       int _dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
       int _dataSize = sizeof(DATATYPE);
       int _dataNumDims = NUMDIMS;
       int _dataElemSize = sizeof(ELEMTYPE);
       int _dataElemRealSize = sizeof(ELEMTYPEREAL);
       int _dataMaxNodes = TMAXNODES;
       int _dataMinNodes = TMINNODES;
 
       int dataFileId = 0;
       int dataSize = 0;
       int dataNumDims = 0;
       int dataElemSize = 0;
       int dataElemRealSize = 0;
       int dataMaxNodes = 0;
       int dataMinNodes = 0;
 
       a_stream.Read(dataFileId);
       a_stream.Read(dataSize);
       a_stream.Read(dataNumDims);
       a_stream.Read(dataElemSize);
       a_stream.Read(dataElemRealSize);
       a_stream.Read(dataMaxNodes);
       a_stream.Read(dataMinNodes);
 
       bool result = false;
 
       // Test if header was valid and compatible
       if ((dataFileId == _dataFileId)
              && (dataSize == _dataSize)
              && (dataNumDims == _dataNumDims)
              && (dataElemSize == _dataElemSize)
              && (dataElemRealSize == _dataElemRealSize)
              && (dataMaxNodes == _dataMaxNodes)
              && (dataMinNodes == _dataMinNodes)
              )
       {
              // Recursively load tree
              result = LoadRec(m_root, a_stream);
       }
 
       return result;
}
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::LoadRec(Node* a_node, RTFileStream& a_stream)
{
       a_stream.Read(a_node->m_level);
       a_stream.Read(a_node->m_count);
 
       if (a_node->IsInternalNode())  // not a leaf node
       {
              for (int index = 0; index < a_node->m_count; ++index)
              {
                     Branch* curBranch = &a_node->m_branch[index];
 
                     a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS);
                     a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS);
 
                     curBranch->m_child = AllocNode();
                     LoadRec(curBranch->m_child, a_stream);
              }
       }
       else // A leaf node
       {
              for (int index = 0; index < a_node->m_count; ++index)
              {
                     Branch* curBranch = &a_node->m_branch[index];
 
                     a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS);
                     a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS);
 
                     a_stream.Read(curBranch->m_data);
              }
       }
 
       return true; // Should do more error checking on I/O operations
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::CopyRec(Node* current, Node* other)
{
       current->m_level = other->m_level;
       current->m_count = other->m_count;
 
       if (current->IsInternalNode())  // not a leaf node
       {
              for (int index = 0; index < current->m_count; ++index)
              {
                     Branch* currentBranch = &current->m_branch[index];
                     Branch* otherBranch = &other->m_branch[index];
 
                     std::copy(otherBranch->m_rect.m_min,
                            otherBranch->m_rect.m_min + NUMDIMS,
                            currentBranch->m_rect.m_min);
 
                     std::copy(otherBranch->m_rect.m_max,
                            otherBranch->m_rect.m_max + NUMDIMS,
                            currentBranch->m_rect.m_max);
 
                     currentBranch->m_child = AllocNode();
                     CopyRec(currentBranch->m_child, otherBranch->m_child);
              }
       }
       else // A leaf node
       {
              for (int index = 0; index < current->m_count; ++index)
              {
                     Branch* currentBranch = &current->m_branch[index];
                     Branch* otherBranch = &other->m_branch[index];
 
                     std::copy(otherBranch->m_rect.m_min,
                            otherBranch->m_rect.m_min + NUMDIMS,
                            currentBranch->m_rect.m_min);
 
                     std::copy(otherBranch->m_rect.m_max,
                            otherBranch->m_rect.m_max + NUMDIMS,
                            currentBranch->m_rect.m_max);
 
                     currentBranch->m_data = otherBranch->m_data;
              }
       }
}
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::Save(const char* a_fileName)
{
       RTFileStream stream;
       if (!stream.OpenWrite(a_fileName))
       {
              return false;
       }
 
       bool result = Save(stream);
 
       stream.Close();
 
       return result;
}
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::Save(RTFileStream& a_stream)
{
       // Write some kind of header
       int dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
       int dataSize = sizeof(DATATYPE);
       int dataNumDims = NUMDIMS;
       int dataElemSize = sizeof(ELEMTYPE);
       int dataElemRealSize = sizeof(ELEMTYPEREAL);
       int dataMaxNodes = TMAXNODES;
       int dataMinNodes = TMINNODES;
 
       a_stream.Write(dataFileId);
       a_stream.Write(dataSize);
       a_stream.Write(dataNumDims);
       a_stream.Write(dataElemSize);
       a_stream.Write(dataElemRealSize);
       a_stream.Write(dataMaxNodes);
       a_stream.Write(dataMinNodes);
 
       // Recursively save tree
       bool result = SaveRec(m_root, a_stream);
 
       return result;
}
 
 
RTREE_TEMPLATE
bool RTREE_QUAL::SaveRec(Node* a_node, RTFileStream& a_stream)
{
       a_stream.Write(a_node->m_level);
       a_stream.Write(a_node->m_count);
 
       if (a_node->IsInternalNode())  // not a leaf node
       {
              for (int index = 0; index < a_node->m_count; ++index)
              {
                     Branch* curBranch = &a_node->m_branch[index];
 
                     a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS);
                     a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS);
 
                     SaveRec(curBranch->m_child, a_stream);
              }
       }
       else // A leaf node
       {
              for (int index = 0; index < a_node->m_count; ++index)
              {
                     Branch* curBranch = &a_node->m_branch[index];
 
                     a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS);
                     a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS);
 
                     a_stream.Write(curBranch->m_data);
              }
       }
 
       return true; // Should do more error checking on I/O operations
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::RemoveAll()
{
       // Delete all existing nodes
       Reset();
 
       m_root = AllocNode();
       m_root->m_level = 0;
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::Reset()
{
#ifdef RTREE_DONT_USE_MEMPOOLS
       // Delete all existing nodes
       RemoveAllRec(m_root);
#else // RTREE_DONT_USE_MEMPOOLS
       // Just reset memory pools.  We are not using complex types
       // EXAMPLE
#endif // RTREE_DONT_USE_MEMPOOLS
}
 
 
RTREE_TEMPLATE
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)
              {
                     RemoveAllRec(a_node->m_branch[index].m_child);
              }
       }
       FreeNode(a_node);
}
 
 
RTREE_TEMPLATE
typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
{
       Node* newNode;
#ifdef RTREE_DONT_USE_MEMPOOLS
       newNode = new Node;
#else // RTREE_DONT_USE_MEMPOOLS
       // EXAMPLE
#endif // RTREE_DONT_USE_MEMPOOLS
       InitNode(newNode);
       return newNode;
}
 
 
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
       // EXAMPLE
#endif // RTREE_DONT_USE_MEMPOOLS
}
 
 
// Allocate space for a node in the list used in DeletRect to
// store Nodes that are too empty.
RTREE_TEMPLATE
typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
{
#ifdef RTREE_DONT_USE_MEMPOOLS
       return new ListNode;
#else // RTREE_DONT_USE_MEMPOOLS
       // EXAMPLE
#endif // RTREE_DONT_USE_MEMPOOLS
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::FreeListNode(ListNode* a_listNode)
{
#ifdef RTREE_DONT_USE_MEMPOOLS
       delete a_listNode;
#else // RTREE_DONT_USE_MEMPOOLS
       // EXAMPLE
#endif // RTREE_DONT_USE_MEMPOOLS
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::InitNode(Node* a_node)
{
       a_node->m_count = 0;
       a_node->m_level = -1;
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::InitRect(Rect* a_rect)
{
       for (int index = 0; index < NUMDIMS; ++index)
       {
              a_rect->m_min[index] = (ELEMTYPE)0;
              a_rect->m_max[index] = (ELEMTYPE)0;
       }
}
 
 
// Inserts a new data rectangle into the index structure.
// Recursively descends tree, propagates splits back up.
// Returns 0 if node was not split.  Old node updated.
// If node was split, returns 1 and sets the pointer pointed to by
// new_node to point to the new node.  Old node updated to become one of two.
// 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(const Branch& a_branch, Node* a_node, Node** a_newNode, int a_level)
{
       assert(a_node && a_newNode);
       assert(a_level >= 0 && a_level <= a_node->m_level);
 
       // recurse until we reach the correct level for the new record. data records
       // will always be called with a_level == 0 (leaf)
       if (a_node->m_level > a_level)
       {
              // Still above level for insertion, go down tree recursively
              Node* otherNode;
 
              // find the optimal branch for this record
              int index = PickBranch(&a_branch.m_rect, a_node);
 
              // recursively insert this record into the picked branch
              bool childWasSplit = InsertRectRec(a_branch, a_node->m_branch[index].m_child, &otherNode, a_level);
 
              if (!childWasSplit)
              {
                     // Child was not split. Merge the bounding box of the new record with the
                     // existing bounding box
                     a_node->m_branch[index].m_rect = CombineRect(&a_branch.m_rect, &(a_node->m_branch[index].m_rect));
                     return false;
              }
              else
              {
                     // Child was split. The old branches are now re-partitioned to two nodes
                     // so we have to re-calculate the bounding boxes of each node
                     a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child);
                     Branch branch;
                     branch.m_child = otherNode;
                     branch.m_rect = NodeCover(otherNode);
 
                     // The old node is already a child of a_node. Now add the newly-created
                     // node to a_node as well. a_node might be split because of that.
                     return AddBranch(&branch, a_node, a_newNode);
              }
       }
       else if (a_node->m_level == a_level)
       {
              // We have reached level for insertion. Add rect, split if necessary
              return AddBranch(&a_branch, a_node, a_newNode);
       }
       else
       {
              // Should never occur
              assert(0);
              return false;
       }
}
 
 
// Insert a data rectangle into an index structure.
// InsertRect provides for splitting the root;
// returns 1 if root was split, 0 if it was not.
// 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.
// InsertRect2 does the recursion.
//
RTREE_TEMPLATE
bool RTREE_QUAL::InsertRect(const Branch& a_branch, Node** a_root, int a_level)
{
       assert(a_root);
       assert(a_level >= 0 && a_level <= (*a_root)->m_level);
#ifdef _DEBUG
       for (int index = 0; index < NUMDIMS; ++index)
       {
              assert(a_branch.m_rect.m_min[index] <= a_branch.m_rect.m_max[index]);
       }
#endif //_DEBUG
 
       Node* newNode;
 
       if (InsertRectRec(a_branch, *a_root, &newNode, a_level))  // Root split
       {
              // Grow tree taller and new root
              Node* newRoot = AllocNode();
              newRoot->m_level = (*a_root)->m_level + 1;
 
              Branch branch;
 
              // add old root node as a child of the new root
              branch.m_rect = NodeCover(*a_root);
              branch.m_child = *a_root;
              AddBranch(&branch, newRoot, NULL);
 
              // add the split node as a child of the new root
              branch.m_rect = NodeCover(newNode);
              branch.m_child = newNode;
              AddBranch(&branch, newRoot, NULL);
 
              // set the new root as the root node
              *a_root = newRoot;
 
              return true;
       }
 
       return false;
}
 
 
// 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);
 
       Rect rect = a_node->m_branch[0].m_rect;
       for (int index = 1; index < a_node->m_count; ++index)
       {
              rect = CombineRect(&rect, &(a_node->m_branch[index].m_rect));
       }
 
       return rect;
}
 
 
// Add a branch to a node.  Split the node if necessary.
// Returns 0 if node not split.  Old node updated.
// Returns 1 if node split, sets *new_node to address of new node.
// Old node updated, becomes one of two.
RTREE_TEMPLATE
bool RTREE_QUAL::AddBranch(const 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
       {
              a_node->m_branch[a_node->m_count] = *a_branch;
              ++a_node->m_count;
 
              return false;
       }
       else
       {
              assert(a_newNode);
 
              SplitNode(a_node, a_branch, a_newNode);
              return true;
       }
}
 
 
// Disconnect a dependent node.
// 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;
}
 
 
// Pick a branch.  Pick the one that will need the smallest increase
// in area to accomodate the new rectangle.  This will result in the
// least total area for the covering rectangles in the current node.
// In case of a tie, pick the one which was smaller before, to get
// the best resolution when searching.
RTREE_TEMPLATE
int RTREE_QUAL::PickBranch(const Rect* a_rect, Node* a_node)
{
       assert(a_rect && a_node);
 
       bool firstTime = true;
       ELEMTYPEREAL increase;
       ELEMTYPEREAL bestIncr = (ELEMTYPEREAL)-1;
       ELEMTYPEREAL area;
       ELEMTYPEREAL bestArea  = 0;
       int best = 0;
       Rect tempRect;
 
       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)
              {
                     best = index;
                     bestArea = area;
                     bestIncr = increase;
                     firstTime = false;
              }
              else if ((increase == bestIncr) && (area < bestArea))
              {
                     best = index;
                     bestArea = area;
                     bestIncr = increase;
              }
       }
       return best;
}
 
 
// Combine two rectangles into larger one containing both
RTREE_TEMPLATE
typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect(const Rect* a_rectA, const Rect* a_rectB)
{
       assert(a_rectA && a_rectB);
 
       Rect newRect;
 
       for (int index = 0; index < NUMDIMS; ++index)
       {
              newRect.m_min[index] = std::min(a_rectA->m_min[index], a_rectB->m_min[index]);
              newRect.m_max[index] = std::max(a_rectA->m_max[index], a_rectB->m_max[index]);
       }
 
       return newRect;
}
 
 
 
// Split a node.
// Divides the nodes branches and the extra one between two nodes.
// Old node is one of the new ones, and one really new one is created.
// Tries more than one method for choosing a partition, uses best result.
RTREE_TEMPLATE
void RTREE_QUAL::SplitNode(Node* a_node, const 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
       PartitionVars localVars;
       PartitionVars* parVars = &localVars;
 
       // Load all the branches into a buffer, initialize old node
       GetBranches(a_node, a_branch, parVars);
 
       // Find partition
       ChoosePartition(parVars, MINNODES);
 
       // Create a new node to hold (about) half of the branches
       *a_newNode = AllocNode();
       (*a_newNode)->m_level = a_node->m_level;
 
       // Put branches from buffer into 2 nodes according to the chosen partition
       a_node->m_count = 0;
       LoadNodes(a_node, *a_newNode, parVars);
 
       assert((a_node->m_count + (*a_newNode)->m_count) == parVars->m_total);
}
 
 
// Calculate the n-dimensional volume of a rectangle
RTREE_TEMPLATE
ELEMTYPEREAL RTREE_QUAL::RectVolume(Rect* a_rect)
{
       assert(a_rect);
 
       ELEMTYPEREAL volume = (ELEMTYPEREAL)1;
 
       for (int index = 0; index < NUMDIMS; ++index)
       {
              volume *= a_rect->m_max[index] - a_rect->m_min[index];
       }
 
       assert(volume >= (ELEMTYPEREAL)0);
 
       return volume;
}
 
 
// The exact volume of the bounding sphere for the given Rect
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]) * (ELEMTYPEREAL)0.5;
              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)
       {
              return (radius * radius * radius * m_unitSphereVolume);
       }
       else if (NUMDIMS == 2)
       {
              return (radius * radius * m_unitSphereVolume);
       }
       else
       {
              return (ELEMTYPEREAL)(pow(radius, NUMDIMS) * m_unitSphereVolume);
       }
}
 
 
// Use one of the methods to calculate retangle volume
RTREE_TEMPLATE
ELEMTYPEREAL RTREE_QUAL::CalcRectVolume(Rect* a_rect)
{
#ifdef RTREE_USE_SPHERICAL_VOLUME
       return RectSphericalVolume(a_rect); // Slower but helps certain merge cases
#else // RTREE_USE_SPHERICAL_VOLUME
       return RectVolume(a_rect); // Faster but can cause poor merges
#endif // RTREE_USE_SPHERICAL_VOLUME
}
 
 
// Load branch buffer with branches from full node plus the extra branch.
RTREE_TEMPLATE
void RTREE_QUAL::GetBranches(Node* a_node, const 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)
       {
              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);
       }
       a_parVars->m_coverSplitArea = CalcRectVolume(&a_parVars->m_coverSplit);
}
 
 
// Method #0 for choosing a partition:
// As the seeds for the two groups, pick the two rects that would waste the
// most area if covered by a single rectangle, i.e. evidently the worst pair
// to have in the same group.
// Of the remaining, one at a time is chosen to be put in one of the two groups.
// The one chosen is the one with the greatest difference in area expansion
// depending on which group - the rect most strongly attracted to one group
// and repelled from the other.
// If one group gets too full (more would force other group to violate min
// fill requirement) then other group gets the rest.
// These last are the ones that can go in either group most easily.
RTREE_TEMPLATE
void RTREE_QUAL::ChoosePartition(PartitionVars* a_parVars, int a_minFill)
{
       assert(a_parVars);
 
       ELEMTYPEREAL biggestDiff;
       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)))
       {
              biggestDiff = (ELEMTYPEREAL)-1;
              for (int index = 0; index < a_parVars->m_total; ++index)
              {
                     if (PartitionVars::NOT_TAKEN == a_parVars->m_partition[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)
                            {
                                   group = 0;
                            }
                            else
                            {
                                   group = 1;
                                   diff = -diff;
                            }
 
                            if (diff > biggestDiff)
                            {
                                   biggestDiff = diff;
                                   chosen = index;
                                   betterGroup = group;
                            }
                            else if ((diff == biggestDiff) && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]))
                            {
                                   chosen = index;
                                   betterGroup = group;
                            }
                     }
              }
              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)
              {
                     group = 1;
              }
              else
              {
                     group = 0;
              }
              for (int index = 0; index < a_parVars->m_total; ++index)
              {
                     if (PartitionVars::NOT_TAKEN == a_parVars->m_partition[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));
}
 
 
// Copy branches from the buffer into two nodes according to the partition.
RTREE_TEMPLATE
void RTREE_QUAL::LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars)
{
       assert(a_nodeA);
       assert(a_nodeB);
       assert(a_parVars);
 
       for (int index = 0; index < a_parVars->m_total; ++index)
       {
              assert(a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1);
 
              int targetNodeIndex = a_parVars->m_partition[index];
              Node* targetNodes[] = { a_nodeA, a_nodeB };
 
              // It is assured that AddBranch here will not cause a node split.
              bool nodeWasSplit = AddBranch(&a_parVars->m_branchBuf[index], targetNodes[targetNodeIndex], NULL);
              assert(!nodeWasSplit);
       }
}
 
 
// Initialize a PartitionVars structure.
RTREE_TEMPLATE
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)
       {
              a_parVars->m_partition[index] = PartitionVars::NOT_TAKEN;
       }
}
 
 
RTREE_TEMPLATE
void RTREE_QUAL::PickSeeds(PartitionVars* a_parVars)
{
       int seed0 = 0, seed1 = 0;
       ELEMTYPEREAL worst, waste;
       ELEMTYPEREAL area[MAXNODES + 1];
 
       for (int index = 0; index < a_parVars->m_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);
                     waste = CalcRectVolume(&oneRect) - area[indexA] - area[indexB];
                     if (waste > worst)
                     {
                            worst = waste;
                            seed0 = indexA;
                            seed1 = indexB;
                     }
              }
       }
 
       Classify(seed0, 0, a_parVars);
       Classify(seed1, 1, a_parVars);
}
 
 
// Put a branch in one of the groups.
RTREE_TEMPLATE
void RTREE_QUAL::Classify(int a_index, int a_group, PartitionVars* a_parVars)
{
       assert(a_parVars);
       assert(PartitionVars::NOT_TAKEN == a_parVars->m_partition[a_index]);
 
       a_parVars->m_partition[a_index] = a_group;
 
       // Calculate combined rect
       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]);
       }
 
       // Calculate volume of combined rect
       a_parVars->m_area[a_group] = CalcRectVolume(&a_parVars->m_cover[a_group]);
 
       ++a_parVars->m_count[a_group];
}
 
 
// Delete a data rectangle from an index structure.
// Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root node.
// Returns 1 if record not found, 0 if success.
// RemoveRect provides for eliminating the root.
RTREE_TEMPLATE
bool RTREE_QUAL::RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root)
{
       assert(a_rect && a_root);
       assert(*a_root);
 
       ListNode* reInsertList = NULL;
 
       if (!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList))
       {
              // Found and deleted a data item
              // Reinsert any branches from eliminated nodes
              while (reInsertList)
              {
                     Node* tempNode = reInsertList->m_node;
 
                     for (int index = 0; index < tempNode->m_count; ++index)
                     {
                            // TODO go over this code. should I use (tempNode->m_level - 1)?
                            InsertRect(tempNode->m_branch[index],
                                   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 TODO replace
              // if with while? In case there is a whole branch of redundant roots...
              if ((*a_root)->m_count == 1 && (*a_root)->IsInternalNode())
              {
                     Node* tempNode = (*a_root)->m_branch[0].m_child;
 
                     assert(tempNode);
                     FreeNode(*a_root);
                     *a_root = tempNode;
              }
              return false;
       }
       else
       {
              return true;
       }
}
 
 
// Delete a rectangle from non-root part of an index structure.
// Called by RemoveRect.  Descends tree recursively,
// 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)
{
       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)
                                   {
                                          // 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
                                          ReInsert(a_node->m_branch[index].m_child, a_listNode);
                                          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)
              {
                     if (a_node->m_branch[index].m_data == a_id)
                     {
                            DisconnectBranch(a_node, index); // Must return after this call as count has changed
                            return false;
                     }
              }
              return true;
       }
}
 
 
// Decide whether two rectangles overlap.
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)
       {
              if (a_rectA->m_min[index] > a_rectB->m_max[index] ||
                     a_rectB->m_min[index] > a_rectA->m_max[index])
              {
                     return false;
              }
       }
       return true;
}
 
 
// Add a node to the reinsertion list.  All its branches will later
// be reinserted into the index structure.
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;
       *a_listNode = newListNode;
}
 
 
// Search in an index tree or subtree for all data retangles that overlap the argument rectangle.
RTREE_TEMPLATE
template<typename Func>
bool RTREE_QUAL::Search(Node* a_node, Rect* a_rect, int& a_foundCount, Func&& callback) 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, std::forward<Func>(callback)))
                            {
                                   // The callback indicated to stop searching
                                   return false;
                            }
                     }
              }
       }
       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;
                            ++a_foundCount;
 
                            if (!callback(id))
                            {
                                   return false; // Don't continue searching
                            }
                     }
              }
       }
 
       return true; // Continue searching
}
 
 
#undef RTREE_TEMPLATE
#undef RTREE_QUAL
 
#endif //RTREE_H