Branch data Line data Source code
1 : : //
2 : : // RTree.h
3 : : //
4 : : #ifndef RTREE_H
5 : : #define RTREE_H
6 : :
7 : : #include <cassert>
8 : : #include <algorithm>
9 : : #include <functional>
10 : : #include <type_traits>
11 : : #include <cmath>
12 : :
13 : : #define RTreeAssert assert // RTree uses RTreeAssert( condition )
14 : :
15 : :
16 : : #define RTREE_DONT_USE_MEMPOOLS // This version does not contain a fixed memory allocator, fill in lines with EXAMPLE to implement one.
17 : : #define RTREE_USE_SPHERICAL_VOLUME // Better split classification, may be slower on some systems
18 : :
19 : :
20 : : // Because there is not stream support, this is a quick and dirty file I/O helper.
21 : : // Users will likely replace its usage with a Stream implementation from their favorite API.
22 : : class RTFileStream {
23 : : public:
24 : : RTFileStream() = default;
25 : :
26 : : ~RTFileStream() {
27 : : Close();
28 : : }
29 : :
30 : : bool OpenRead(const char* a_fileName) {
31 : : m_file = fopen(a_fileName, "rb");
32 : : if (!m_file) {
33 : : return false;
34 : : }
35 : : return true;
36 : : }
37 : :
38 : : bool OpenWrite(const char* a_fileName) {
39 : : m_file = fopen(a_fileName, "wb");
40 : : if (!m_file) {
41 : : return false;
42 : : }
43 : : return true;
44 : : }
45 : :
46 : : void Close() {
47 : : if (m_file) {
48 : : fclose(m_file);
49 : : m_file = nullptr;
50 : : }
51 : : }
52 : :
53 : : template< typename TYPE >
54 : : size_t Write(const TYPE& a_value) {
55 : : RTreeAssert(m_file);
56 : : return fwrite((void*)&a_value, sizeof(a_value), 1, m_file);
57 : : }
58 : :
59 : : template< typename TYPE >
60 : : size_t WriteArray(const TYPE* a_array, int a_count) {
61 : : RTreeAssert(m_file);
62 : : return fwrite((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
63 : : }
64 : :
65 : : template< typename TYPE >
66 : : size_t Read(TYPE& a_value) {
67 : : RTreeAssert(m_file);
68 : : return fread((void*)&a_value, sizeof(a_value), 1, m_file);
69 : : }
70 : :
71 : : template< typename TYPE >
72 : : size_t ReadArray(TYPE* a_array, int a_count) {
73 : : RTreeAssert(m_file);
74 : : return fread((void*)a_array, sizeof(TYPE) * a_count, 1, m_file);
75 : : }
76 : :
77 : : private:
78 : : FILE* m_file = nullptr;
79 : : };
80 : :
81 : :
82 : : /// \class RTree
83 : : /// Implementation of RTree, a multidimensional bounding rectangle tree.
84 : : /// Example usage: For a 3-dimensional tree use RTree<Object*, float, 3> myTree;
85 : : ///
86 : : /// This modified, templated C++ version by Greg Douglas at Auran (http://www.auran.com)
87 : : ///
88 : : /// _DataType Referenced data, should be int, void*, obj* etc. no larger than sizeof<void*> and simple type
89 : : /// _ElementType Type of element such as int or float
90 : : /// _NumDimensions Number of dimensions such as 2 or 3
91 : : /// _ElementTypeReal Type of element that allows fractional and large values such as float or double, for use in volume calcs
92 : : ///
93 : : /// NOTES: Inserting and removing data requires the knowledge of its constant Minimal Bounding Rectangle.
94 : : /// This version uses new/delete for nodes, I recommend using a fixed size allocator for efficiency.
95 : : /// Instead of using a callback function for returned results, I recommend and efficient pre-sized, grow-only memory
96 : : /// array similar to MFC CArray or STL Vector for returning search query result.
97 : : ///
98 : : template<class _DataType, class _ElementType, int _NumDimensions,
99 : : class _ElementTypeReal = _ElementType, int _MaxNodeCount = 8, int _MinNodeCount = _MaxNodeCount / 2>
100 : : class RTree
101 : : {
102 : : static_assert(1 < _NumDimensions, "_NumDimensions must be larger than 1");
103 : : static_assert(0 < _MinNodeCount, "_MinNodeCount must be larger than 0");
104 : : static_assert(_MinNodeCount < _MaxNodeCount, "_MaxNodeCount must be larger than _MinNodeCount");
105 : : static_assert(std::is_floating_point<_ElementTypeReal>::value, "_ElementTypeReal must be floating point type");
106 : : protected:
107 : :
108 : : struct Node; // Fwd decl. Used by other internal structs and iterator
109 : :
110 : : public:
111 : : using DataType = _DataType;
112 : : using ElementType = _ElementType;
113 : : using ElementTypeReal = _ElementTypeReal;
114 : : using Element = ElementType[_NumDimensions];
115 : :
116 : : constexpr static const int kNumDimensions = _NumDimensions;
117 : : constexpr static const int kMaxNodeCount = _MaxNodeCount;
118 : : constexpr static const int kMinNodeCount = _MinNodeCount;
119 : :
120 : : template<typename ValueType>
121 : : constexpr static ElementType CastElementType(const ValueType val) noexcept {
122 : : return static_cast<ElementType>(val);
123 : : }
124 : : template<typename ValueType>
125 : 0 : constexpr static ElementTypeReal CastElementTypeReal(const ValueType val) noexcept {
126 : 0 : return static_cast<ElementTypeReal>(val);
127 : : }
128 : : constexpr static const ElementTypeReal kElementTypeRealOne = CastElementTypeReal(1.0);
129 : : constexpr static const ElementTypeReal kElementTypeRealZero = CastElementTypeReal(0.0);
130 : :
131 : 0 : RTree() {
132 : : // Precomputed volumes of the unit spheres for the first few dimensions
133 : 0 : constexpr const ElementTypeReal kUnitSphereVolumes[] = {
134 : : CastElementTypeReal(0.000000), CastElementTypeReal(2.000000), CastElementTypeReal(3.141593), // Dimension 0,1,2
135 : : CastElementTypeReal(4.188790), CastElementTypeReal(4.934802), CastElementTypeReal(5.263789), // Dimension 3,4,5
136 : : CastElementTypeReal(5.167713), CastElementTypeReal(4.724766), CastElementTypeReal(4.058712), // Dimension 6,7,8
137 : : CastElementTypeReal(3.298509), CastElementTypeReal(2.550164), CastElementTypeReal(1.884104), // Dimension 9,10,11
138 : : CastElementTypeReal(1.335263), CastElementTypeReal(0.910629), CastElementTypeReal(0.599265), // Dimension 12,13,14
139 : : CastElementTypeReal(0.381443), CastElementTypeReal(0.235331), CastElementTypeReal(0.140981), // Dimension 15,16,17
140 : : CastElementTypeReal(0.082146), CastElementTypeReal(0.046622), CastElementTypeReal(0.025807), // Dimension 18,19,20
141 : : };
142 : :
143 : 0 : this->m_root = this->AllocNode();
144 : 0 : this->m_root->m_level = 0;
145 : 0 : this->m_unitSphereVolume = kUnitSphereVolumes[kNumDimensions];
146 : 0 : }
147 : :
148 : : RTree(const RTree& other) : RTree() {
149 : : this->CopyRec(this->m_root, other.m_root);
150 : : }
151 : :
152 : 0 : ~RTree() {
153 : 0 : this->Reset();
154 : 0 : }
155 : :
156 : : /// Insert entry
157 : : /// \param a_min Min of bounding rect
158 : : /// \param a_max Max of bounding rect
159 : : /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed.
160 : 0 : void Insert(const Element& a_min, const Element& a_max, const DataType& a_dataId) {
161 : : #ifdef _DEBUG
162 : : for (int index = 0; index < kNumDimensions; ++index) {
163 : : RTreeAssert(a_min[index] <= a_max[index]);
164 : : }
165 : : #endif //_DEBUG
166 : :
167 : 0 : Branch branch;
168 : 0 : branch.m_data = a_dataId;
169 : 0 : branch.m_child = nullptr;
170 : :
171 : 0 : for (int axis = 0; axis < kNumDimensions; ++axis) {
172 : 0 : branch.m_rect.m_min[axis] = a_min[axis];
173 : 0 : branch.m_rect.m_max[axis] = a_max[axis];
174 : 0 : }
175 : :
176 : 0 : InsertRect(branch, &m_root, 0);
177 : 0 : }
178 : :
179 : : /// Remove entry
180 : : /// \param a_min Min of bounding rect
181 : : /// \param a_max Max of bounding rect
182 : : /// \param a_dataId Positive Id of data. Maybe zero, but negative numbers not allowed.
183 : 0 : void Remove(const Element& a_min, const Element& a_max, const DataType& a_dataId) {
184 : : #ifdef _DEBUG
185 : : for (int index = 0; index < kNumDimensions; ++index) {
186 : : RTreeAssert(a_min[index] <= a_max[index]);
187 : : }
188 : : #endif //_DEBUG
189 : :
190 : 0 : Rect rect;
191 : :
192 : 0 : for (int axis = 0; axis < kNumDimensions; ++axis) {
193 : 0 : rect.m_min[axis] = a_min[axis];
194 : 0 : rect.m_max[axis] = a_max[axis];
195 : 0 : }
196 : :
197 : 0 : RemoveRect(&rect, a_dataId, &m_root);
198 : 0 : }
199 : :
200 : : /// Find all within search rectangle
201 : : /// \param a_min Min of search bounding rect
202 : : /// \param a_max Max of search bounding rect
203 : : /// \param a_searchResult Search result array. Caller should set grow size. Function will reset, not append to array.
204 : : /// \param a_resultCallback Callback function to return result. Callback should return 'true' to continue searching
205 : : /// \param a_context User context to pass as parameter to a_resultCallback
206 : : /// \return Returns the number of entries found
207 : 0 : int Search(const Element& a_min, const Element& a_max, std::function<bool(const DataType&)> callback) const {
208 : : #ifdef _DEBUG
209 : : for (int index = 0; index < kNumDimensions; ++index) {
210 : : RTreeAssert(a_min[index] <= a_max[index]);
211 : : }
212 : : #endif //_DEBUG
213 : :
214 : 0 : Rect rect;
215 : :
216 : 0 : for (int axis = 0; axis < kNumDimensions; ++axis) {
217 : 0 : rect.m_min[axis] = a_min[axis];
218 : 0 : rect.m_max[axis] = a_max[axis];
219 : 0 : }
220 : :
221 : : // NOTE: May want to return search result another way, perhaps returning the number of found elements here.
222 : :
223 : 0 : int foundCount = 0;
224 : 0 : Search(m_root, &rect, foundCount, callback);
225 : :
226 : 0 : return foundCount;
227 : 0 : }
228 : :
229 : : /// Remove all entries from tree
230 : : void RemoveAll() {
231 : :
232 : : // Delete all existing nodes
233 : : Reset();
234 : :
235 : : m_root = AllocNode();
236 : : m_root->m_level = 0;
237 : : }
238 : :
239 : : /// Count the data elements in this container. This is slow as no internal counter is maintained.
240 : : int Count() const {
241 : :
242 : : int count = 0;
243 : : CountRec(m_root, count);
244 : :
245 : : return count;
246 : : }
247 : :
248 : : /// Load tree contents from file
249 : : bool Load(const char* a_fileName) {
250 : : RemoveAll(); // Clear existing tree
251 : :
252 : : RTFileStream stream;
253 : : if (!stream.OpenRead(a_fileName)) {
254 : : return false;
255 : : }
256 : :
257 : : bool result = Load(stream);
258 : :
259 : : stream.Close();
260 : :
261 : : return result;
262 : : }
263 : : /// Load tree contents from stream
264 : : bool Load(RTFileStream& a_stream) {
265 : : // Write some kind of header
266 : : int _dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
267 : : int _dataSize = sizeof(DataType);
268 : : int _dataNumDims = kNumDimensions;
269 : : int _dataElemSize = sizeof(ElementType);
270 : : int _dataElemRealSize = sizeof(ElementTypeReal);
271 : : int _dataMaxNodes = kMaxNodeCount;
272 : : int _dataMinNodes = kMinNodeCount;
273 : :
274 : : int dataFileId = 0;
275 : : int dataSize = 0;
276 : : int dataNumDims = 0;
277 : : int dataElemSize = 0;
278 : : int dataElemRealSize = 0;
279 : : int dataMaxNodes = 0;
280 : : int dataMinNodes = 0;
281 : :
282 : : a_stream.Read(dataFileId);
283 : : a_stream.Read(dataSize);
284 : : a_stream.Read(dataNumDims);
285 : : a_stream.Read(dataElemSize);
286 : : a_stream.Read(dataElemRealSize);
287 : : a_stream.Read(dataMaxNodes);
288 : : a_stream.Read(dataMinNodes);
289 : :
290 : : bool result = false;
291 : :
292 : : // Test if header was valid and compatible
293 : : if ((dataFileId == _dataFileId)
294 : : && (dataSize == _dataSize)
295 : : && (dataNumDims == _dataNumDims)
296 : : && (dataElemSize == _dataElemSize)
297 : : && (dataElemRealSize == _dataElemRealSize)
298 : : && (dataMaxNodes == _dataMaxNodes)
299 : : && (dataMinNodes == _dataMinNodes)
300 : : ) {
301 : : // Recursively load tree
302 : : result = LoadRec(m_root, a_stream);
303 : : }
304 : :
305 : : return result;
306 : : }
307 : :
308 : :
309 : : /// Save tree contents to file
310 : : bool Save(const char* a_fileName) {
311 : :
312 : : RTFileStream stream;
313 : : if (!stream.OpenWrite(a_fileName)) {
314 : : return false;
315 : : }
316 : :
317 : : bool result = Save(stream);
318 : :
319 : : stream.Close();
320 : :
321 : : return result;
322 : : }
323 : : /// Save tree contents to stream
324 : : bool Save(RTFileStream& a_stream) {
325 : :
326 : : // Write some kind of header
327 : : int dataFileId = ('R' << 0) | ('T' << 8) | ('R' << 16) | ('E' << 24);
328 : : int dataSize = sizeof(DataType);
329 : : int dataNumDims = kNumDimensions;
330 : : int dataElemSize = sizeof(ElementType);
331 : : int dataElemRealSize = sizeof(ElementTypeReal);
332 : : int dataMaxNodes = kMaxNodeCount;
333 : : int dataMinNodes = kMinNodeCount;
334 : :
335 : : a_stream.Write(dataFileId);
336 : : a_stream.Write(dataSize);
337 : : a_stream.Write(dataNumDims);
338 : : a_stream.Write(dataElemSize);
339 : : a_stream.Write(dataElemRealSize);
340 : : a_stream.Write(dataMaxNodes);
341 : : a_stream.Write(dataMinNodes);
342 : :
343 : : // Recursively save tree
344 : : bool result = SaveRec(m_root, a_stream);
345 : :
346 : : return result;
347 : : }
348 : :
349 : : /// Iterator is not remove safe.
350 : : class Iterator
351 : : {
352 : : private:
353 : :
354 : : constexpr static const int kMaxStackSize = 32; // Max stack size. Allows almost n^32 where n is number of branches in node
355 : :
356 : : struct StackElement
357 : : {
358 : : Node* m_node = nullptr;
359 : : int m_branchIndex = 0;
360 : : };
361 : :
362 : : public:
363 : :
364 : : Iterator() { Init(); }
365 : :
366 : : ~Iterator() = default;
367 : :
368 : : /// Is iterator invalid
369 : : bool IsNull() const noexcept { return (m_tos <= 0); }
370 : :
371 : : /// Is iterator pointing to valid data
372 : : bool IsNotNull() const noexcept { return (m_tos > 0); }
373 : :
374 : : /// Access the current data element. Caller must be sure iterator is not NULL first.
375 : : DataType& operator*()
376 : : {
377 : : RTreeAssert(IsNotNull());
378 : : auto& curTos = m_stack[m_tos - 1];
379 : : return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
380 : : }
381 : :
382 : : /// Access the current data element. Caller must be sure iterator is not NULL first.
383 : : const DataType& operator*() const
384 : : {
385 : : RTreeAssert(IsNotNull());
386 : : auto& curTos = m_stack[m_tos - 1];
387 : : return curTos.m_node->m_branch[curTos.m_branchIndex].m_data;
388 : : }
389 : :
390 : : /// Find the next data element
391 : : bool operator++() { return FindNextData(); }
392 : :
393 : : /// Get the bounds for this node
394 : : void GetBounds(Element& a_min, Element& a_max)
395 : : {
396 : : RTreeAssert(IsNotNull());
397 : : auto& curTos = m_stack[m_tos - 1];
398 : : auto& curBranch = curTos.m_node->m_branch[curTos.m_branchIndex];
399 : :
400 : : for (int index = 0; index < kNumDimensions; ++index)
401 : : {
402 : : a_min[index] = curBranch.m_rect.m_min[index];
403 : : a_max[index] = curBranch.m_rect.m_max[index];
404 : : }
405 : : }
406 : :
407 : : private:
408 : :
409 : : /// Reset iterator
410 : : void Init() { m_tos = 0; }
411 : :
412 : : /// Find the next data element in the tree (For internal use only)
413 : : bool FindNextData()
414 : : {
415 : : for (;;)
416 : : {
417 : : if (m_tos <= 0)
418 : : {
419 : : return false;
420 : : }
421 : : auto curTos = Pop(); // Copy stack top cause it may change as we use it
422 : :
423 : : if (curTos.m_node->IsLeaf())
424 : : {
425 : : // Keep walking through data while we can
426 : : if (curTos.m_branchIndex + 1 < curTos.m_node->m_count)
427 : : {
428 : : // There is more data, just point to the next one
429 : : Push(curTos.m_node, curTos.m_branchIndex + 1);
430 : : return true;
431 : : }
432 : : // No more data, so it will fall back to previous level
433 : : }
434 : : else
435 : : {
436 : : if (curTos.m_branchIndex + 1 < curTos.m_node->m_count)
437 : : {
438 : : // Push sibling on for future tree walk
439 : : // This is the 'fall back' node when we finish with the current level
440 : : Push(curTos.m_node, curTos.m_branchIndex + 1);
441 : : }
442 : : // Since cur node is not a leaf, push first of next level to get deeper into the tree
443 : : auto nextLevelnode = curTos.m_node->m_branch[curTos.m_branchIndex].m_child;
444 : : Push(nextLevelnode, 0);
445 : :
446 : : // If we pushed on a new leaf, exit as the data is ready at TOS
447 : : if (nextLevelnode->IsLeaf())
448 : : {
449 : : return true;
450 : : }
451 : : }
452 : : }
453 : : }
454 : :
455 : : /// Push node and branch onto iteration stack (For internal use only)
456 : : void Push(Node* a_node, int a_branchIndex)
457 : : {
458 : : m_stack[m_tos].m_node = a_node;
459 : : m_stack[m_tos].m_branchIndex = a_branchIndex;
460 : : ++m_tos;
461 : : RTreeAssert(m_tos <= kMaxStackSize);
462 : : }
463 : :
464 : : /// Pop element off iteration stack (For internal use only)
465 : : StackElement& Pop()
466 : : {
467 : : RTreeAssert(m_tos > 0);
468 : : --m_tos;
469 : : return m_stack[m_tos];
470 : : }
471 : :
472 : : StackElement m_stack[kMaxStackSize]; ///< Stack as we are doing iteration instead of recursion
473 : : int m_tos = 0; ///< Top Of Stack index
474 : :
475 : : friend class RTree; // Allow hiding of non-public functions while allowing manipulation by logical owner
476 : : };
477 : :
478 : : /// Get 'first' for iteration
479 : : void GetFirst(Iterator& a_it) const
480 : : {
481 : : a_it.Init();
482 : : auto first = m_root;
483 : : while (first)
484 : : {
485 : : if (first->IsInternalNode() && first->m_count > 1)
486 : : {
487 : : a_it.Push(first, 1); // Descend sibling branch later
488 : : }
489 : : else if (first->IsLeaf())
490 : : {
491 : : if (first->m_count)
492 : : {
493 : : a_it.Push(first, 0);
494 : : }
495 : : break;
496 : : }
497 : : first = first->m_branch[0].m_child;
498 : : }
499 : : }
500 : :
501 : : /// Get Next for iteration
502 : : void GetNext(Iterator& a_it) const { ++a_it; }
503 : :
504 : : /// Is iterator NULL, or at end?
505 : : bool IsNull(const Iterator& a_it) const { return a_it.IsNull(); }
506 : :
507 : : /// Get object at iterator position
508 : : DataType& GetAt(Iterator& a_it) { return *a_it; }
509 : :
510 : : protected:
511 : :
512 : : /// Minimal bounding rectangle (n-dimensional)
513 : 0 : struct Rect
514 : : {
515 : 0 : ElementType m_min[kNumDimensions] = { 0, }; ///< Min dimensions of bounding box
516 : 0 : ElementType m_max[kNumDimensions] = { 0, }; ///< Max dimensions of bounding box
517 : : };
518 : :
519 : : /// May be data or may be another subtree
520 : : /// The parents level determines this.
521 : : /// If the parents level is 0, then this is data
522 : 0 : struct Branch
523 : : {
524 : : Rect m_rect; ///< Bounds
525 : 0 : Node* m_child = nullptr; ///< Child node
526 : : DataType m_data; ///< Data Id
527 : : };
528 : :
529 : : /// Node for each branch level
530 : 0 : struct Node
531 : : {
532 : 0 : bool IsInternalNode() const noexcept { return (m_level > 0); } // Not a leaf, but a internal node
533 : : bool IsLeaf() const noexcept { return (m_level == 0); } // A leaf, contains data
534 : :
535 : 0 : int m_count = 0; ///< Count
536 : 0 : int m_level = 0; ///< Leaf is zero, others positive
537 : : Branch m_branch[_MaxNodeCount]; ///< Branch
538 : : };
539 : :
540 : : /// A link list of nodes for reinsertion after a delete operation
541 : 0 : struct ListNode
542 : : {
543 : 0 : ListNode* m_next = nullptr; ///< Next in list
544 : 0 : Node* m_node = nullptr; ///< Node
545 : : };
546 : :
547 : : /// Variables for finding a split partition
548 : 0 : struct PartitionVars
549 : : {
550 : : constexpr static const int kNotTaken = -1; // indicates that position
551 : :
552 : 0 : int m_partition[_MaxNodeCount + 1] = { 0, };
553 : 0 : int m_total = 0;
554 : 0 : int m_minFill = 0;
555 : 0 : int m_count[2] = { 0, };
556 : : Rect m_cover[2];
557 : 0 : ElementTypeReal m_area[2] = { kElementTypeRealZero, };
558 : :
559 : : Branch m_branchBuf[_MaxNodeCount + 1];
560 : 0 : int m_branchCount = 0;
561 : : Rect m_coverSplit;
562 : 0 : ElementTypeReal m_coverSplitArea = kElementTypeRealZero;
563 : : };
564 : :
565 : 0 : Node* AllocNode() {
566 : : Node* newNode;
567 : : #ifdef RTREE_DONT_USE_MEMPOOLS
568 : 0 : newNode = new Node;
569 : : #else // RTREE_DONT_USE_MEMPOOLS
570 : : // EXAMPLE
571 : : #endif // RTREE_DONT_USE_MEMPOOLS
572 : 0 : InitNode(newNode);
573 : 0 : return newNode;
574 : : }
575 : :
576 : 0 : void FreeNode(Node* a_node) {
577 : 0 : RTreeAssert(a_node);
578 : :
579 : : #ifdef RTREE_DONT_USE_MEMPOOLS
580 : 0 : delete a_node;
581 : : #else // RTREE_DONT_USE_MEMPOOLS
582 : : // EXAMPLE
583 : : #endif // RTREE_DONT_USE_MEMPOOLS
584 : 0 : }
585 : :
586 : 0 : void InitNode(Node* a_node) {
587 : 0 : a_node->m_count = 0;
588 : 0 : a_node->m_level = -1;
589 : 0 : }
590 : :
591 : : void InitRect(Rect* a_rect) {
592 : : for (int index = 0; index < kNumDimensions; ++index) {
593 : : a_rect->m_min[index] = CastElementType(0);
594 : : a_rect->m_max[index] = CastElementType(0);
595 : : }
596 : : }
597 : :
598 : : // Inserts a new data rectangle into the index structure.
599 : : // Recursively descends tree, propagates splits back up.
600 : : // Returns 0 if node was not split. Old node updated.
601 : : // If node was split, returns 1 and sets the pointer pointed to by
602 : : // new_node to point to the new node. Old node updated to become one of two.
603 : : // The level argument specifies the number of steps up from the leaf
604 : : // level to insert; e.g. a data rectangle goes in at level = 0.
605 : 0 : bool InsertRectRec(const Branch& a_branch, Node* a_node, Node** a_newNode, int a_level) {
606 : 0 : RTreeAssert(a_node && a_newNode);
607 : 0 : RTreeAssert(a_level >= 0 && a_level <= a_node->m_level);
608 : :
609 : : // recurse until we reach the correct level for the new record. data records
610 : : // will always be called with a_level == 0 (leaf)
611 : 0 : if (a_node->m_level > a_level) {
612 : : // Still above level for insertion, go down tree recursively
613 : : Node* otherNode;
614 : :
615 : : // find the optimal branch for this record
616 : 0 : int index = PickBranch(&a_branch.m_rect, a_node);
617 : :
618 : : // recursively insert this record into the picked branch
619 : 0 : bool childWasSplit = InsertRectRec(a_branch, a_node->m_branch[index].m_child, &otherNode, a_level);
620 : :
621 : 0 : if (!childWasSplit) {
622 : : // Child was not split. Merge the bounding box of the new record with the
623 : : // existing bounding box
624 : 0 : a_node->m_branch[index].m_rect = CombineRect(&a_branch.m_rect, &(a_node->m_branch[index].m_rect));
625 : 0 : return false;
626 : : } else {
627 : : // Child was split. The old branches are now re-partitioned to two nodes
628 : : // so we have to re-calculate the bounding boxes of each node
629 : 0 : a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child);
630 : 0 : Branch branch;
631 : 0 : branch.m_child = otherNode;
632 : 0 : branch.m_rect = NodeCover(otherNode);
633 : :
634 : : // The old node is already a child of a_node. Now add the newly-created
635 : : // node to a_node as well. a_node might be split because of that.
636 : 0 : return AddBranch(&branch, a_node, a_newNode);
637 : : }
638 : 0 : } else if (a_node->m_level == a_level) {
639 : : // We have reached level for insertion. Add rect, split if necessary
640 : 0 : return AddBranch(&a_branch, a_node, a_newNode);
641 : : } else {
642 : : // Should never occur
643 : 0 : RTreeAssert(0);
644 : : return false;
645 : : }
646 : 0 : }
647 : :
648 : : // Insert a data rectangle into an index structure.
649 : : // InsertRect provides for splitting the root;
650 : : // returns 1 if root was split, 0 if it was not.
651 : : // The level argument specifies the number of steps up from the leaf
652 : : // level to insert; e.g. a data rectangle goes in at level = 0.
653 : : // InsertRect2 does the recursion.
654 : 0 : bool InsertRect(const Branch& a_branch, Node** a_root, int a_level) {
655 : :
656 : 0 : RTreeAssert(a_root);
657 : 0 : RTreeAssert(a_level >= 0 && a_level <= (*a_root)->m_level);
658 : : #ifdef _DEBUG
659 : : for (int index = 0; index < kNumDimensions; ++index) {
660 : : RTreeAssert(a_branch.m_rect.m_min[index] <= a_branch.m_rect.m_max[index]);
661 : : }
662 : : #endif //_DEBUG
663 : :
664 : : Node* newNode;
665 : :
666 : 0 : if (InsertRectRec(a_branch, *a_root, &newNode, a_level)) // Root split
667 : : {
668 : : // Grow tree taller and new root
669 : 0 : Node* newRoot = AllocNode();
670 : 0 : newRoot->m_level = (*a_root)->m_level + 1;
671 : :
672 : 0 : Branch branch;
673 : :
674 : : // add old root node as a child of the new root
675 : 0 : branch.m_rect = NodeCover(*a_root);
676 : 0 : branch.m_child = *a_root;
677 : 0 : AddBranch(&branch, newRoot, nullptr);
678 : :
679 : : // add the split node as a child of the new root
680 : 0 : branch.m_rect = NodeCover(newNode);
681 : 0 : branch.m_child = newNode;
682 : 0 : AddBranch(&branch, newRoot, nullptr);
683 : :
684 : : // set the new root as the root node
685 : 0 : *a_root = newRoot;
686 : :
687 : 0 : return true;
688 : : }
689 : :
690 : 0 : return false;
691 : 0 : }
692 : :
693 : : // Find the smallest rectangle that includes all rectangles in branches of a node.
694 : 0 : Rect NodeCover(Node* a_node) {
695 : 0 : RTreeAssert(a_node);
696 : :
697 : 0 : Rect rect = a_node->m_branch[0].m_rect;
698 : 0 : for (int index = 1; index < a_node->m_count; ++index) {
699 : 0 : rect = CombineRect(&rect, &(a_node->m_branch[index].m_rect));
700 : 0 : }
701 : :
702 : 0 : return rect;
703 : : }
704 : :
705 : : // Add a branch to a node. Split the node if necessary.
706 : : // Returns 0 if node not split. Old node updated.
707 : : // Returns 1 if node split, sets *new_node to address of new node.
708 : : // Old node updated, becomes one of two.
709 : 0 : bool AddBranch(const Branch* a_branch, Node* a_node, Node** a_newNode) {
710 : :
711 : 0 : RTreeAssert(a_branch);
712 : 0 : RTreeAssert(a_node);
713 : :
714 : 0 : if (a_node->m_count < _MaxNodeCount) // Split won't be necessary
715 : : {
716 : 0 : a_node->m_branch[a_node->m_count] = *a_branch;
717 : 0 : ++a_node->m_count;
718 : :
719 : 0 : return false;
720 : : } else {
721 : 0 : RTreeAssert(a_newNode);
722 : :
723 : 0 : SplitNode(a_node, a_branch, a_newNode);
724 : 0 : return true;
725 : : }
726 : 0 : }
727 : :
728 : : // Disconnect a dependent node.
729 : : // Caller must return (or stop using iteration index) after this as count has changed
730 : 0 : void DisconnectBranch(Node* a_node, int a_index) {
731 : 0 : RTreeAssert(a_node && (a_index >= 0) && (a_index < _MaxNodeCount));
732 : 0 : RTreeAssert(a_node->m_count > 0);
733 : :
734 : : // Remove element by swapping with the last element to prevent gaps in array
735 : 0 : a_node->m_branch[a_index] = a_node->m_branch[a_node->m_count - 1];
736 : :
737 : 0 : --a_node->m_count;
738 : 0 : }
739 : :
740 : : // Pick a branch. Pick the one that will need the smallest increase
741 : : // in area to accomodate the new rectangle. This will result in the
742 : : // least total area for the covering rectangles in the current node.
743 : : // In case of a tie, pick the one which was smaller before, to get
744 : : // the best resolution when searching.
745 : 0 : int PickBranch(const Rect* a_rect, Node* a_node) {
746 : 0 : RTreeAssert(a_rect && a_node);
747 : :
748 : 0 : bool firstTime = true;
749 : : ElementTypeReal increase;
750 : 0 : ElementTypeReal bestIncr = CastElementTypeReal(-1);
751 : : ElementTypeReal area;
752 : 0 : ElementTypeReal bestArea = CastElementTypeReal(-1);
753 : 0 : int best = 0;
754 : 0 : Rect tempRect;
755 : :
756 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
757 : 0 : Rect* curRect = &a_node->m_branch[index].m_rect;
758 : 0 : area = CalcRectVolume(curRect);
759 : 0 : tempRect = CombineRect(a_rect, curRect);
760 : 0 : increase = CalcRectVolume(&tempRect) - area;
761 : 0 : if ((increase < bestIncr) || firstTime) {
762 : 0 : best = index;
763 : 0 : bestArea = area;
764 : 0 : bestIncr = increase;
765 : 0 : firstTime = false;
766 : 0 : } else if ((increase == bestIncr) && (area < bestArea)) {
767 : 0 : best = index;
768 : 0 : bestArea = area;
769 : 0 : bestIncr = increase;
770 : 0 : }
771 : 0 : }
772 : 0 : return best;
773 : : }
774 : :
775 : : // Combine two rectangles into larger one containing both
776 : 0 : Rect CombineRect(const Rect* a_rectA, const Rect* a_rectB) {
777 : 0 : RTreeAssert(a_rectA && a_rectB);
778 : :
779 : 0 : Rect newRect;
780 : :
781 : 0 : for (int index = 0; index < kNumDimensions; ++index) {
782 : 0 : newRect.m_min[index] = (std::min)(a_rectA->m_min[index], a_rectB->m_min[index]);
783 : 0 : newRect.m_max[index] = (std::max)(a_rectA->m_max[index], a_rectB->m_max[index]);
784 : 0 : }
785 : :
786 : 0 : return newRect;
787 : : }
788 : :
789 : : // Split a node.
790 : : // Divides the nodes branches and the extra one between two nodes.
791 : : // Old node is one of the new ones, and one really new one is created.
792 : : // Tries more than one method for choosing a partition, uses best result.
793 : 0 : void SplitNode(Node* a_node, const Branch* a_branch, Node** a_newNode) {
794 : 0 : RTreeAssert(a_node);
795 : 0 : RTreeAssert(a_branch);
796 : :
797 : : // Could just use local here, but member or external is faster since it is reused
798 : 0 : PartitionVars localVars;
799 : 0 : PartitionVars* parVars = &localVars;
800 : :
801 : : // Load all the branches into a buffer, initialize old node
802 : 0 : GetBranches(a_node, a_branch, parVars);
803 : :
804 : : // Find partition
805 : 0 : ChoosePartition(parVars, _MinNodeCount);
806 : :
807 : : // Create a new node to hold (about) half of the branches
808 : 0 : *a_newNode = AllocNode();
809 : 0 : (*a_newNode)->m_level = a_node->m_level;
810 : :
811 : : // Put branches from buffer into 2 nodes according to the chosen partition
812 : 0 : a_node->m_count = 0;
813 : 0 : LoadNodes(a_node, *a_newNode, parVars);
814 : :
815 : 0 : RTreeAssert((a_node->m_count + (*a_newNode)->m_count) == parVars->m_total);
816 : 0 : }
817 : :
818 : : // The exact volume of the bounding sphere for the given Rect
819 : 0 : ElementTypeReal RectSphericalVolume(Rect* a_rect) {
820 : 0 : RTreeAssert(a_rect);
821 : :
822 : 0 : ElementTypeReal sumOfSquares = kElementTypeRealZero;
823 : :
824 : 0 : for (int index = 0; index < kNumDimensions; ++index) {
825 : 0 : const auto halfExtent = ((ElementTypeReal)a_rect->m_max[index] - (ElementTypeReal)a_rect->m_min[index]) * 0.5f;
826 : 0 : sumOfSquares += halfExtent * halfExtent;
827 : 0 : }
828 : :
829 : 0 : const auto radius = CastElementTypeReal(sqrt(sumOfSquares));
830 : :
831 : : // Pow maybe slow, so test for common dims like 2,3 and just use x*x, x*x*x.
832 : : if (kNumDimensions == 3) {
833 : : return (radius * radius * radius * m_unitSphereVolume);
834 : : } else if (kNumDimensions == 2) {
835 : 0 : return (radius * radius * m_unitSphereVolume);
836 : : } else {
837 : : return CastElementTypeReal(pow(radius, kNumDimensions) * m_unitSphereVolume);
838 : : }
839 : : }
840 : :
841 : : // Calculate the n-dimensional volume of a rectangle
842 : : ElementTypeReal RectVolume(Rect* a_rect) {
843 : : RTreeAssert(a_rect);
844 : :
845 : : auto volume = kElementTypeRealOne;
846 : :
847 : : for (int index = 0; index < kNumDimensions; ++index) {
848 : : volume *= a_rect->m_max[index] - a_rect->m_min[index];
849 : : }
850 : :
851 : : RTreeAssert(volume >= kElementTypeRealZero);
852 : :
853 : : return volume;
854 : : }
855 : :
856 : : // Use one of the methods to calculate retangle volume
857 : 0 : ElementTypeReal CalcRectVolume(Rect* a_rect) {
858 : : #ifdef RTREE_USE_SPHERICAL_VOLUME
859 : 0 : return RectSphericalVolume(a_rect); // Slower but helps certain merge cases
860 : : #else // RTREE_USE_SPHERICAL_VOLUME
861 : : return RectVolume(a_rect); // Faster but can cause poor merges
862 : : #endif // RTREE_USE_SPHERICAL_VOLUME
863 : : }
864 : :
865 : : // Load branch buffer with branches from full node plus the extra branch.
866 : 0 : void GetBranches(Node* a_node, const Branch* a_branch, PartitionVars* a_parVars) {
867 : 0 : RTreeAssert(a_node);
868 : 0 : RTreeAssert(a_branch);
869 : :
870 : 0 : RTreeAssert(a_node->m_count == _MaxNodeCount);
871 : :
872 : : // Load the branch buffer
873 : 0 : for (int index = 0; index < _MaxNodeCount; ++index) {
874 : 0 : a_parVars->m_branchBuf[index] = a_node->m_branch[index];
875 : 0 : }
876 : 0 : a_parVars->m_branchBuf[_MaxNodeCount] = *a_branch;
877 : 0 : a_parVars->m_branchCount = _MaxNodeCount + 1;
878 : :
879 : : // Calculate rect containing all in the set
880 : 0 : a_parVars->m_coverSplit = a_parVars->m_branchBuf[0].m_rect;
881 : 0 : for (int index = 1; index < _MaxNodeCount + 1; ++index) {
882 : 0 : a_parVars->m_coverSplit = CombineRect(&a_parVars->m_coverSplit, &a_parVars->m_branchBuf[index].m_rect);
883 : 0 : }
884 : 0 : a_parVars->m_coverSplitArea = CalcRectVolume(&a_parVars->m_coverSplit);
885 : 0 : }
886 : :
887 : : // Method #0 for choosing a partition:
888 : : // As the seeds for the two groups, pick the two rects that would waste the
889 : : // most area if covered by a single rectangle, i.e. evidently the worst pair
890 : : // to have in the same group.
891 : : // Of the remaining, one at a time is chosen to be put in one of the two groups.
892 : : // The one chosen is the one with the greatest difference in area expansion
893 : : // depending on which group - the rect most strongly attracted to one group
894 : : // and repelled from the other.
895 : : // If one group gets too full (more would force other group to violate min
896 : : // fill requirement) then other group gets the rest.
897 : : // These last are the ones that can go in either group most easily.
898 : 0 : void ChoosePartition(PartitionVars* a_parVars, int a_minFill) {
899 : 0 : RTreeAssert(a_parVars);
900 : :
901 : : ElementTypeReal biggestDiff;
902 : 0 : int group, chosen = 0, betterGroup = 0;
903 : :
904 : 0 : InitParVars(a_parVars, a_parVars->m_branchCount, a_minFill);
905 : 0 : PickSeeds(a_parVars);
906 : :
907 : 0 : while (((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total)
908 : 0 : && (a_parVars->m_count[0] < (a_parVars->m_total - a_parVars->m_minFill))
909 : 0 : && (a_parVars->m_count[1] < (a_parVars->m_total - a_parVars->m_minFill))) {
910 : 0 : biggestDiff = CastElementTypeReal(-1);
911 : 0 : for (int index = 0; index < a_parVars->m_total; ++index) {
912 : 0 : if (PartitionVars::kNotTaken == a_parVars->m_partition[index]) {
913 : 0 : Rect* curRect = &a_parVars->m_branchBuf[index].m_rect;
914 : 0 : Rect rect0 = CombineRect(curRect, &a_parVars->m_cover[0]);
915 : 0 : Rect rect1 = CombineRect(curRect, &a_parVars->m_cover[1]);
916 : 0 : ElementTypeReal growth0 = CalcRectVolume(&rect0) - a_parVars->m_area[0];
917 : 0 : ElementTypeReal growth1 = CalcRectVolume(&rect1) - a_parVars->m_area[1];
918 : 0 : ElementTypeReal diff = growth1 - growth0;
919 : 0 : if (diff >= 0) {
920 : 0 : group = 0;
921 : 0 : } else {
922 : 0 : group = 1;
923 : 0 : diff = -diff;
924 : : }
925 : :
926 : 0 : if (diff > biggestDiff) {
927 : 0 : biggestDiff = diff;
928 : 0 : chosen = index;
929 : 0 : betterGroup = group;
930 : 0 : } else if ((diff == biggestDiff) && (a_parVars->m_count[group] < a_parVars->m_count[betterGroup])) {
931 : 0 : chosen = index;
932 : 0 : betterGroup = group;
933 : 0 : }
934 : 0 : }
935 : 0 : }
936 : 0 : Classify(chosen, betterGroup, a_parVars);
937 : : }
938 : :
939 : : // If one group too full, put remaining rects in the other
940 : 0 : if ((a_parVars->m_count[0] + a_parVars->m_count[1]) < a_parVars->m_total) {
941 : 0 : if (a_parVars->m_count[0] >= a_parVars->m_total - a_parVars->m_minFill) {
942 : 0 : group = 1;
943 : 0 : } else {
944 : 0 : group = 0;
945 : : }
946 : 0 : for (int index = 0; index < a_parVars->m_total; ++index) {
947 : 0 : if (PartitionVars::kNotTaken == a_parVars->m_partition[index]) {
948 : 0 : Classify(index, group, a_parVars);
949 : 0 : }
950 : 0 : }
951 : 0 : }
952 : :
953 : 0 : RTreeAssert((a_parVars->m_count[0] + a_parVars->m_count[1]) == a_parVars->m_total);
954 : 0 : RTreeAssert((a_parVars->m_count[0] >= a_parVars->m_minFill) &&
955 : : (a_parVars->m_count[1] >= a_parVars->m_minFill));
956 : 0 : }
957 : :
958 : : // Copy branches from the buffer into two nodes according to the partition.
959 : 0 : void LoadNodes(Node* a_nodeA, Node* a_nodeB, PartitionVars* a_parVars) {
960 : 0 : RTreeAssert(a_nodeA);
961 : 0 : RTreeAssert(a_nodeB);
962 : 0 : RTreeAssert(a_parVars);
963 : :
964 : 0 : for (int index = 0; index < a_parVars->m_total; ++index) {
965 : 0 : RTreeAssert(a_parVars->m_partition[index] == 0 || a_parVars->m_partition[index] == 1);
966 : :
967 : 0 : int targetNodeIndex = a_parVars->m_partition[index];
968 : 0 : Node* targetNodes[] = { a_nodeA, a_nodeB };
969 : :
970 : : // It is assured that AddBranch here will not cause a node split.
971 : 0 : bool nodeWasSplit = AddBranch(&a_parVars->m_branchBuf[index], targetNodes[targetNodeIndex], nullptr);
972 : : (void)nodeWasSplit;
973 : 0 : RTreeAssert(!nodeWasSplit);
974 : 0 : }
975 : 0 : }
976 : :
977 : : // Initialize a PartitionVars structure.
978 : 0 : void InitParVars(PartitionVars* a_parVars, int a_maxRects, int a_minFill) {
979 : 0 : RTreeAssert(a_parVars);
980 : :
981 : 0 : a_parVars->m_count[0] = a_parVars->m_count[1] = 0;
982 : 0 : a_parVars->m_area[0] = a_parVars->m_area[1] = kElementTypeRealZero;
983 : 0 : a_parVars->m_total = a_maxRects;
984 : 0 : a_parVars->m_minFill = a_minFill;
985 : 0 : for (int index = 0; index < a_maxRects; ++index) {
986 : 0 : a_parVars->m_partition[index] = PartitionVars::kNotTaken;
987 : 0 : }
988 : 0 : }
989 : :
990 : 0 : void PickSeeds(PartitionVars* a_parVars) {
991 : 0 : int seed0 = 0, seed1 = 0;
992 : 0 : _ElementTypeReal worst{ 0.0 }, waste{ 0.0 };
993 : 0 : _ElementTypeReal area[_MaxNodeCount + 1] = { 0.0, };
994 : :
995 : 0 : for (int index = 0; index < a_parVars->m_total; ++index) {
996 : 0 : area[index] = CalcRectVolume(&a_parVars->m_branchBuf[index].m_rect);
997 : 0 : }
998 : :
999 : 0 : worst = -a_parVars->m_coverSplitArea - 1;
1000 : 0 : for (int indexA = 0; indexA < a_parVars->m_total - 1; ++indexA) {
1001 : 0 : for (int indexB = indexA + 1; indexB < a_parVars->m_total; ++indexB) {
1002 : 0 : auto oneRect = CombineRect(&a_parVars->m_branchBuf[indexA].m_rect, &a_parVars->m_branchBuf[indexB].m_rect);
1003 : 0 : waste = CalcRectVolume(&oneRect) - area[indexA] - area[indexB];
1004 : 0 : if (waste > worst) {
1005 : 0 : worst = waste;
1006 : 0 : seed0 = indexA;
1007 : 0 : seed1 = indexB;
1008 : 0 : }
1009 : 0 : }
1010 : 0 : }
1011 : :
1012 : 0 : Classify(seed0, 0, a_parVars);
1013 : 0 : Classify(seed1, 1, a_parVars);
1014 : 0 : }
1015 : :
1016 : : // Put a branch in one of the groups.
1017 : 0 : void Classify(int a_index, int a_group, PartitionVars* a_parVars) {
1018 : 0 : RTreeAssert(a_parVars);
1019 : 0 : RTreeAssert(PartitionVars::kNotTaken == a_parVars->m_partition[a_index]);
1020 : :
1021 : 0 : a_parVars->m_partition[a_index] = a_group;
1022 : :
1023 : : // Calculate combined rect
1024 : 0 : if (a_parVars->m_count[a_group] == 0) {
1025 : 0 : a_parVars->m_cover[a_group] = a_parVars->m_branchBuf[a_index].m_rect;
1026 : 0 : } else {
1027 : 0 : a_parVars->m_cover[a_group] = CombineRect(&a_parVars->m_branchBuf[a_index].m_rect, &a_parVars->m_cover[a_group]);
1028 : : }
1029 : :
1030 : : // Calculate volume of combined rect
1031 : 0 : a_parVars->m_area[a_group] = CalcRectVolume(&a_parVars->m_cover[a_group]);
1032 : :
1033 : 0 : ++a_parVars->m_count[a_group];
1034 : 0 : }
1035 : :
1036 : : // Delete a data rectangle from an index structure.
1037 : : // Pass in a pointer to a Rect, the tid of the record, ptr to ptr to root node.
1038 : : // Returns 1 if record not found, 0 if success.
1039 : : // RemoveRect provides for eliminating the root.
1040 : 0 : bool RemoveRect(Rect* a_rect, const DataType& a_id, Node** a_root) {
1041 : 0 : RTreeAssert(a_rect && a_root);
1042 : 0 : RTreeAssert(*a_root);
1043 : :
1044 : 0 : ListNode* reInsertList{ nullptr };
1045 : :
1046 : 0 : if (!RemoveRectRec(a_rect, a_id, *a_root, &reInsertList)) {
1047 : : // Found and deleted a data item
1048 : : // Reinsert any branches from eliminated nodes
1049 : 0 : while (reInsertList) {
1050 : 0 : Node* tempNode = reInsertList->m_node;
1051 : :
1052 : 0 : for (int index = 0; index < tempNode->m_count; ++index) {
1053 : : // TODO go over this code. should I use (tempNode->m_level - 1)?
1054 : 0 : InsertRect(tempNode->m_branch[index],
1055 : 0 : a_root,
1056 : 0 : tempNode->m_level);
1057 : 0 : }
1058 : :
1059 : 0 : ListNode* remLNode = reInsertList;
1060 : 0 : reInsertList = reInsertList->m_next;
1061 : :
1062 : 0 : FreeNode(remLNode->m_node);
1063 : 0 : FreeListNode(remLNode);
1064 : : }
1065 : :
1066 : : // Check for redundant root (not leaf, 1 child) and eliminate TODO replace
1067 : : // if with while? In case there is a whole branch of redundant roots...
1068 : 0 : if ((*a_root)->m_count == 1 && (*a_root)->IsInternalNode()) {
1069 : 0 : auto tempNode = (*a_root)->m_branch[0].m_child;
1070 : :
1071 : 0 : RTreeAssert(tempNode);
1072 : 0 : FreeNode(*a_root);
1073 : 0 : *a_root = tempNode;
1074 : 0 : }
1075 : 0 : return false;
1076 : : } else {
1077 : 0 : return true;
1078 : : }
1079 : 0 : }
1080 : :
1081 : : // Delete a rectangle from non-root part of an index structure.
1082 : : // Called by RemoveRect. Descends tree recursively,
1083 : : // merges branches on the way back up.
1084 : : // Returns 1 if record not found, 0 if success.
1085 : 0 : bool RemoveRectRec(Rect* a_rect, const DataType& a_id, Node* a_node, ListNode** a_listNode) {
1086 : 0 : RTreeAssert(a_rect && a_node && a_listNode);
1087 : 0 : RTreeAssert(a_node->m_level >= 0);
1088 : :
1089 : 0 : if (a_node->IsInternalNode()) // not a leaf node
1090 : : {
1091 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
1092 : 0 : if (Overlap(a_rect, &(a_node->m_branch[index].m_rect))) {
1093 : 0 : if (!RemoveRectRec(a_rect, a_id, a_node->m_branch[index].m_child, a_listNode)) {
1094 : 0 : if (a_node->m_branch[index].m_child->m_count >= _MinNodeCount) {
1095 : : // child removed, just resize parent rect
1096 : 0 : a_node->m_branch[index].m_rect = NodeCover(a_node->m_branch[index].m_child);
1097 : 0 : } else {
1098 : : // child removed, not enough entries in node, eliminate node
1099 : 0 : ReInsert(a_node->m_branch[index].m_child, a_listNode);
1100 : 0 : DisconnectBranch(a_node, index); // Must return after this call as count has changed
1101 : : }
1102 : 0 : return false;
1103 : : }
1104 : 0 : }
1105 : 0 : }
1106 : 0 : return true;
1107 : : } else // A leaf node
1108 : : {
1109 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
1110 : 0 : if (a_node->m_branch[index].m_data == a_id) {
1111 : 0 : DisconnectBranch(a_node, index); // Must return after this call as count has changed
1112 : 0 : return false;
1113 : : }
1114 : 0 : }
1115 : 0 : return true;
1116 : : }
1117 : 0 : }
1118 : :
1119 : : // Allocate space for a node in the list used in DeletRect to
1120 : : // store Nodes that are too empty.
1121 : 0 : ListNode* AllocListNode() {
1122 : : #ifdef RTREE_DONT_USE_MEMPOOLS
1123 : 0 : return new ListNode;
1124 : : #else // RTREE_DONT_USE_MEMPOOLS
1125 : : // EXAMPLE
1126 : : #endif // RTREE_DONT_USE_MEMPOOLS
1127 : : }
1128 : :
1129 : 0 : void FreeListNode(ListNode* a_listNode) {
1130 : : #ifdef RTREE_DONT_USE_MEMPOOLS
1131 : 0 : delete a_listNode;
1132 : : #else // RTREE_DONT_USE_MEMPOOLS
1133 : : // EXAMPLE
1134 : : #endif // RTREE_DONT_USE_MEMPOOLS
1135 : 0 : }
1136 : :
1137 : : // Decide whether two rectangles overlap.
1138 : 0 : bool Overlap(Rect* a_rectA, Rect* a_rectB) const {
1139 : 0 : RTreeAssert(a_rectA && a_rectB);
1140 : :
1141 : 0 : for (int index = 0; index < kNumDimensions; ++index) {
1142 : 0 : if (a_rectA->m_min[index] > a_rectB->m_max[index] ||
1143 : 0 : a_rectB->m_min[index] > a_rectA->m_max[index]) {
1144 : 0 : return false;
1145 : : }
1146 : 0 : }
1147 : 0 : return true;
1148 : 0 : }
1149 : :
1150 : : // Add a node to the reinsertion list. All its branches will later
1151 : : // be reinserted into the index structure.
1152 : 0 : void ReInsert(Node* a_node, ListNode** a_listNode) {
1153 : : ListNode* newListNode;
1154 : :
1155 : 0 : newListNode = AllocListNode();
1156 : 0 : newListNode->m_node = a_node;
1157 : 0 : newListNode->m_next = *a_listNode;
1158 : 0 : *a_listNode = newListNode;
1159 : 0 : }
1160 : :
1161 : : // Search in an index tree or subtree for all data retangles that overlap the argument rectangle.
1162 : 0 : bool Search(Node* a_node, Rect* a_rect, int& a_foundCount, std::function<bool(const DataType&)> callback) const {
1163 : 0 : RTreeAssert(a_node);
1164 : 0 : RTreeAssert(a_node->m_level >= 0);
1165 : 0 : RTreeAssert(a_rect);
1166 : :
1167 : 0 : if (a_node->IsInternalNode()) {
1168 : : // This is an internal node in the tree
1169 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
1170 : 0 : if (Overlap(a_rect, &a_node->m_branch[index].m_rect)) {
1171 : 0 : if (!Search(a_node->m_branch[index].m_child, a_rect, a_foundCount, callback)) {
1172 : : // The callback indicated to stop searching
1173 : 0 : return false;
1174 : : }
1175 : 0 : }
1176 : 0 : }
1177 : 0 : } else {
1178 : : // This is a leaf node
1179 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
1180 : 0 : if (Overlap(a_rect, &a_node->m_branch[index].m_rect)) {
1181 : 0 : const auto& id = a_node->m_branch[index].m_data;
1182 : 0 : ++a_foundCount;
1183 : :
1184 : 0 : if (callback && !callback(id)) {
1185 : 0 : return false; // Don't continue searching
1186 : : }
1187 : 0 : }
1188 : 0 : }
1189 : : }
1190 : :
1191 : 0 : return true; // Continue searching
1192 : 0 : }
1193 : :
1194 : 0 : void RemoveAllRec(Node* a_node) {
1195 : 0 : RTreeAssert(a_node);
1196 : 0 : RTreeAssert(a_node->m_level >= 0);
1197 : :
1198 : 0 : if (a_node->IsInternalNode()) // This is an internal node in the tree
1199 : : {
1200 : 0 : for (int index = 0; index < a_node->m_count; ++index) {
1201 : 0 : RemoveAllRec(a_node->m_branch[index].m_child);
1202 : 0 : }
1203 : 0 : }
1204 : 0 : FreeNode(a_node);
1205 : 0 : }
1206 : :
1207 : 0 : void Reset() {
1208 : : #ifdef RTREE_DONT_USE_MEMPOOLS
1209 : : // Delete all existing nodes
1210 : 0 : RemoveAllRec(m_root);
1211 : : #else // RTREE_DONT_USE_MEMPOOLS
1212 : : // Just reset memory pools. We are not using complex types
1213 : : // EXAMPLE
1214 : : #endif // RTREE_DONT_USE_MEMPOOLS
1215 : 0 : }
1216 : :
1217 : : void CountRec(Node* a_node, int& a_count) const {
1218 : : if (a_node->IsInternalNode()) // not a leaf node
1219 : : {
1220 : : for (int index = 0; index < a_node->m_count; ++index) {
1221 : : CountRec(a_node->m_branch[index].m_child, a_count);
1222 : : }
1223 : : } else // A leaf node
1224 : : {
1225 : : a_count += a_node->m_count;
1226 : : }
1227 : : }
1228 : :
1229 : : bool SaveRec(Node* a_node, RTFileStream& a_stream) {
1230 : : a_stream.Write(a_node->m_level);
1231 : : a_stream.Write(a_node->m_count);
1232 : :
1233 : : if (a_node->IsInternalNode()) // not a leaf node
1234 : : {
1235 : : for (int index = 0; index < a_node->m_count; ++index) {
1236 : : const auto& curBranch = a_node->m_branch[index];
1237 : :
1238 : : a_stream.WriteArray(curBranch->m_rect.m_min, kNumDimensions);
1239 : : a_stream.WriteArray(curBranch->m_rect.m_max, kNumDimensions);
1240 : :
1241 : : SaveRec(curBranch->m_child, a_stream);
1242 : : }
1243 : : } else // A leaf node
1244 : : {
1245 : : for (int index = 0; index < a_node->m_count; ++index) {
1246 : : const auto& curBranch = &a_node->m_branch[index];
1247 : :
1248 : : a_stream.WriteArray(curBranch->m_rect.m_min, kNumDimensions);
1249 : : a_stream.WriteArray(curBranch->m_rect.m_max, kNumDimensions);
1250 : :
1251 : : a_stream.Write(curBranch->m_data);
1252 : : }
1253 : : }
1254 : :
1255 : : return true; // Should do more error checking on I/O operations
1256 : : }
1257 : :
1258 : : bool LoadRec(Node* a_node, RTFileStream& a_stream) {
1259 : : a_stream.Read(a_node->m_level);
1260 : : a_stream.Read(a_node->m_count);
1261 : :
1262 : : if (a_node->IsInternalNode()) // not a leaf node
1263 : : {
1264 : : for (int index = 0; index < a_node->m_count; ++index) {
1265 : : Branch* curBranch = &a_node->m_branch[index];
1266 : :
1267 : : a_stream.ReadArray(curBranch->m_rect.m_min, kNumDimensions);
1268 : : a_stream.ReadArray(curBranch->m_rect.m_max, kNumDimensions);
1269 : :
1270 : : curBranch->m_child = AllocNode();
1271 : : LoadRec(curBranch->m_child, a_stream);
1272 : : }
1273 : : } else // A leaf node
1274 : : {
1275 : : for (int index = 0; index < a_node->m_count; ++index) {
1276 : : Branch* curBranch = &a_node->m_branch[index];
1277 : :
1278 : : a_stream.ReadArray(curBranch->m_rect.m_min, kNumDimensions);
1279 : : a_stream.ReadArray(curBranch->m_rect.m_max, kNumDimensions);
1280 : :
1281 : : a_stream.Read(curBranch->m_data);
1282 : : }
1283 : : }
1284 : :
1285 : : return true; // Should do more error checking on I/O operations
1286 : : }
1287 : :
1288 : : void CopyRec(Node* current, Node* other) {
1289 : : current->m_level = other->m_level;
1290 : : current->m_count = other->m_count;
1291 : :
1292 : : if (current->IsInternalNode()) // not a leaf node
1293 : : {
1294 : : for (int index = 0; index < current->m_count; ++index) {
1295 : : auto& currentBranch = current->m_branch[index];
1296 : : const auto& otherBranch = other->m_branch[index];
1297 : :
1298 : : std::copy(otherBranch.m_rect.m_min,
1299 : : otherBranch.m_rect.m_min + kNumDimensions,
1300 : : currentBranch.m_rect.m_min);
1301 : :
1302 : : std::copy(otherBranch.m_rect.m_max,
1303 : : otherBranch.m_rect.m_max + kNumDimensions,
1304 : : currentBranch.m_rect.m_max);
1305 : :
1306 : : currentBranch.m_child = AllocNode();
1307 : : CopyRec(currentBranch.m_child, otherBranch.m_child);
1308 : : }
1309 : : } else // A leaf node
1310 : : {
1311 : : for (int index = 0; index < current->m_count; ++index) {
1312 : : auto& currentBranch = current->m_branch[index];
1313 : : const auto& otherBranch = other->m_branch[index];
1314 : :
1315 : : std::copy(otherBranch.m_rect.m_min,
1316 : : otherBranch.m_rect.m_min + kNumDimensions,
1317 : : currentBranch.m_rect.m_min);
1318 : :
1319 : : std::copy(otherBranch.m_rect.m_max,
1320 : : otherBranch.m_rect.m_max + kNumDimensions,
1321 : : currentBranch.m_rect.m_max);
1322 : :
1323 : : currentBranch.m_data = otherBranch.m_data;
1324 : : }
1325 : : }
1326 : : }
1327 : :
1328 : 0 : Node* m_root = nullptr; ///< Root of tree
1329 : 0 : ElementTypeReal m_unitSphereVolume = kElementTypeRealZero; ///< Unit sphere constant for required number of dimensions
1330 : : };
1331 : :
1332 : : #endif //RTREE_H
|
