Point Cloud Library (PCL)  1.9.1-dev
opennurbs_rtree.h
1 /* $NoKeywords: $ */
2 /*
3 //
4 // Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
5 // OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
6 // McNeel & Associates.
7 //
8 // THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
9 // ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
10 // MERCHANTABILITY ARE HEREBY DISCLAIMED.
11 //
12 // For complete openNURBS copyright information see <http://www.opennurbs.org>.
13 //
14 ////////////////////////////////////////////////////////////////
15 */
16 
17 #if !defined(OPENNURBS_RTREE_INC_)
18 #define OPENNURBS_RTREE_INC_
19 
20 /*
21 The opennurbs rtree code is a modifed version of the
22 free and unrestricted R-tree implementation obtianed from
23 http://www.superliminal.com/sources/sources.htm
24 
25 The first lines on the website indicate the code is free and unrestricted:
26 
27  Free Source Code
28  Here are a few useful bits of free source code.
29  You're completely free to use them for any purpose whatsoever.
30  All I ask is that if you find one to be particularly valuable,
31  then consider sending feedback. Please send bugs and suggestions too.
32  Enjoy
33 
34 The readme.txt file included with the R-tree source says
35 
36  LICENSE:
37  Entirely free for all uses. Enjoy!
38 
39 The original authors are
40 
41 AUTHORS
42  * 1983 Original algorithm and test code by Antonin Guttman and Michael Stonebraker, UC Berkely
43  * 1994 ANCI C ported from original test code by Melinda Green - melinda@superliminal.com
44  * 1995 Sphere volume fix for degeneracy problem submitted by Paul Brook
45  * 2004 Templated C++ port by Greg Douglas
46 
47 The opennurbs version adds some custom memory allocation and replaces
48 the leaf iterator. The rest of the changes are cosmetic.
49 
50 */
51 
52 
53 
54 // Minimum and maximum number of elements
55 // in ON_RTreeNode::m_branch[].
56 // must have ON_RTree_MAX_NODE_COUNT > ON_RTree_MIN_NODE_COUNT
57 #define ON_RTree_MIN_NODE_COUNT 2
58 #define ON_RTree_MAX_NODE_COUNT 6
59 
60 /*
61 In a test of a sphere mesh with mesh: 8385 vertices, 8192 polygons
62 and ON_RTree_MAX_NODE_COUNT = 3, 4, 5, and 6, the memory use was
63 most efficient with ON_RTree_MAX_NODE_COUNT=6
64 
65 Memory Usage MAX_NODE_COUNT = 3
66  ON_RTree: 1212 KB (1241136) (352 wasted)
67  ON_RTree: 7036 nodes, 5881 unused branches (321 KB) 0.835844 per node
68 
69 Memory Usage MAX_NODE_COUNT = 4
70  ON_RTree: 1152 KB (1179720) (5568 wasted)
71  ON_RTree: 5051 nodes, 6962 unused branches (380 KB) 1.37834 per node
72 
73 Memory Usage MAX_NODE_COUNT = 5
74  ON_RTree: 1040 KB (1065504) (11808 wasted)
75  ON_RTree: 3655 nodes, 6429 unused branches (351 KB) 1.75896 per node
76 
77 Memory Usage MAX_NODE_COUNT = 6
78  ON_RTree: 995 KB (1019592) (3440 wasted)
79  ON_RTree: 2951 nodes, 6564 unused branches (358 KB) 2.22433 per node
80 */
81 
82 // This struct is used instead of ON_BoundingBox to avoid calling
83 // constructors.
85 {
86  double m_min[3];
87  double m_max[3];
88 };
89 
91 {
92  double m_point[3];
93  double m_radius;
94 };
95 
97 {
98  double m_point[2][3];
99  double m_radius;
100  double m_domain[2];
101 };
102 
104 {
106 
107  // If ON_RTreeNode.m_level > 0, then m_child points to a child node.
108  // If ON_RTreeNode.m_level == 0, then m_id identifies the leaf element.
109  union
110  {
112  ON__INT_PTR m_id;
113  };
114 };
115 
117 {
119  ON__INT_PTR m_id;
120 };
121 
122 // The ON_RTreeNode is used at root, branch and leaf nodes.
123 // When m_level > 0, the node is a branch.
124 // When m_level = 0, the node is a leaf.
126 {
127  inline bool IsInternalNode() const
128  { return (m_level > 0); } // internal nodes have m_level > 0
129  inline bool IsLeaf() const
130  { return (m_level == 0); } // branch nodes have m_level = 0
131 
132  // m_level must be a signed int to insure signed compares work correctly
133  int m_level; // =0 at leaf nodes, > 0 at branch nodes
134 
135  // The m_branch[] array contains m_count elements
136  // 0 <= m_count <= ON_RTree_MAX_NODE_COUNT
137  // m_count must be a signed int to insure signed compares work correctly
138  int m_count;
139  ON_RTreeBranch m_branch[ON_RTree_MAX_NODE_COUNT];
140 };
141 
143 {
144  int m_capacity; // m_id[] array capacity (search terminates when m_count == m_capacity)
145  int m_count; // number of elements in m_id[]
146  ON__INT_PTR* m_id; // m_id[] = array of search results.
147 };
148 
149 class ON_CLASS ON_RTreeMemPool
150 {
151 public:
152  ON_RTreeMemPool( ON_MEMORY_POOL* heap, std::size_t leaf_count );
153  ~ON_RTreeMemPool();
154 
155  ON_RTreeNode* AllocNode();
156  void FreeNode(ON_RTreeNode* node);
157 
158  struct ON_RTreeListNode* AllocListNode();
159  void FreeListNode(struct ON_RTreeListNode* list_node);
160 
161  void DeallocateAll();
162 
163  /*
164  Returns:
165  Total number of bytes of heap memory allocated.
166  */
167  std::size_t SizeOf() const;
168 
169  /*
170  Returns:
171  Number of bytes of heap memory not currently in use.
172  */
173  std::size_t SizeOfUnusedBuffer() const;
174 
175 private:
176  void GrowBuffer();
177 
178  struct Blk
179  {
180  struct Blk* m_next;
181  };
182 
183  // linked list of unused ON_RTreeNode
184  struct Blk* m_nodes;
185  // linked list of unused ON_RTreeListNode
186  struct Blk* m_list_nodes;
187 
188  // buffer for new allocations
189  unsigned char* m_buffer;
190  std::size_t m_buffer_capacity;
191 
192  struct Blk* m_blk_list; // linked list used to free all allocated memory
193  std::size_t m_sizeof_blk; // total amount of memory in each block.
194 
195  ON_MEMORY_POOL* m_heap;
196  std::size_t m_sizeof_heap; // total amount of heap memory in this rtree
197 };
198 
199 ////////////////////////////////////////////////////////////////
200 //
201 // ON_RTreeIterator
202 //
203 // The ON_RTreeIterator class can be used to iterate each leaf
204 // in an ON_RTree.
205 //
206 class ON_CLASS ON_RTreeIterator
207 {
208 public:
209  /*
210  Description:
211  Construct an empty iterator. Call Initialize() to attach
212  the iterator to an R-tree.
213  Remark:
214  Any calls to ON_RTree::Insert() or ON_RTree::Remove() that modify
215  an R-tree being iterated invalidate the iterator. The iterator
216  must be re-initialized before being used again.
217 
218  There is no connection between the order elements are inserted
219  in an R-tree and the order the elements are iterated by an
220  iterator.
221  */
223  ON_RTreeIterator(const class ON_RTree& a_rtree);
224 
225  ~ON_RTreeIterator();
226 
227  /*
228  Description:
229  Initialize an iterator to iterate every leaf in the rtree.
230  Parameters:
231  a_rtree - [in]
232  R-tree to iterate
233  Example:
234  See the comment for ON_RTreeIterator::First().
235  Returns:
236  True if a_rtree has at least one element.
237  Remarks:
238  Any calls to ON_RTree::Insert() or ON_RTree::Remove() that modify
239  this node or its children will invalidate this iterator and it
240  must be re-initialized.
241 
242  There is no connection between the order elements are inserted
243  in an R-tree and the order the elements are iterated by an
244  iterator.
245  */
246  bool Initialize(const class ON_RTree& a_rtree);
247 
248  /*
249  Description:
250  Initialize an iterator to iterate every leaf on or below a_node.
251  Parameters:
252  a_node - [in]
253  R-tree node to iterate
254  Example:
255  See the comment for ON_RTreeIterator::First().
256  Returns:
257  True if a_node has at least one element.
258  Remarks:
259  Any calls to ON_RTree::Insert() or ON_RTree::Remove() that modify
260  this node or its children will invalidate this iterator and it
261  must be re-initialized.
262 
263  There is no connection between the order elements are inserted
264  in an R-tree and the order the elements are iterated by an
265  iterator.
266  */
267  bool Initialize(const struct ON_RTreeNode* a_node);
268 
269  /*
270  Description:
271  Get the value of the current leaf element. Calling Value()
272  does not increment or decrement the iterator.
273  Example:
274  See the comment for ON_RTreeIterator::First().
275  Return:
276  Null pointer if there are no more leaves to iterate
277  A pointer to the current R-tree leaf. When there are no more leaves,
278  the returned pointer is null.
279  */
280  const ON_RTreeBranch* Value() const;
281 
282  /*
283  Description:
284  Reset the iterator so the current leaf is the first leaf in
285  the R-tree. The Initialize() functions automatically do
286  this, but First() can be called if an iterator needs to be
287  used more than once or to make code easy to read and understand.
288  Example:
289  Iterate every leaf in an R-tree.
290 
291  ON_RTree rtree;
292  ...
293  ON_RtreeIterator rit(rtree);
294  const ON_RTreeBranch* rtree_leaf;
295  for ( rit.First(); 0 != (rtree_leaf = rit.Value()); rit.Next() )
296  {
297  // leaf id = rtree_leaf->m_id
298  // leaf bounding box = rtree->m_rect
299  }
300 
301  Returns:
302  True if a call to Value() will return a non-null pointer.
303  See Also:
304  ON_RTreeIterator::Last();
305  */
306  bool First();
307 
308  /*
309  Description:
310  Increment the iterator to the next leaf in the R-tree.
311  Example:
312  See the comment for ON_RTreeIterator::First()
313  Returns:
314  True if a call to Value() will return a non-null pointer.
315  False if there is not a next leaf and all susequent calls to
316  Value() will return null.
317  See Also:
318  ON_RTreeIterator::Prev();
319  */
320  bool Next();
321 
322 
323  /*
324  Description:
325  Set the iterator so the current leaf is the last leaf in the R-tree.
326 
327  Example:
328  Iterate an R-tree in reverse order.
329 
330  ON_RTree rtree;
331  ...
332  ON_RTreeIterator rit(rtree);
333  const ON_RTreeBranch* rtree_leaf;
334  for ( rit.Last(); 0 != (rtree_leaf = rit.Value()); rit.Prev() )
335  {
336  // leaf id = rtree_leaf->m_id
337  // leaf bounding box = rtree->m_rect
338  }
339 
340  Returns:
341  True if a call to Value() will return a non-null pointer.
342  See Also:
343  ON_RTreeIterator::First();
344  */
345  bool Last();
346 
347  /*
348  Description:
349  Decrement the iterator to the previous leaf in the R-tree.
350  Example:
351  See the comment for ON_RTreeIterator::Last()
352  Returns:
353  True if a call to Value() will return a non-null pointer.
354  False if there is not a previous leaf and all susequent calls to
355  Value() will return null.
356  See Also:
357  ON_RTreeIterator::Next();
358  */
359  bool Prev();
360 
361 private:
362  enum { MAX_STACK = 32 }; // Max stack size. Allows almost n^32 where n is number of branches in node
363 
364  struct StackElement
365  {
366  const struct ON_RTreeNode* m_node;
367  int m_branchIndex; // must be a signed int to insure signed compares work correctly
368  };
369 
370  bool PushChildren(struct StackElement* sp, bool bFirstChild);
371 
372  StackElement m_stack[MAX_STACK]; // stack
373  StackElement* m_sp; // stack pointer (null or points into m_stack[])
374  const ON_RTreeNode* m_root; // root of tree being iterated
375 };
376 
377 
378 class ON_CLASS ON_RTree
379 {
380 public:
381  ON_RTree( ON_MEMORY_POOL* heap = 0, std::size_t leaf_count = 0 );
382  ~ON_RTree();
383 
384  /*
385  Description:
386  Create an R-tree with an element for each face in the mesh.
387  The element id is set to the index of the face.
388  Parameters:
389  mesh - [in]
390  Returns:
391  True if successful.
392  */
393  bool CreateMeshFaceTree( const class ON_Mesh* mesh );
394 
395  /*
396  Description:
397  Insert an element into the RTree.
398  Parameters:
399  a_min - [in]
400  a_max - [in]
401  3d bounding box of the element. The values in a_min[3] and a_max[3]
402  must satisfy
403  a_min[0] <= a_max[0],
404  a_min[1] <= a_max[1], and
405  a_min[1] <= a_max[1].
406  a_dataId - [in]
407  id of the element. This can be either a pointer or an integer id.
408  Returns:
409  True if element was successfully inserted.
410  Remarks:
411  Calling Insert() or Remove() invalidates any ON_RTreeIterator
412  used to iterate this rtree.
413  */
414  bool Insert(const double a_min[3], const double a_max[3], void* a_element_id);
415  bool Insert(const double a_min[3], const double a_max[3], int a_element_id);
416  bool Insert2d(const double a_min[2], const double a_max[2], void* a_element_id);
417  bool Insert2d(const double a_min[2], const double a_max[2], int a_element_id);
418 
419  /*
420  Description:
421  Remove an element from the RTree.
422  Parameters:
423  a_min - [in]
424  a_max - [in]
425  3d bounding box of the element. The values in a_min[3] and a_max[3]
426  must satisfy
427  a_min[0] <= a_max[0],
428  a_min[1] <= a_max[1], and
429  a_min[2] <= a_max[2].
430  a_dataId - [in]
431  id of the element. This can be either a pointer or an integer id.
432  Returns:
433  True if element was successfully removed.
434  Remarks:
435  Calling Insert() or Remove() invalidates any ON_RTreeIterator
436  used to iterate this rtree.
437  */
438  bool Remove(const double a_min[3], const double a_max[3], void* a_elementId);
439  bool Remove(const double a_min[3], const double a_max[3], int a_elementId);
440  bool Remove2d(const double a_min[2], const double a_max[2], void* a_elementId);
441  bool Remove2d(const double a_min[2], const double a_max[2], int a_elementId);
442 
443  /*
444  Description:
445  Remove all elements from the R-tree.
446  */
447  void RemoveAll();
448 
449  /*
450  Description:
451  Search the R-tree for all elements whose bounding boxes overlap
452  a_rect.
453  Parameters:
454  a_rect - [in/out]
455  The version of search that has ON_RTreeBBox* a_rect as the first
456  argument, allows you to shrink the a_rect as the search progresses.
457  This is useful for doing things like searching for closest points.
458  If you want to shrink a_rect, you must use a_context to pass it
459  to the resultCallback function and shrink it in the resultCallback
460  function. In the callback, the modified rect must be contained
461  in the previous rect.
462  a_sphere - [in/out]
463  The version of search that has ON_RTreeSphere* a_sphere as the first
464  argument, allows you to shrink the a_sphere as the search progresses.
465  This is useful for doing things like searching for closest points.
466  If you want to shrink a_sphere, you must use a_context to pass it
467  to the resultCallback function and shrink it in the resultCallback
468  function. In the callback, the modified sphere must be contained
469  in the previous sphere.
470  a_capsule - [in/out]
471  The version of search that has ON_RTreeSphere* a_capsule as the first
472  argument, allows you to shrink the a_capsule as the search progresses.
473  This is useful for doing things like searching for closest points.
474  If you want to shrink a_capsule, you must use a_context to pass it
475  to the resultCallback function and shrink it in the resultCallback
476  function. In the callback, the modified capsule must be contained
477  in the previous capsule.
478  a_min - [in]
479  a_max - [in]
480  (a_min,a_max) is the bounding box of the search region.
481  a_results - [out]
482  The ids of elements that overlaps the search region.
483  resultCallback - [in]
484  A function to call when leaf nodes overlap.
485  a_context - [in]
486  pointer passed to the resultCallback() function.
487  Returns:
488  True if entire tree was searched. It is possible no results were found.
489  Remarks:
490  If you are using a Search() that uses a resultCallback() function,
491  then return true to keep searching and false to terminate the search.
492  */
493  bool Search(
494  ON_RTreeSphere* a_sphere,
495  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id),
496  void* a_context
497  ) const;
498 
499  bool Search(
500  ON_RTreeCapsule* a_capsule,
501  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id),
502  void* a_context
503  ) const;
504 
505  bool Search(
506  ON_RTreeBBox* a_rect,
507  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id),
508  void* a_context
509  ) const;
510 
511  /*
512  Description:
513  Search the R-tree for all elements whose bounding boxes overlap
514  the set of points between to parallel planes.
515  Parameters:
516  a_plane_eqn - [in]
517  a_min - [in]
518  a_max - [in]
519  The region between the parallel planes is the set point points
520  where the value of the plane equation is >= a_min and <= a_max.
521  resultCallback - [in]
522  A function to call when leaf nodes overlap the region between
523  the parallel planes.
524  a_context - [in]
525  pointer passed to the resultCallback() function.
526  Returns:
527  True if entire tree was searched. It is possible no results were found.
528  Remarks:
529  If you are using a Search() that uses a resultCallback() function,
530  then return true to keep searching and false to terminate the search.
531  */
532  bool Search(
533  const double a_plane_eqn[4],
534  double a_min,
535  double a_max,
536  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id),
537  void* a_context
538  ) const;
539 
540  bool Search(const double a_min[3], const double a_max[3],
541  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id), void* a_context
542  ) const;
543 
544  bool Search(const double a_min[3], const double a_max[3],
545  ON_RTreeSearchResult& a_result
546  ) const;
547 
548  bool Search(const double a_min[3], const double a_max[3],
550  ) const;
551 
552  bool Search(const double a_min[3], const double a_max[3],
553  ON_SimpleArray<void*>& a_result
554  ) const;
555 
556  bool Search(const double a_min[3], const double a_max[3],
557  ON_SimpleArray<int>& a_result
558  ) const;
559 
560  bool Search2d(const double a_min[2], const double a_max[2],
561  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_id), void* a_context
562  ) const;
563 
564  bool Search2d(const double a_min[2], const double a_max[2],
565  ON_RTreeSearchResult& a_result
566  ) const;
567 
568  bool Search2d(const double a_min[2], const double a_max[2],
570  ) const;
571 
572  bool Search2d(const double a_min[2], const double a_max[2],
573  ON_SimpleArray<void*>& a_result
574  ) const;
575 
576  bool Search2d(const double a_min[2], const double a_max[2],
577  ON_SimpleArray<int>& a_result
578  ) const;
579 
580  /*
581  Description:
582  Search two R-trees for all pairs elements whose bounding boxes overlap.
583  Parameters:
584  a_rtreeA - [in]
585  a_rtreeB - [in]
586  tolerance - [in]
587  If the distance between a pair of bounding boxes is <= tolerance,
588  then the pair is added to a_result[].
589  a_result - [out]
590  Pairs of ids of elements who bounding boxes overlap.
591  Returns:
592  True if entire tree was searched. It is possible no results were found.
593  */
594  static bool Search(
595  const ON_RTree& a_rtreeA,
596  const ON_RTree& a_rtreeB,
597  double tolerance,
598  ON_SimpleArray<ON_2dex>& a_result
599  );
600 
601  /*
602  Description:
603  Search two R-trees for all pairs elements whose bounding boxes overlap.
604  Parameters:
605  a_rtreeA - [in]
606  a_rtreeB - [in]
607  tolerance - [in]
608  If the distance between a pair of bounding boxes is <= tolerance,
609  then resultCallback() is called.
610  resultCallback - [out]
611  callback function
612  a_context - [in] argument passed through to resultCallback().
613  Returns:
614  True if entire tree was searched. It is possible no results were found.
615  */
616  static bool Search(
617  const ON_RTree& a_rtreeA,
618  const ON_RTree& a_rtreeB,
619  double tolerance,
620  void ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_idA, ON__INT_PTR a_idB),
621  void* a_context
622  );
623 
624  /*
625  Description:
626  Search two R-trees for all pairs elements whose bounding boxes overlap.
627  Parameters:
628  a_rtreeA - [in]
629  a_rtreeB - [in]
630  tolerance - [in]
631  If the distance between a pair of bounding boxes is <= tolerance,
632  then resultCallback() is called.
633  resultCallback - [out]
634  callback function
635  Return true for the search to continue and false to terminate the search.
636  a_context - [in] argument passed through to resultCallback().
637  Returns:
638  True if entire tree was searched. It is possible no results were found.
639  */
640  static bool Search(
641  const ON_RTree& a_rtreeA,
642  const ON_RTree& a_rtreeB,
643  double tolerance,
644  bool ON_MSC_CDECL resultCallback(void* a_context, ON__INT_PTR a_idA, ON__INT_PTR a_idB),
645  void* a_context
646  );
647  /*
648  Returns:
649  Number of elements (leaves).
650  Remark:
651  No internal count is maintained, so this function traverses the
652  tree to count the leaves. If efficiency is important, save the
653  result.
654  */
655  int ElementCount();
656 
657  /*
658  Returns:
659  Pointer to the root node.
660  */
661  const ON_RTreeNode* Root() const;
662 
663  /*
664  Returns:
665  Bounding box of the entire R-tree;
666  */
667  ON_BoundingBox BoundingBox() const;
668 
669  /*
670  Returns:
671  Number of bytes of heap memory used by this R-tree.
672  */
673  std::size_t SizeOf() const;
674 
675 private:
676  void SplitNode(ON_RTreeNode*, ON_RTreeBranch*, ON_RTreeNode**);
677  bool AddBranch(ON_RTreeBranch*, ON_RTreeNode*, ON_RTreeNode**);
678  bool InsertRectRec(ON_RTreeBBox*, ON__INT_PTR, ON_RTreeNode*, ON_RTreeNode**, int);
679  bool InsertRect(ON_RTreeBBox*, ON__INT_PTR, ON_RTreeNode**, int);
680  void LoadNodes(ON_RTreeNode*, ON_RTreeNode*, struct ON_RTreePartitionVars*);
681  bool RemoveRect(ON_RTreeBBox*, ON__INT_PTR, ON_RTreeNode**);
682  bool RemoveRectRec(ON_RTreeBBox*, ON__INT_PTR, ON_RTreeNode*, struct ON_RTreeListNode**);
683  void ReInsert(ON_RTreeNode*, struct ON_RTreeListNode**);
684  void RemoveAllRec(ON_RTreeNode*);
685  ON_RTreeNode* m_root;
686  std::size_t m_reserved;
687  ON_RTreeMemPool m_mem_pool;
688 };
689 
690 #endif
ON_RTreeBBox m_rect
ON__INT_PTR m_id
bool IsInternalNode() const
double m_min[3]
ON__INT_PTR m_id
ON_RTreeBBox m_rect
bool IsLeaf() const
struct ON_RTreeNode * m_child
double m_max[3]