SUMO - Simulation of Urban MObility
RTree.h
Go to the documentation of this file.
00001 #ifndef RTREE_H
00002 #define RTREE_H
00003 
00004 // NOTE This file compiles under MSVC 6 SP5 and MSVC .Net 2003 it may not work on other compilers without modification.
00005 
00006 // NOTE These next few lines may be win32 specific, you may need to modify them to compile on other platform
00007 #include <stdio.h>
00008 #include <math.h>
00009 #include <assert.h>
00010 #include <stdlib.h>
00011 
00012 #define ASSERT assert // RTree uses ASSERT( condition )
00013 #ifndef Min
00014   #define Min __min 
00015 #endif //Min
00016 #ifndef Max
00017   #define Max __max 
00018 #endif //Max
00019 
00020 #define rtree_min(a,b) (a<b?a:b)
00021 #define rtree_max(a,b) (a>b?a:b)
00022 
00023 
00024 
00025 //
00026 // RTree.h
00027 //
00028 
00029 #define RTREE_TEMPLATE template<class DATATYPE, class DATATYPENP, class ELEMTYPE, int NUMDIMS, class CONTEXT, class ELEMTYPEREAL, int TMAXNODES, int TMINNODES>
00030 #define RTREE_QUAL RTree<DATATYPE, DATATYPENP, ELEMTYPE, NUMDIMS, CONTEXT, ELEMTYPEREAL, TMAXNODES, TMINNODES>
00031 
00032 #define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one.
00033 #define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems
00034 
00035 #undef RTREE_WANTS_IO
00036 
00037 // Fwd decl
00038 #ifdef RTREE_WANTS_IO
00039 class RTFileStream;  // File I/O helper class, look below for implementation and notes.
00040 #endif
00041 
00042 
00059 template<class DATATYPE, class DATATYPENP, class ELEMTYPE, int NUMDIMS, class CONTEXT,
00060          class ELEMTYPEREAL = ELEMTYPE, int TMAXNODES = 8, int TMINNODES = TMAXNODES / 2>
00061 class RTree
00062 {
00063 protected: 
00064 
00065   struct Node;  // Fwd decl.  Used by other internal structs and iterator
00066 
00067 public:
00068 
00069   // These constant must be declared after Branch and before Node struct
00070   // Stuck up here for MSVC 6 compiler.  NSVC .NET 2003 is much happier.
00071   enum
00072   {
00073     MAXNODES = TMAXNODES,                         
00074     MINNODES = TMINNODES                         
00075   };
00076 
00077 
00078 public:
00079   typedef void(DATATYPENP::* Operation)(const CONTEXT &) const;
00080 
00081   RTree(Operation operation);
00082   virtual ~RTree();
00083   
00088   void Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId);
00089   
00094   void Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId);
00095   
00096 
00098   //typedef void(DATATYPENP::* Operation)(const CONTEXT &) const;
00099 
00107   int Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const CONTEXT &c);
00108 
00110 
00112   void RemoveAll();
00113 
00115   int Count();
00116 
00117 #ifdef RTREE_WANTS_IO
00118 
00119   bool Load(const char* a_fileName);
00121   bool Load(RTFileStream& a_stream);
00122 
00123   
00125   bool Save(const char* a_fileName);
00127   bool Save(RTFileStream& a_stream);
00128 #endif
00129 
00131   class Iterator
00132   {
00133   private:
00134   
00135     enum { MAX_STACK = 32 }; //  Max stack size. Allows almost n^32 where n is number of branches in node
00136     
00137     struct StackElement
00138     {
00139       Node* m_node;
00140       int m_branchIndex;
00141     };
00142     
00143   public:
00144   
00145     Iterator()                                    { Init(); }
00146 
00147     ~Iterator()                                   { }
00148     
00150     bool IsNull()                                 { return (m_tos <= 0); }
00151 
00153     bool IsNotNull()                              { return (m_tos > 0); }
00154 
00156     DATATYPE& operator*()
00157     {
00158       ASSERT(IsNotNull());
00159       StackElement& curTos = m_stack[m_tos - 1];
00160       return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
00161     } 
00162 
00164     const DATATYPE& operator*() const
00165     {
00166       ASSERT(IsNotNull());
00167       StackElement& curTos = m_stack[m_tos - 1];
00168       return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
00169     } 
00170 
00172     bool operator++()                             { return FindNextData(); }
00173 
00174   private:
00175   
00177     void Init()                                   { m_tos = 0; }
00178 
00180     bool FindNextData()
00181     {
00182       for(;;)
00183       {
00184         if(m_tos <= 0)
00185         {
00186           return false;
00187         }
00188         StackElement curTos = Pop(); // Copy stack top cause it may change as we use it
00189 
00190         if(curTos.m_node->IsLeaf())
00191         {
00192           // Keep walking through data while we can
00193           if(curTos.m_branchIndex+1 < curTos.m_node->m_count)
00194           {
00195             // There is more data, just point to the next one
00196             Push(curTos.m_node, curTos.m_branchIndex + 1);
00197             return true;
00198           }
00199           // No more data, so it will fall back to previous level
00200         }
00201         else
00202         {
00203           if(curTos.m_branchIndex+1 < curTos.m_node->m_count)
00204           {
00205             // Push sibling on for future tree walk
00206             // This is the 'fall back' node when we finish with the current level
00207             Push(curTos.m_node, curTos.m_branchIndex + 1);
00208           }
00209           // Since cur node is not a leaf, push first of next level to get deeper into the tree
00210           Node* nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
00211           Push(nextLevelnode, 0);
00212           
00213           // If we pushed on a new leaf, exit as the data is ready at TOS
00214           if(nextLevelnode->IsLeaf())
00215           {
00216             return true;
00217           }
00218         }
00219       }
00220     }
00221 
00223     void Push(Node* a_node, int a_branchIndex)
00224     {
00225       m_stack[m_tos].m_node = a_node;
00226       m_stack[m_tos].m_branchIndex = a_branchIndex;
00227       ++m_tos;
00228       ASSERT(m_tos <= MAX_STACK);
00229     }
00230     
00232     StackElement& Pop()
00233     {
00234       ASSERT(m_tos > 0);
00235       --m_tos;
00236       return m_stack[m_tos];
00237     }
00238 
00239     StackElement m_stack[MAX_STACK];              
00240     int m_tos;                                    
00241   
00242     friend class RTree<DATATYPE, DATATYPENP, ELEMTYPE, NUMDIMS, CONTEXT, ELEMTYPEREAL, TMAXNODES, TMINNODES>; // Allow hiding of non-public functions while allowing manipulation by logical owner
00243   };
00244 
00246   void GetFirst(Iterator& a_it)
00247   {
00248     a_it.Init();
00249     if(m_root && (m_root->m_count > 0))
00250     {
00251       a_it.Push(m_root, 0);
00252       a_it.FindNextData();
00253     }
00254   }
00255 
00257   void GetNext(Iterator& a_it)                    { ++a_it; }
00258 
00260   bool IsNull(Iterator& a_it)                     { return a_it.IsNull(); }
00261 
00263   DATATYPE& GetAt(Iterator& a_it)                 { return *a_it; }
00264 
00265 protected:
00266 
00268   struct Rect
00269   {
00270     ELEMTYPE m_min[NUMDIMS];                      
00271     ELEMTYPE m_max[NUMDIMS];                      
00272   };
00273 
00277   struct Branch
00278   {
00279     Rect m_rect;                                  
00280     union
00281     {
00282       Node* m_child;                              
00283       DATATYPE m_data;                            
00284     };
00285   };
00286 
00288   struct Node
00289   {
00290     bool IsInternalNode()                         { return (m_level > 0); } // Not a leaf, but a internal node
00291     bool IsLeaf()                                 { return (m_level == 0); } // A leaf, contains data
00292     
00293     int m_count;                                  
00294     int m_level;                                  
00295     Branch m_branch[MAXNODES];                    
00296   };
00297   
00299   struct ListNode
00300   {
00301     ListNode* m_next;                             
00302     Node* m_node;                                 
00303   };
00304 
00306   struct PartitionVars
00307   {
00308     int m_partition[MAXNODES+1];
00309     int m_total;
00310     int m_minFill;
00311     int m_taken[MAXNODES+1];
00312     int m_count[2];
00313     Rect m_cover[2];
00314     ELEMTYPEREAL m_area[2];
00315 
00316     Branch m_branchBuf[MAXNODES+1];
00317     int m_branchCount;
00318     Rect m_coverSplit;
00319     ELEMTYPEREAL m_coverSplitArea;
00320   }; 
00321  
00322   Node* AllocNode();
00323   void FreeNode(Node* a_node);
00324   void InitNode(Node* a_node);
00325   void InitRect(Rect* a_rect);
00326   bool InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, Node** a_newNode, int a_level);
00327   bool InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level);
00328   Rect NodeCover(Node* a_node);
00329   bool AddBranch(Branch* a_branch, Node* a_node, Node** a_newNode);
00330   void DisconnectBranch(Node* a_node, int a_index);
00331   int PickBranch(Rect* a_rect, Node* a_node);
00332   Rect CombineRect(Rect* a_rectA, Rect* a_rectB);
00333   void SplitNode(Node* a_node, Branch* a_branch, Node** a_newNode);
00334   ELEMTYPEREAL RectSphericalVolume(Rect* a_rect);
00335   ELEMTYPEREAL RectVolume(Rect* a_rect);
00336   ELEMTYPEREAL CalcRectVolume(Rect* a_rect);
00337   void GetBranches(Node* a_node, Branch* a_branch, PartitionVars* a_parVars);
00338   void ChoosePartition(PartitionVars* a_parVars, int a_minFill);
00339   void LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars);
00340   void InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill);
00341   void PickSeeds(PartitionVars* a_parVars);
00342   void Classify(int a_index, int a_group, PartitionVars* a_parVars);
00343   bool RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root);
00344   bool RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode);
00345   ListNode* AllocListNode();
00346   void FreeListNode(ListNode* a_listNode);
00347   bool Overlap(Rect* a_rectA, Rect* a_rectB);
00348   void ReInsert(Node* a_node, ListNode** a_listNode);
00349   bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, const CONTEXT &c);
00350   void RemoveAllRec(Node* a_node);
00351   void Reset();
00352   void CountRec(Node* a_node, int& a_count);
00353 
00354 #ifdef RTREE_WANTS_IO
00355   bool SaveRec(Node* a_node, RTFileStream& a_stream);
00356   bool LoadRec(Node* a_node, RTFileStream& a_stream);
00357 #endif
00358 
00359   Node* m_root;                                    
00360   ELEMTYPEREAL m_unitSphereVolume;                 
00361   Operation myOperation;
00362 };
00363 
00364 
00365 #ifdef RTREE_WANTS_IO
00366 // Because there is not stream support, this is a quick and dirty file I/O helper.
00367 // Users will likely replace its usage with a Stream implementation from their favorite API.
00368 class RTFileStream
00369 {
00370   FILE* m_file;
00371 
00372 public:
00373 
00374   
00375   RTFileStream()
00376   {
00377     m_file = NULL;
00378   }
00379 
00380   ~RTFileStream()
00381   {
00382     Close();
00383   }
00384 
00385   bool OpenRead(const char* a_fileName)
00386   {
00387     m_file = fopen(a_fileName, "rb");
00388     if(!m_file)
00389     {
00390       return false;
00391     }
00392     return true;
00393   }
00394 
00395   bool OpenWrite(const char* a_fileName)
00396   {
00397     m_file = fopen(a_fileName, "wb");
00398     if(!m_file)
00399     {
00400       return false;
00401     }
00402     return true;
00403   }
00404 
00405   void Close()
00406   {
00407     if(m_file)
00408     {
00409       fclose(m_file);
00410       m_file = NULL;
00411     }
00412   }
00413 
00414   template< typename TYPE >
00415   size_t Write(const TYPE& a_value)
00416   {
00417     ASSERT(m_file);
00418     return fwrite((void*)&a_value, sizeof(a_value), 1, m_file);
00419   }
00420 
00421   template< typename TYPE >
00422   size_t WriteArray(const TYPE* a_array, int a_count)
00423   {
00424     ASSERT(m_file);
00425     return fwrite((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
00426   }
00427 
00428   template< typename TYPE >
00429   size_t Read(TYPE& a_value)
00430   {
00431     ASSERT(m_file);
00432     return fread((void*)&a_value, sizeof(a_value), 1, m_file);
00433   }
00434 
00435   template< typename TYPE >
00436   size_t ReadArray(TYPE* a_array, int a_count)
00437   {
00438     ASSERT(m_file);
00439     return fread((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
00440   }
00441 };
00442 #endif
00443 
00444 
00445 RTREE_TEMPLATE
00446 RTREE_QUAL::RTree(Operation operation)
00447 : myOperation(operation)
00448 {
00449   ASSERT(MAXNODES > MINNODES);
00450   ASSERT(MINNODES > 0);
00451 
00452 
00453   // We only support machine word size simple data type eg. integer index or object pointer.
00454   // Since we are storing as union with non data branch
00455   ASSERT(sizeof(DATATYPE) == sizeof(void*) || sizeof(DATATYPE) == sizeof(int));
00456 
00457   // Precomputed volumes of the unit spheres for the first few dimensions
00458   const float UNIT_SPHERE_VOLUMES[] = {
00459     0.000000f, 2.000000f, 3.141593f, // Dimension  0,1,2
00460     4.188790f, 4.934802f, 5.263789f, // Dimension  3,4,5
00461     5.167713f, 4.724766f, 4.058712f, // Dimension  6,7,8
00462     3.298509f, 2.550164f, 1.884104f, // Dimension  9,10,11
00463     1.335263f, 0.910629f, 0.599265f, // Dimension  12,13,14
00464     0.381443f, 0.235331f, 0.140981f, // Dimension  15,16,17
00465     0.082146f, 0.046622f, 0.025807f, // Dimension  18,19,20 
00466   };
00467 
00468   m_root = AllocNode();
00469   m_root->m_level = 0;
00470   m_unitSphereVolume = (ELEMTYPEREAL)UNIT_SPHERE_VOLUMES[NUMDIMS];
00471 }
00472 
00473 
00474 RTREE_TEMPLATE
00475 RTREE_QUAL::~RTree()
00476 {
00477   Reset(); // Free, or reset node memory
00478 }
00479 
00480 
00481 RTREE_TEMPLATE
00482 void RTREE_QUAL::Insert(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId)
00483 {
00484 #ifdef _DEBUG
00485   for(int index=0; index<NUMDIMS; ++index)
00486   {
00487     ASSERT(a_min[index] <= a_max[index]);
00488   }
00489 #endif //_DEBUG
00490 
00491   Rect rect;
00492   
00493   for(int axis=0; axis<NUMDIMS; ++axis)
00494   {
00495     rect.m_min[axis] = a_min[axis];
00496     rect.m_max[axis] = a_max[axis];
00497   }
00498   
00499   InsertRect(&rect, a_dataId, &m_root, 0);
00500 }
00501 
00502 
00503 RTREE_TEMPLATE
00504 void RTREE_QUAL::Remove(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const DATATYPE& a_dataId)
00505 {
00506 #ifdef _DEBUG
00507   for(int index=0; index<NUMDIMS; ++index)
00508   {
00509     ASSERT(a_min[index] <= a_max[index]);
00510   }
00511 #endif //_DEBUG
00512 
00513   Rect rect;
00514   
00515   for(int axis=0; axis<NUMDIMS; ++axis)
00516   {
00517     rect.m_min[axis] = a_min[axis];
00518     rect.m_max[axis] = a_max[axis];
00519   }
00520 
00521   RemoveRect(&rect, a_dataId, &m_root);
00522 }
00523 
00524 
00525 
00526 RTREE_TEMPLATE
00527 int RTREE_QUAL::Search(const ELEMTYPE a_min[NUMDIMS], const ELEMTYPE a_max[NUMDIMS], const CONTEXT &c)
00528 {
00529 #ifdef _DEBUG
00530   for(int index=0; index<NUMDIMS; ++index)
00531   {
00532     ASSERT(a_min[index] <= a_max[index]);
00533   }
00534 #endif //_DEBUG
00535 
00536   Rect rect;
00537   
00538   for(int axis=0; axis<NUMDIMS; ++axis)
00539   {
00540     rect.m_min[axis] = a_min[axis];
00541     rect.m_max[axis] = a_max[axis];
00542   }
00543 
00544   // NOTE: May want to return search result another way, perhaps returning the number of found elements here.
00545 
00546   int foundCount = 0;
00547   Search(m_root, &rect, foundCount, c);
00548 
00549   return foundCount;
00550 }
00551 
00552 
00553 
00554 
00555 
00556 
00557 RTREE_TEMPLATE
00558 int RTREE_QUAL::Count()
00559 {
00560   int count = 0;
00561   CountRec(m_root, count);
00562   
00563   return count;
00564 }
00565 
00566 
00567 
00568 RTREE_TEMPLATE
00569 void RTREE_QUAL::CountRec(Node* a_node, int& a_count)
00570 {
00571   if(a_node->IsInternalNode())  // not a leaf node
00572   {
00573     for(int index = 0; index < a_node->m_count; ++index)
00574     {
00575       CountRec(a_node->m_branch[index].m_child, a_count);
00576     }
00577   }
00578   else // A leaf node
00579   {
00580     a_count += a_node->m_count;
00581   }
00582 }
00583 
00584 
00585 #ifdef RTREE_WANTS_IO
00586 RTREE_TEMPLATE
00587 bool RTREE_QUAL::Load(const char* a_fileName)
00588 {
00589   RemoveAll(); // Clear existing tree
00590 
00591   RTFileStream stream;
00592   if(!stream.OpenRead(a_fileName))
00593   {
00594     return false;
00595   }
00596 
00597   bool result = Load(stream);
00598   
00599   stream.Close();
00600 
00601   return result;
00602 }
00603 
00604 
00605 
00606 RTREE_TEMPLATE
00607 bool RTREE_QUAL::Load(RTFileStream& a_stream)
00608 {
00609   // Write some kind of header
00610   int _dataFileId = ('R'<<0)|('T'<<8)|('R'<<16)|('E'<<24);
00611   int _dataSize = sizeof(DATATYPE);
00612   int _dataNumDims = NUMDIMS;
00613   int _dataElemSize = sizeof(ELEMTYPE);
00614   int _dataElemRealSize = sizeof(ELEMTYPEREAL);
00615   int _dataMaxNodes = TMAXNODES;
00616   int _dataMinNodes = TMINNODES;
00617 
00618   int dataFileId = 0;
00619   int dataSize = 0;
00620   int dataNumDims = 0;
00621   int dataElemSize = 0;
00622   int dataElemRealSize = 0;
00623   int dataMaxNodes = 0;
00624   int dataMinNodes = 0;
00625 
00626   a_stream.Read(dataFileId);
00627   a_stream.Read(dataSize);
00628   a_stream.Read(dataNumDims);
00629   a_stream.Read(dataElemSize);
00630   a_stream.Read(dataElemRealSize);
00631   a_stream.Read(dataMaxNodes);
00632   a_stream.Read(dataMinNodes);
00633 
00634   bool result = false;
00635 
00636   // Test if header was valid and compatible
00637   if(    (dataFileId == _dataFileId) 
00638       && (dataSize == _dataSize) 
00639       && (dataNumDims == _dataNumDims) 
00640       && (dataElemSize == _dataElemSize) 
00641       && (dataElemRealSize == _dataElemRealSize) 
00642       && (dataMaxNodes == _dataMaxNodes) 
00643       && (dataMinNodes == _dataMinNodes) 
00644     )
00645   {
00646     // Recursively load tree
00647     result = LoadRec(m_root, a_stream);
00648   }
00649 
00650   return result;
00651 }
00652 
00653 
00654 RTREE_TEMPLATE
00655 bool RTREE_QUAL::LoadRec(Node* a_node, RTFileStream& a_stream)
00656 {
00657   a_stream.Read(a_node->m_level);
00658   a_stream.Read(a_node->m_count);
00659 
00660   if(a_node->IsInternalNode())  // not a leaf node
00661   {
00662     for(int index = 0; index < a_node->m_count; ++index)
00663     {
00664       Branch* curBranch = &a_node->m_branch[index];
00665 
00666       a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS);
00667       a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS);
00668 
00669       curBranch->m_child = AllocNode();
00670       LoadRec(curBranch->m_child, a_stream);
00671     }
00672   }
00673   else // A leaf node
00674   {
00675     for(int index = 0; index < a_node->m_count; ++index)
00676     {
00677       Branch* curBranch = &a_node->m_branch[index];
00678 
00679       a_stream.ReadArray(curBranch->m_rect.m_min, NUMDIMS);
00680       a_stream.ReadArray(curBranch->m_rect.m_max, NUMDIMS);
00681 
00682       a_stream.Read(curBranch->m_data);
00683     }
00684   }
00685 
00686   return true; // Should do more error checking on I/O operations
00687 }
00688 
00689 
00690 RTREE_TEMPLATE
00691 bool RTREE_QUAL::Save(const char* a_fileName)
00692 {
00693   RTFileStream stream;
00694   if(!stream.OpenWrite(a_fileName))
00695   {
00696     return false;
00697   }
00698 
00699   bool result = Save(stream);
00700 
00701   stream.Close();
00702 
00703   return result;
00704 }
00705 
00706 
00707 RTREE_TEMPLATE
00708 bool RTREE_QUAL::Save(RTFileStream& a_stream)
00709 {
00710   // Write some kind of header
00711   int dataFileId = ('R'<<0)|('T'<<8)|('R'<<16)|('E'<<24);
00712   int dataSize = sizeof(DATATYPE);
00713   int dataNumDims = NUMDIMS;
00714   int dataElemSize = sizeof(ELEMTYPE);
00715   int dataElemRealSize = sizeof(ELEMTYPEREAL);
00716   int dataMaxNodes = TMAXNODES;
00717   int dataMinNodes = TMINNODES;
00718 
00719   a_stream.Write(dataFileId);
00720   a_stream.Write(dataSize);
00721   a_stream.Write(dataNumDims);
00722   a_stream.Write(dataElemSize);
00723   a_stream.Write(dataElemRealSize);
00724   a_stream.Write(dataMaxNodes);
00725   a_stream.Write(dataMinNodes);
00726 
00727   // Recursively save tree
00728   bool result = SaveRec(m_root, a_stream);
00729   
00730   return result;
00731 }
00732 
00733 
00734 RTREE_TEMPLATE
00735 bool RTREE_QUAL::SaveRec(Node* a_node, RTFileStream& a_stream)
00736 {
00737   a_stream.Write(a_node->m_level);
00738   a_stream.Write(a_node->m_count);
00739 
00740   if(a_node->IsInternalNode())  // not a leaf node
00741   {
00742     for(int index = 0; index < a_node->m_count; ++index)
00743     {
00744       Branch* curBranch = &a_node->m_branch[index];
00745 
00746       a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS);
00747       a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS);
00748 
00749       SaveRec(curBranch->m_child, a_stream);
00750     }
00751   }
00752   else // A leaf node
00753   {
00754     for(int index = 0; index < a_node->m_count; ++index)
00755     {
00756       Branch* curBranch = &a_node->m_branch[index];
00757 
00758       a_stream.WriteArray(curBranch->m_rect.m_min, NUMDIMS);
00759       a_stream.WriteArray(curBranch->m_rect.m_max, NUMDIMS);
00760 
00761       a_stream.Write(curBranch->m_data);
00762     }
00763   }
00764 
00765   return true; // Should do more error checking on I/O operations
00766 }
00767 #endif
00768 
00769 
00770 RTREE_TEMPLATE
00771 void RTREE_QUAL::RemoveAll()
00772 {
00773   // Delete all existing nodes
00774   Reset();
00775 
00776   m_root = AllocNode();
00777   m_root->m_level = 0;
00778 }
00779 
00780 
00781 RTREE_TEMPLATE
00782 void RTREE_QUAL::Reset()
00783 {
00784 #ifdef RTREE_DONT_USE_MEMPOOLS
00785   // Delete all existing nodes
00786   RemoveAllRec(m_root);
00787 #else // RTREE_DONT_USE_MEMPOOLS
00788   // Just reset memory pools.  We are not using complex types
00789   // EXAMPLE
00790 #endif // RTREE_DONT_USE_MEMPOOLS
00791 }
00792 
00793 
00794 RTREE_TEMPLATE
00795 void RTREE_QUAL::RemoveAllRec(Node* a_node)
00796 {
00797   ASSERT(a_node);
00798   ASSERT(a_node->m_level >= 0);
00799 
00800   if(a_node->IsInternalNode()) // This is an internal node in the tree
00801   {
00802     for(int index=0; index < a_node->m_count; ++index)
00803     {
00804       RemoveAllRec(a_node->m_branch[index].m_child);
00805     }
00806   }
00807   FreeNode(a_node); 
00808 }
00809 
00810 
00811 RTREE_TEMPLATE
00812 typename RTREE_QUAL::Node* RTREE_QUAL::AllocNode()
00813 {
00814   Node* newNode;
00815 #ifdef RTREE_DONT_USE_MEMPOOLS
00816   newNode = new Node;
00817 #else // RTREE_DONT_USE_MEMPOOLS
00818   // EXAMPLE
00819 #endif // RTREE_DONT_USE_MEMPOOLS
00820   InitNode(newNode);
00821   return newNode;
00822 }
00823 
00824 
00825 RTREE_TEMPLATE
00826 void RTREE_QUAL::FreeNode(Node* a_node)
00827 {
00828   ASSERT(a_node);
00829 
00830 #ifdef RTREE_DONT_USE_MEMPOOLS
00831   delete a_node;
00832 #else // RTREE_DONT_USE_MEMPOOLS
00833   // EXAMPLE
00834 #endif // RTREE_DONT_USE_MEMPOOLS
00835 }
00836 
00837 
00838 // Allocate space for a node in the list used in DeletRect to
00839 // store Nodes that are too empty.
00840 RTREE_TEMPLATE
00841 typename RTREE_QUAL::ListNode* RTREE_QUAL::AllocListNode()
00842 {
00843 #ifdef RTREE_DONT_USE_MEMPOOLS
00844   return new ListNode;
00845 #else // RTREE_DONT_USE_MEMPOOLS
00846   // EXAMPLE
00847 #endif // RTREE_DONT_USE_MEMPOOLS
00848 }
00849 
00850 
00851 RTREE_TEMPLATE
00852 void RTREE_QUAL::FreeListNode(ListNode* a_listNode)
00853 {
00854 #ifdef RTREE_DONT_USE_MEMPOOLS
00855   delete a_listNode;
00856 #else // RTREE_DONT_USE_MEMPOOLS
00857   // EXAMPLE
00858 #endif // RTREE_DONT_USE_MEMPOOLS
00859 }
00860 
00861 
00862 RTREE_TEMPLATE
00863 void RTREE_QUAL::InitNode(Node* a_node)
00864 {
00865   a_node->m_count = 0;
00866   a_node->m_level = -1;
00867 }
00868 
00869 
00870 RTREE_TEMPLATE
00871 void RTREE_QUAL::InitRect(Rect* a_rect)
00872 {
00873   for(int index = 0; index < NUMDIMS; ++index)
00874   {
00875     a_rect->m_min[index] = (ELEMTYPE)0;
00876     a_rect->m_max[index] = (ELEMTYPE)0;
00877   }
00878 }
00879 
00880 
00881 // Inserts a new data rectangle into the index structure.
00882 // Recursively descends tree, propagates splits back up.
00883 // Returns 0 if node was not split.  Old node updated.
00884 // If node was split, returns 1 and sets the pointer pointed to by
00885 // new_node to point to the new node.  Old node updated to become one of two.
00886 // The level argument specifies the number of steps up from the leaf
00887 // level to insert; e.g. a data rectangle goes in at level = 0.
00888 RTREE_TEMPLATE
00889 bool RTREE_QUAL::InsertRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, Node** a_newNode, int a_level)
00890 {
00891   ASSERT(a_rect && a_node && a_newNode);
00892   ASSERT(a_level >= 0 && a_level <= a_node->m_level);
00893 
00894   int index;
00895   Branch branch;
00896   Node* otherNode;
00897 
00898   // Still above level for insertion, go down tree recursively
00899   if(a_node->m_level > a_level)
00900   {
00901     index = PickBranch(a_rect, a_node);
00902     if (!InsertRectRec(a_rect, a_id, a_node->m_branch[index].m_child, &otherNode, a_level))
00903     {
00904       // Child was not split
00905       a_node->m_branch[index].m_rect = CombineRect(a_rect, &(a_node->m_branch[index].m_rect));
00906       return false;
00907     }
00908     else // Child was split
00909     {
00910       a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child);
00911       branch.m_child = otherNode;
00912       branch.m_rect = NodeCover(otherNode);
00913       return AddBranch(&branch, a_node, a_newNode);
00914     }
00915   }
00916   else if(a_node->m_level == a_level) // Have reached level for insertion. Add rect, split if necessary
00917   {
00918     branch.m_rect = *a_rect;
00919     branch.m_child = (Node*) a_id;
00920     // Child field of leaves contains id of data record
00921     return AddBranch(&branch, a_node, a_newNode);
00922   }
00923   else
00924   {
00925     // Should never occur
00926     ASSERT(0);
00927     return false;
00928   }
00929 }
00930 
00931 
00932 // Insert a data rectangle into an index structure.
00933 // InsertRect provides for splitting the root;
00934 // returns 1 if root was split, 0 if it was not.
00935 // The level argument specifies the number of steps up from the leaf
00936 // level to insert; e.g. a data rectangle goes in at level = 0.
00937 // InsertRect2 does the recursion.
00938 //
00939 RTREE_TEMPLATE
00940 bool RTREE_QUAL::InsertRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root, int a_level)
00941 {
00942   ASSERT(a_rect && a_root);
00943   ASSERT(a_level >= 0 && a_level <= (*a_root)->m_level);
00944 #ifdef _DEBUG
00945   for(int index=0; index < NUMDIMS; ++index)
00946   {
00947     ASSERT(a_rect->m_min[index] <= a_rect->m_max[index]);
00948   }
00949 #endif //_DEBUG  
00950 
00951   Node* newRoot;
00952   Node* newNode;
00953   Branch branch;
00954 
00955   if(InsertRectRec(a_rect, a_id, *a_root, &newNode, a_level))  // Root split
00956   {
00957     newRoot = AllocNode();  // Grow tree taller and new root
00958     newRoot->m_level = (*a_root)->m_level + 1;
00959     branch.m_rect = NodeCover(*a_root);
00960     branch.m_child = *a_root;
00961     AddBranch(&branch, newRoot, NULL);
00962     branch.m_rect = NodeCover(newNode);
00963     branch.m_child = newNode;
00964     AddBranch(&branch, newRoot, NULL);
00965     *a_root = newRoot;
00966     return true;
00967   }
00968 
00969   return false;
00970 }
00971 
00972 
00973 // Find the smallest rectangle that includes all rectangles in branches of a node.
00974 RTREE_TEMPLATE
00975 typename RTREE_QUAL::Rect RTREE_QUAL::NodeCover(Node* a_node)
00976 {
00977   ASSERT(a_node);
00978   
00979   int firstTime = true;
00980   Rect rect;
00981   InitRect(&rect);
00982   
00983   for(int index = 0; index < a_node->m_count; ++index)
00984   {
00985     if(firstTime)
00986     {
00987       rect = a_node->m_branch[index].m_rect;
00988       firstTime = false;
00989     }
00990     else
00991     {
00992       rect = CombineRect(&rect, &(a_node->m_branch[index].m_rect));
00993     }
00994   }
00995   
00996   return rect;
00997 }
00998 
00999 
01000 // Add a branch to a node.  Split the node if necessary.
01001 // Returns 0 if node not split.  Old node updated.
01002 // Returns 1 if node split, sets *new_node to address of new node.
01003 // Old node updated, becomes one of two.
01004 RTREE_TEMPLATE
01005 bool RTREE_QUAL::AddBranch(Branch* a_branch, Node* a_node, Node** a_newNode)
01006 {
01007   ASSERT(a_branch);
01008   ASSERT(a_node);
01009 
01010   if(a_node->m_count < MAXNODES)  // Split won't be necessary
01011   {
01012     a_node->m_branch[a_node->m_count] = *a_branch;
01013     ++a_node->m_count;
01014 
01015     return false;
01016   }
01017   else
01018   {
01019     ASSERT(a_newNode);
01020     
01021     SplitNode(a_node, a_branch, a_newNode);
01022     return true;
01023   }
01024 }
01025 
01026 
01027 // Disconnect a dependent node.
01028 // Caller must return (or stop using iteration index) after this as count has changed
01029 RTREE_TEMPLATE
01030 void RTREE_QUAL::DisconnectBranch(Node* a_node, int a_index)
01031 {
01032   ASSERT(a_node && (a_index >= 0) && (a_index < MAXNODES));
01033   ASSERT(a_node->m_count > 0);
01034 
01035   // Remove element by swapping with the last element to prevent gaps in array
01036   a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
01037   
01038   --a_node->m_count;
01039 }
01040 
01041 
01042 // Pick a branch.  Pick the one that will need the smallest increase
01043 // in area to accomodate the new rectangle.  This will result in the
01044 // least total area for the covering rectangles in the current node.
01045 // In case of a tie, pick the one which was smaller before, to get
01046 // the best resolution when searching.
01047 RTREE_TEMPLATE
01048 int RTREE_QUAL::PickBranch(Rect* a_rect, Node* a_node)
01049 {
01050   ASSERT(a_rect && a_node);
01051   
01052   bool firstTime = true;
01053   ELEMTYPEREAL increase;
01054   ELEMTYPEREAL bestIncr = (ELEMTYPEREAL)-1;
01055   ELEMTYPEREAL area;
01056   ELEMTYPEREAL bestArea;
01057   int best;
01058   Rect tempRect;
01059 
01060   for(int index=0; index < a_node->m_count; ++index)
01061   {
01062     Rect* curRect = &a_node->m_branch[index].m_rect;
01063     area = CalcRectVolume(curRect);
01064     tempRect = CombineRect(a_rect, curRect);
01065     increase = CalcRectVolume(&tempRect) - area;
01066     if((increase < bestIncr) || firstTime)
01067     {
01068       best = index;
01069       bestArea = area;
01070       bestIncr = increase;
01071       firstTime = false;
01072     }
01073     else if((increase == bestIncr) && (area < bestArea))
01074     {
01075       best = index;
01076       bestArea = area;
01077       bestIncr = increase;
01078     }
01079   }
01080   return best;
01081 }
01082 
01083 
01084 // Combine two rectangles into larger one containing both
01085 RTREE_TEMPLATE
01086 typename RTREE_QUAL::Rect RTREE_QUAL::CombineRect(Rect* a_rectA, Rect* a_rectB)
01087 {
01088   ASSERT(a_rectA && a_rectB);
01089 
01090   Rect newRect;
01091 
01092   for(int index = 0; index < NUMDIMS; ++index)
01093   {
01094     newRect.m_min[index] = rtree_min(a_rectA->m_min[index], a_rectB->m_min[index]);
01095     newRect.m_max[index] = rtree_max(a_rectA->m_max[index], a_rectB->m_max[index]);
01096   }
01097 
01098   return newRect;
01099 }
01100 
01101 
01102 
01103 // Split a node.
01104 // Divides the nodes branches and the extra one between two nodes.
01105 // Old node is one of the new ones, and one really new one is created.
01106 // Tries more than one method for choosing a partition, uses best result.
01107 RTREE_TEMPLATE
01108 void RTREE_QUAL::SplitNode(Node* a_node, Branch* a_branch, Node** a_newNode)
01109 {
01110   ASSERT(a_node);
01111   ASSERT(a_branch);
01112 
01113   // Could just use local here, but member or external is faster since it is reused
01114   PartitionVars localVars;
01115   PartitionVars* parVars = &localVars;
01116   int level;
01117 
01118   // Load all the branches into a buffer, initialize old node
01119   level = a_node->m_level;
01120   GetBranches(a_node, a_branch, parVars);
01121 
01122   // Find partition
01123   ChoosePartition(parVars, MINNODES);
01124 
01125   // Put branches from buffer into 2 nodes according to chosen partition
01126   *a_newNode = AllocNode();
01127   (*a_newNode)->m_level = a_node->m_level = level;
01128   LoadNodes(a_node, *a_newNode, parVars);
01129   
01130   ASSERT((a_node->m_count + (*a_newNode)->m_count) == parVars->m_total);
01131 }
01132 
01133 
01134 // Calculate the n-dimensional volume of a rectangle
01135 RTREE_TEMPLATE
01136 ELEMTYPEREAL RTREE_QUAL::RectVolume(Rect* a_rect)
01137 {
01138   ASSERT(a_rect);
01139   
01140   ELEMTYPEREAL volume = (ELEMTYPEREAL)1;
01141 
01142   for(int index=0; index<NUMDIMS; ++index)
01143   {
01144     volume *= a_rect->m_max[index] - a_rect->m_min[index];
01145   }
01146   
01147   ASSERT(volume >= (ELEMTYPEREAL)0);
01148   
01149   return volume;
01150 }
01151 
01152 
01153 // The exact volume of the bounding sphere for the given Rect
01154 RTREE_TEMPLATE
01155 ELEMTYPEREAL RTREE_QUAL::RectSphericalVolume(Rect* a_rect)
01156 {
01157   ASSERT(a_rect);
01158    
01159   ELEMTYPEREAL sumOfSquares = (ELEMTYPEREAL)0;
01160   ELEMTYPEREAL radius;
01161 
01162   for(int index=0; index < NUMDIMS; ++index) 
01163   {
01164     ELEMTYPEREAL halfExtent = ((ELEMTYPEREAL)a_rect->m_max[index] - (ELEMTYPEREAL)a_rect->m_min[index]) * 0.5f;
01165     sumOfSquares += halfExtent * halfExtent;
01166   }
01167 
01168   radius = (ELEMTYPEREAL)sqrt(sumOfSquares);
01169   
01170   // Pow maybe slow, so test for common dims like 2,3 and just use x*x, x*x*x.
01171   if(NUMDIMS == 3)
01172   {
01173     return (radius * radius * radius * m_unitSphereVolume);
01174   }
01175   else if(NUMDIMS == 2)
01176   {
01177     return (radius * radius * m_unitSphereVolume);
01178   }
01179   else
01180   {
01181     return (ELEMTYPEREAL)(pow(radius, NUMDIMS) * m_unitSphereVolume);
01182   }
01183 }
01184 
01185 
01186 // Use one of the methods to calculate retangle volume
01187 RTREE_TEMPLATE
01188 ELEMTYPEREAL RTREE_QUAL::CalcRectVolume(Rect* a_rect)
01189 {
01190 #ifdef RTREE_USE_SPHERICAL_VOLUME
01191   return RectSphericalVolume(a_rect); // Slower but helps certain merge cases
01192 #else // RTREE_USE_SPHERICAL_VOLUME
01193   return RectVolume(a_rect); // Faster but can cause poor merges
01194 #endif // RTREE_USE_SPHERICAL_VOLUME  
01195 }
01196 
01197 
01198 // Load branch buffer with branches from full node plus the extra branch.
01199 RTREE_TEMPLATE
01200 void RTREE_QUAL::GetBranches(Node* a_node, Branch* a_branch, PartitionVars* a_parVars)
01201 {
01202   ASSERT(a_node);
01203   ASSERT(a_branch);
01204 
01205   ASSERT(a_node->m_count == MAXNODES);
01206     
01207   // Load the branch buffer
01208   int index;
01209   for(index=0; index < MAXNODES; ++index)
01210   {
01211     a_parVars->m_branchBuf[index] = a_node->m_branch[index];
01212   }
01213   a_parVars->m_branchBuf[MAXNODES] = *a_branch;
01214   a_parVars->m_branchCount = MAXNODES + 1;
01215 
01216   // Calculate rect containing all in the set
01217   a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
01218   for(index=1; index < MAXNODES+1; ++index)
01219   {
01220     a_parVars->m_coverSplit = CombineRect(&a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect);
01221   }
01222   a_parVars->m_coverSplitArea = CalcRectVolume(&a_parVars->m_coverSplit);
01223 
01224   InitNode(a_node);
01225 }
01226 
01227 
01228 // Method #0 for choosing a partition:
01229 // As the seeds for the two groups, pick the two rects that would waste the
01230 // most area if covered by a single rectangle, i.e. evidently the worst pair
01231 // to have in the same group.
01232 // Of the remaining, one at a time is chosen to be put in one of the two groups.
01233 // The one chosen is the one with the greatest difference in area expansion
01234 // depending on which group - the rect most strongly attracted to one group
01235 // and repelled from the other.
01236 // If one group gets too full (more would force other group to violate min
01237 // fill requirement) then other group gets the rest.
01238 // These last are the ones that can go in either group most easily.
01239 RTREE_TEMPLATE
01240 void RTREE_QUAL::ChoosePartition(PartitionVars* a_parVars, int a_minFill)
01241 {
01242   ASSERT(a_parVars);
01243   
01244   ELEMTYPEREAL biggestDiff;
01245   int group, chosen, betterGroup;
01246   
01247   InitParVars(a_parVars, a_parVars->m_branchCount, a_minFill);
01248   PickSeeds(a_parVars);
01249 
01250   while (((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total)
01251        && (a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill))
01252        && (a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill)))
01253   {
01254     biggestDiff = (ELEMTYPEREAL) -1;
01255     for(int index=0; index<a_parVars->m_total; ++index)
01256     {
01257       if(!a_parVars->m_taken[index])
01258       {
01259         Rect* curRect = &a_parVars->m_branchBuf[index].m_rect;
01260         Rect rect0 = CombineRect(curRect, &a_parVars->m_cover[0]);
01261         Rect rect1 = CombineRect(curRect, &a_parVars->m_cover[1]);
01262         ELEMTYPEREAL growth0 = CalcRectVolume(&rect0) - a_parVars->m_area[0];
01263         ELEMTYPEREAL growth1 = CalcRectVolume(&rect1) - a_parVars->m_area[1];
01264         ELEMTYPEREAL diff = growth1 - growth0;
01265         if(diff >= 0)
01266         {
01267           group = 0;
01268         }
01269         else
01270         {
01271           group = 1;
01272           diff = -diff;
01273         }
01274 
01275         if(diff > biggestDiff)
01276         {
01277           biggestDiff = diff;
01278           chosen = index;
01279           betterGroup = group;
01280         }
01281         else if((diff == biggestDiff) && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup]))
01282         {
01283           chosen = index;
01284           betterGroup = group;
01285         }
01286       }
01287     }
01288     Classify(chosen, betterGroup, a_parVars);
01289   }
01290 
01291   // If one group too full, put remaining rects in the other
01292   if((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total)
01293   {
01294     if(a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill)
01295     {
01296       group = 1;
01297     }
01298     else
01299     {
01300       group = 0;
01301     }
01302     for(int index=0; index<a_parVars->m_total; ++index)
01303     {
01304       if(!a_parVars->m_taken[index])
01305       {
01306         Classify(index, group, a_parVars);
01307       }
01308     }
01309   }
01310 
01311   ASSERT((a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total);
01312   ASSERT((a_parVars->m_count[0] >= a_parVars->m_minFill) && 
01313         (a_parVars->m_count[1] >= a_parVars->m_minFill));
01314 }
01315 
01316 
01317 // Copy branches from the buffer into two nodes according to the partition.
01318 RTREE_TEMPLATE
01319 void RTREE_QUAL::LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars)
01320 {
01321   ASSERT(a_nodeA);
01322   ASSERT(a_nodeB);
01323   ASSERT(a_parVars);
01324 
01325   for(int index=0; index < a_parVars->m_total; ++index)
01326   {
01327     ASSERT(a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1);
01328     
01329     if(a_parVars->m_partition[index] == 0)
01330     {
01331       AddBranch(&a_parVars->m_branchBuf[index], a_nodeA, NULL);
01332     }
01333     else if(a_parVars->m_partition[index] == 1)
01334     {
01335       AddBranch(&a_parVars->m_branchBuf[index], a_nodeB, NULL);
01336     }
01337   }
01338 }
01339 
01340 
01341 // Initialize a PartitionVars structure.
01342 RTREE_TEMPLATE
01343 void RTREE_QUAL::InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill)
01344 {
01345   ASSERT(a_parVars);
01346 
01347   a_parVars->m_count[0] = a_parVars->m_count[1] = 0;
01348   a_parVars->m_area[0] = a_parVars->m_area[1] = (ELEMTYPEREAL)0;
01349   a_parVars->m_total = a_maxRects;
01350   a_parVars->m_minFill = a_minFill;
01351   for(int index=0; index < a_maxRects; ++index)
01352   {
01353     a_parVars->m_taken[index] = false;
01354     a_parVars->m_partition[index] = -1;
01355   }
01356 }
01357 
01358 
01359 RTREE_TEMPLATE
01360 void RTREE_QUAL::PickSeeds(PartitionVars* a_parVars)
01361 {
01362   int seed0, seed1;
01363   ELEMTYPEREAL worst, waste;
01364   ELEMTYPEREAL area[MAXNODES+1];
01365 
01366   for(int index=0; index<a_parVars->m_total; ++index)
01367   {
01368     area[index] = CalcRectVolume(&a_parVars->m_branchBuf[index].m_rect);
01369   }
01370 
01371   worst = -a_parVars->m_coverSplitArea - 1;
01372   for(int indexA=0; indexA < a_parVars->m_total-1; ++indexA)
01373   {
01374     for(int indexB = indexA+1; indexB < a_parVars->m_total; ++indexB)
01375     {
01376       Rect oneRect = CombineRect(&a_parVars->m_branchBuf[indexA].m_rect, &a_parVars->m_branchBuf[indexB].m_rect);
01377       waste = CalcRectVolume(&oneRect) - area[indexA] - area[indexB];
01378       if(waste > worst)
01379       {
01380         worst = waste;
01381         seed0 = indexA;
01382         seed1 = indexB;
01383       }
01384     }
01385   }
01386   Classify(seed0, 0, a_parVars);
01387   Classify(seed1, 1, a_parVars);
01388 }
01389 
01390 
01391 // Put a branch in one of the groups.
01392 RTREE_TEMPLATE
01393 void RTREE_QUAL::Classify(int a_index, int a_group, PartitionVars* a_parVars)
01394 {
01395   ASSERT(a_parVars);
01396   ASSERT(!a_parVars->m_taken[a_index]);
01397 
01398   a_parVars->m_partition[a_index] = a_group;
01399   a_parVars->m_taken[a_index] = true;
01400 
01401   if (a_parVars->m_count[a_group] == 0)
01402   {
01403     a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
01404   }
01405   else
01406   {
01407     a_parVars->m_cover[a_group] = CombineRect(&a_parVars->m_branchBuf[a_index].m_rect, &a_parVars->m_cover[a_group]);
01408   }
01409   a_parVars->m_area[a_group] = CalcRectVolume(&a_parVars->m_cover[a_group]);
01410   ++a_parVars->m_count[a_group];
01411 }
01412 
01413 
01414 // Delete a data rectangle from an index structure.
01415 // Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root node.
01416 // Returns 1 if record not found, 0 if success.
01417 // RemoveRect provides for eliminating the root.
01418 RTREE_TEMPLATE
01419 bool RTREE_QUAL::RemoveRect(Rect* a_rect, const DATATYPE& a_id, Node** a_root)
01420 {
01421   ASSERT(a_rect && a_root);
01422   ASSERT(*a_root);
01423 
01424   Node* tempNode;
01425   ListNode* reInsertList = NULL;
01426 
01427   if(!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList))
01428   {
01429     // Found and deleted a data item
01430     // Reinsert any branches from eliminated nodes
01431     while(reInsertList)
01432     {
01433       tempNode = reInsertList->m_node;
01434 
01435       for(int index = 0; index < tempNode->m_count; ++index)
01436       {
01437         InsertRect(&(tempNode->m_branch[index].m_rect),
01438                    tempNode->m_branch[index].m_data,
01439                    a_root,
01440                    tempNode->m_level);
01441       }
01442       
01443       ListNode* remLNode = reInsertList;
01444       reInsertList = reInsertList->m_next;
01445       
01446       FreeNode(remLNode->m_node);
01447       FreeListNode(remLNode);
01448     }
01449     
01450     // Check for redundant root (not leaf, 1 child) and eliminate
01451     if((*a_root)->m_count == 1 && (*a_root)->IsInternalNode())
01452     {
01453       tempNode = (*a_root)->m_branch[0].m_child;
01454       
01455       ASSERT(tempNode);
01456       FreeNode(*a_root);
01457       *a_root = tempNode;
01458     }
01459     return false;
01460   }
01461   else
01462   {
01463     return true;
01464   }
01465 }
01466 
01467 
01468 // Delete a rectangle from non-root part of an index structure.
01469 // Called by RemoveRect.  Descends tree recursively,
01470 // merges branches on the way back up.
01471 // Returns 1 if record not found, 0 if success.
01472 RTREE_TEMPLATE
01473 bool RTREE_QUAL::RemoveRectRec(Rect* a_rect, const DATATYPE& a_id, Node* a_node, ListNode** a_listNode)
01474 {
01475   ASSERT(a_rect && a_node && a_listNode);
01476   ASSERT(a_node->m_level >= 0);
01477 
01478   if(a_node->IsInternalNode())  // not a leaf node
01479   {
01480     for(int index = 0; index < a_node->m_count; ++index)
01481     {
01482       if(Overlap(a_rect, &(a_node->m_branch[index].m_rect)))
01483       {
01484         if(!RemoveRectRec(a_rect, a_id, a_node->m_branch[index].m_child, a_listNode))
01485         {
01486           if(a_node->m_branch[index].m_child->m_count >= MINNODES)
01487           {
01488             // child removed, just resize parent rect
01489             a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child);
01490           }
01491           else
01492           {
01493             // child removed, not enough entries in node, eliminate node
01494             ReInsert(a_node->m_branch[index].m_child, a_listNode);
01495             DisconnectBranch(a_node, index); // Must return after this call as count has changed
01496           }
01497           return false;
01498         }
01499       }
01500     }
01501     return true;
01502   }
01503   else // A leaf node
01504   {
01505     for(int index = 0; index < a_node->m_count; ++index)
01506     {
01507       if(a_node->m_branch[index].m_child == (Node*)a_id)
01508       {
01509         DisconnectBranch(a_node, index); // Must return after this call as count has changed
01510         return false;
01511       }
01512     }
01513     return true;
01514   }
01515 }
01516 
01517 
01518 // Decide whether two rectangles overlap.
01519 RTREE_TEMPLATE
01520 bool RTREE_QUAL::Overlap(Rect* a_rectA, Rect* a_rectB)
01521 {
01522   ASSERT(a_rectA && a_rectB);
01523 
01524   for(int index=0; index < NUMDIMS; ++index)
01525   {
01526     if (a_rectA->m_min[index] > a_rectB->m_max[index] ||
01527         a_rectB->m_min[index] > a_rectA->m_max[index])
01528     {
01529       return false;
01530     }
01531   }
01532   return true;
01533 }
01534 
01535 
01536 // Add a node to the reinsertion list.  All its branches will later
01537 // be reinserted into the index structure.
01538 RTREE_TEMPLATE
01539 void RTREE_QUAL::ReInsert(Node* a_node, ListNode** a_listNode)
01540 {
01541   ListNode* newListNode;
01542 
01543   newListNode = AllocListNode();
01544   newListNode->m_node = a_node;
01545   newListNode->m_next = *a_listNode;
01546   *a_listNode = newListNode;
01547 }
01548 
01549 
01550 
01551 // Search in an index tree or subtree for all data retangles that overlap the argument rectangle.
01552 RTREE_TEMPLATE
01553 bool RTREE_QUAL::Search(Node* a_node, Rect* a_rect, int& a_foundCount, const CONTEXT &c)
01554 {
01555   ASSERT(a_node);
01556   ASSERT(a_node->m_level >= 0);
01557   ASSERT(a_rect);
01558 
01559   if(a_node->IsInternalNode()) // This is an internal node in the tree
01560   {
01561     for(int index=0; index < a_node->m_count; ++index)
01562     {
01563       if(Overlap(a_rect, &a_node->m_branch[index].m_rect))
01564       {
01565         if(!Search(a_node->m_branch[index].m_child, a_rect, a_foundCount, c))
01566         {
01567           return false; // Don't continue searching
01568         }
01569       }
01570     }
01571   }
01572   else // This is a leaf node
01573   {
01574     for(int index=0; index < a_node->m_count; ++index)
01575     {
01576       if(Overlap(a_rect, &a_node->m_branch[index].m_rect))
01577       {
01578         DATATYPE& id = a_node->m_branch[index].m_data;
01579         
01580         // NOTE: There are different ways to return results.  Here's where to modify
01581 /*        if(&a_resultCallback)
01582         {*/
01583           ++a_foundCount;
01584           (id->*myOperation)(c);
01585           /*
01586           if(!a_resultCallback(id, a_context))
01587           {
01588             return false; // Don't continue searching
01589           }
01590           */
01591 //        }
01592       }
01593     }
01594   }
01595 
01596   return true; // Continue searching
01597 }
01598 
01599 
01600 
01601 
01602 #undef RTREE_TEMPLATE
01603 #undef RTREE_QUAL
01604 
01605 #endif //RTREE_H
01606 
01607 
01608 
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines