hierarchy_tree-inl.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_HIERARCHY_TREE_INL_H
39 #define FCL_HIERARCHY_TREE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 namespace detail
47 {
48 
49 //==============================================================================
50 template<typename BV>
51 HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_)
52 {
53  root_node = nullptr;
54  n_leaves = 0;
55  free_node = nullptr;
56  max_lookahead_level = -1;
57  opath = 0;
58  bu_threshold = bu_threshold_;
59  topdown_level = topdown_level_;
60 }
61 
62 //==============================================================================
63 template<typename BV>
65 {
66  clear();
67 }
68 
69 //==============================================================================
70 template<typename BV>
71 void HierarchyTree<BV>::init(std::vector<NodeType*>& leaves, int level)
72 {
73  switch(level)
74  {
75  case 0:
76  init_0(leaves);
77  break;
78  case 1:
79  init_1(leaves);
80  break;
81  case 2:
82  init_2(leaves);
83  break;
84  case 3:
85  init_3(leaves);
86  break;
87  default:
88  init_0(leaves);
89  }
90 }
91 
92 //==============================================================================
93 template<typename BV>
94 typename HierarchyTree<BV>::NodeType* HierarchyTree<BV>::insert(const BV& bv, void* data)
95 {
96  NodeType* leaf = createNode(nullptr, bv, data);
97  insertLeaf(root_node, leaf);
98  ++n_leaves;
99  return leaf;
100 }
101 
102 //==============================================================================
103 template<typename BV>
105 {
106  removeLeaf(leaf);
107  deleteNode(leaf);
108  --n_leaves;
109 }
110 
111 //==============================================================================
112 template<typename BV>
114 {
115  if(root_node)
116  recurseDeleteNode(root_node);
117  n_leaves = 0;
118  delete free_node;
119  free_node = nullptr;
120  max_lookahead_level = -1;
121  opath = 0;
122 }
123 
124 //==============================================================================
125 template<typename BV>
127 {
128  return (nullptr == root_node);
129 }
130 
131 //==============================================================================
132 template<typename BV>
133 void HierarchyTree<BV>::update(NodeType* leaf, int lookahead_level)
134 {
135  // TODO(DamrongGuoy): Since we update a leaf node by removing and
136  // inserting the same leaf node, it is likely to change the structure of
137  // the tree even if no object's pose has changed. In the future,
138  // find a way to preserve the structure of the tree to solve this issue:
139  // https://github.com/flexible-collision-library/fcl/issues/368
140 
141  // First we remove the leaf node pointed by `leaf` variable from the tree.
142  // The `sub_root` variable is the root of the subtree containing nodes
143  // whose BVs were adjusted due to node removal.
144  typename HierarchyTree<BV>::NodeType* sub_root = removeLeaf(leaf);
145  if(sub_root)
146  {
147  if(lookahead_level > 0)
148  {
149  // For positive `lookahead_level`, we move the `sub_root` variable up.
150  for(int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
151  sub_root = sub_root->parent;
152  }
153  else
154  // By default, lookahead_level = -1, and we reset the `sub_root` variable
155  // to the root of the entire tree.
156  sub_root = root_node;
157  }
158  // Then we insert the node pointed by `leaf` variable back into the
159  // subtree rooted at `sub_root` variable.
160  insertLeaf(sub_root, leaf);
161 }
162 
163 //==============================================================================
164 template<typename BV>
165 bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv)
166 {
167  if(leaf->bv.contain(bv)) return false;
168  update_(leaf, bv);
169  return true;
170 }
171 
172 //==============================================================================
173 template <typename S, typename BV>
175 {
176  static bool run(
177  const HierarchyTree<BV>& tree,
178  typename HierarchyTree<BV>::NodeType* leaf,
179  const BV& bv,
180  const Vector3<S>& /*vel*/,
181  S /*margin*/)
182  {
183  if(leaf->bv.contain(bv)) return false;
184  tree.update_(leaf, bv);
185  return true;
186  }
187 
188  static bool run(
189  const HierarchyTree<BV>& tree,
190  typename HierarchyTree<BV>::NodeType* leaf,
191  const BV& bv,
192  const Vector3<S>& /*vel*/)
193  {
194  if(leaf->bv.contain(bv)) return false;
195  tree.update_(leaf, bv);
196  return true;
197  }
198 };
199 
200 //==============================================================================
201 template<typename BV>
202 bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vector3<S>& vel, S margin)
203 {
204  return UpdateImpl<typename BV::S, BV>::run(*this, leaf, bv, vel, margin);
205 }
206 
207 //==============================================================================
208 template<typename BV>
209 bool HierarchyTree<BV>::update(NodeType* leaf, const BV& bv, const Vector3<S>& vel)
210 {
211  return UpdateImpl<typename BV::S, BV>::run(*this, leaf, bv, vel);
212 }
213 
214 //==============================================================================
215 template<typename BV>
217 {
218  if(!root_node)
219  return 0;
220  return getMaxHeight(root_node);
221 }
222 
223 //==============================================================================
224 template<typename BV>
226 {
227  if(!root_node) return 0;
228 
229  size_t max_depth;
230  getMaxDepth(root_node, 0, max_depth);
231  return max_depth;
232 }
233 
234 //==============================================================================
235 template<typename BV>
237 {
238  if(root_node)
239  {
240  std::vector<NodeType*> leaves;
241  leaves.reserve(n_leaves);
242  fetchLeaves(root_node, leaves);
243  bottomup(leaves.begin(), leaves.end());
244  root_node = leaves[0];
245  }
246 }
247 
248 //==============================================================================
249 template<typename BV>
251 {
252  if(root_node)
253  {
254  std::vector<NodeType*> leaves;
255  leaves.reserve(n_leaves);
256  fetchLeaves(root_node, leaves);
257  root_node = topdown(leaves.begin(), leaves.end());
258  }
259 }
260 
261 //==============================================================================
262 template<typename BV>
264 {
265  if(iterations < 0) iterations = n_leaves;
266  if(root_node && (iterations > 0))
267  {
268  for(int i = 0; i < iterations; ++i)
269  {
270  NodeType* node = root_node;
271  unsigned int bit = 0;
272  while(!node->isLeaf())
273  {
274  node = sort(node, root_node)->children[(opath>>bit)&1];
275  bit = (bit+1)&(sizeof(unsigned int) * 8-1);
276  }
277  update(node);
278  ++opath;
279  }
280  }
281 }
282 
283 //==============================================================================
284 template<typename BV>
286 {
287  if(root_node)
288  recurseRefit(root_node);
289 }
290 
291 //==============================================================================
292 template<typename BV>
293 void HierarchyTree<BV>::extractLeaves(const NodeType* root, std::vector<NodeType*>& leaves) const
294 {
295  if(!root->isLeaf())
296  {
297  extractLeaves(root->children[0], leaves);
298  extractLeaves(root->children[1], leaves);
299  }
300  else
301  leaves.push_back(root);
302 }
303 
304 //==============================================================================
305 template<typename BV>
307 {
308  return n_leaves;
309 }
310 
311 //==============================================================================
312 template<typename BV>
314 {
315  return root_node;
316 }
317 
318 //==============================================================================
319 template<typename BV>
321 {
322  return root_node;
323 }
324 
325 //==============================================================================
326 template<typename BV>
327 void HierarchyTree<BV>::print(NodeType* root, int depth)
328 {
329  for(int i = 0; i < depth; ++i)
330  std::cout << " ";
331  std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
332  if(root->isLeaf())
333  {
334  }
335  else
336  {
337  print(root->children[0], depth+1);
338  print(root->children[1], depth+1);
339  }
340 }
341 
342 //==============================================================================
343 template<typename BV>
345 {
346  NodeVecIterator lcur_end = lend;
347  while(lbeg < lcur_end - 1)
348  {
349  NodeVecIterator min_it1, min_it2;
350  S min_size = std::numeric_limits<S>::max();
351  for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1)
352  {
353  for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2)
354  {
355  S cur_size = ((*it1)->bv + (*it2)->bv).size();
356  if(cur_size < min_size)
357  {
358  min_size = cur_size;
359  min_it1 = it1;
360  min_it2 = it2;
361  }
362  }
363  }
364 
365  NodeType* n[2] = {*min_it1, *min_it2};
366  NodeType* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
367  p->children[0] = n[0];
368  p->children[1] = n[1];
369  n[0]->parent = p;
370  n[1]->parent = p;
371  *min_it1 = p;
372  NodeType* tmp = *min_it2;
373  lcur_end--;
374  *min_it2 = *lcur_end;
375  *lcur_end = tmp;
376  }
377 }
378 
379 //==============================================================================
380 template<typename BV>
382 {
383  switch(topdown_level)
384  {
385  case 0:
386  return topdown_0(lbeg, lend);
387  break;
388  case 1:
389  return topdown_1(lbeg, lend);
390  break;
391  default:
392  return topdown_0(lbeg, lend);
393  }
394 }
395 
396 //==============================================================================
397 template<typename BV>
399 {
400  if(!node->isLeaf())
401  {
402  size_t height1 = getMaxHeight(node->children[0]);
403  size_t height2 = getMaxHeight(node->children[1]);
404  return std::max(height1, height2) + 1;
405  }
406  else
407  return 0;
408 }
409 
410 //==============================================================================
411 template<typename BV>
412 void HierarchyTree<BV>::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const
413 {
414  if(!node->isLeaf())
415  {
416  getMaxDepth(node->children[0], depth+1, max_depth);
417  getMaxDepth(node->children[1], depth+1, max_depth);
418  }
419  else
420  max_depth = std::max(max_depth, depth);
421 }
422 
423 //==============================================================================
424 template<typename BV>
426 {
427  int num_leaves = lend - lbeg;
428  if(num_leaves > 1)
429  {
430  if(num_leaves > bu_threshold)
431  {
432  BV vol = (*lbeg)->bv;
433  for(NodeVecIterator it = lbeg + 1; it < lend; ++it)
434  vol += (*it)->bv;
435 
436  int best_axis = 0;
437  S extent[3] = {vol.width(), vol.height(), vol.depth()};
438  if(extent[1] > extent[0]) best_axis = 1;
439  if(extent[2] > extent[best_axis]) best_axis = 2;
440 
441  // compute median
442  NodeVecIterator lcenter = lbeg + num_leaves / 2;
443  std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess<BV>, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis)));
444 
445  NodeType* node = createNode(nullptr, vol, nullptr);
446  node->children[0] = topdown_0(lbeg, lcenter);
447  node->children[1] = topdown_0(lcenter, lend);
448  node->children[0]->parent = node;
449  node->children[1]->parent = node;
450  return node;
451  }
452  else
453  {
454  bottomup(lbeg, lend);
455  return *lbeg;
456  }
457  }
458  return *lbeg;
459 }
460 
461 //==============================================================================
462 template<typename BV>
464 {
465  int num_leaves = lend - lbeg;
466  if(num_leaves > 1)
467  {
468  if(num_leaves > bu_threshold)
469  {
470  Vector3<S> split_p = (*lbeg)->bv.center();
471  BV vol = (*lbeg)->bv;
472  NodeVecIterator it;
473  for(it = lbeg + 1; it < lend; ++it)
474  {
475  split_p += (*it)->bv.center();
476  vol += (*it)->bv;
477  }
478  split_p /= (S)(num_leaves);
479  int best_axis = -1;
480  int bestmidp = num_leaves;
481  int splitcount[3][2] = {{0,0}, {0,0}, {0,0}};
482  for(it = lbeg; it < lend; ++it)
483  {
484  Vector3<S> x = (*it)->bv.center() - split_p;
485  for(size_t j = 0; j < 3; ++j)
486  ++splitcount[j][x[j] > 0 ? 1 : 0];
487  }
488 
489  for(size_t i = 0; i < 3; ++i)
490  {
491  if((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
492  {
493  int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
494  if(midp < bestmidp)
495  {
496  best_axis = i;
497  bestmidp = midp;
498  }
499  }
500  }
501 
502  if(best_axis < 0) best_axis = 0;
503 
504  S split_value = split_p[best_axis];
505  NodeVecIterator lcenter = lbeg;
506  for(it = lbeg; it < lend; ++it)
507  {
508  if((*it)->bv.center()[best_axis] < split_value)
509  {
510  NodeType* temp = *it;
511  *it = *lcenter;
512  *lcenter = temp;
513  ++lcenter;
514  }
515  }
516 
517  NodeType* node = createNode(nullptr, vol, nullptr);
518  node->children[0] = topdown_1(lbeg, lcenter);
519  node->children[1] = topdown_1(lcenter, lend);
520  node->children[0]->parent = node;
521  node->children[1]->parent = node;
522  return node;
523  }
524  else
525  {
526  bottomup(lbeg, lend);
527  return *lbeg;
528  }
529  }
530  return *lbeg;
531 }
532 
533 //==============================================================================
534 template<typename BV>
535 void HierarchyTree<BV>::init_0(std::vector<NodeType*>& leaves)
536 {
537  clear();
538  root_node = topdown(leaves.begin(), leaves.end());
539  n_leaves = leaves.size();
540  max_lookahead_level = -1;
541  opath = 0;
542 }
543 
544 //==============================================================================
545 template<typename BV>
546 void HierarchyTree<BV>::init_1(std::vector<NodeType*>& leaves)
547 {
548  clear();
549 
550  BV bound_bv;
551  if(leaves.size() > 0)
552  bound_bv = leaves[0]->bv;
553  for(size_t i = 1; i < leaves.size(); ++i)
554  bound_bv += leaves[i]->bv;
555 
556  morton_functor<typename BV::S, uint32> coder(bound_bv);
557  for(size_t i = 0; i < leaves.size(); ++i)
558  leaves[i]->code = coder(leaves[i]->bv.center());
559 
560  std::sort(leaves.begin(), leaves.end(), SortByMorton());
561 
562  root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
563 
564  refit();
565  n_leaves = leaves.size();
566  max_lookahead_level = -1;
567  opath = 0;
568 }
569 
570 //==============================================================================
571 template<typename BV>
572 void HierarchyTree<BV>::init_2(std::vector<NodeType*>& leaves)
573 {
574  clear();
575 
576  BV bound_bv;
577  if(leaves.size() > 0)
578  bound_bv = leaves[0]->bv;
579  for(size_t i = 1; i < leaves.size(); ++i)
580  bound_bv += leaves[i]->bv;
581 
582  morton_functor<typename BV::S, uint32> coder(bound_bv);
583  for(size_t i = 0; i < leaves.size(); ++i)
584  leaves[i]->code = coder(leaves[i]->bv.center());
585 
586  std::sort(leaves.begin(), leaves.end(), SortByMorton());
587 
588  root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1);
589 
590  refit();
591  n_leaves = leaves.size();
592  max_lookahead_level = -1;
593  opath = 0;
594 }
595 
596 //==============================================================================
597 template<typename BV>
598 void HierarchyTree<BV>::init_3(std::vector<NodeType*>& leaves)
599 {
600  clear();
601 
602  BV bound_bv;
603  if(leaves.size() > 0)
604  bound_bv = leaves[0]->bv;
605  for(size_t i = 1; i < leaves.size(); ++i)
606  bound_bv += leaves[i]->bv;
607 
608  morton_functor<typename BV::S, uint32> coder(bound_bv);
609  for(size_t i = 0; i < leaves.size(); ++i)
610  leaves[i]->code = coder(leaves[i]->bv.center());
611 
612  std::sort(leaves.begin(), leaves.end(), SortByMorton());
613 
614  root_node = mortonRecurse_2(leaves.begin(), leaves.end());
615 
616  refit();
617  n_leaves = leaves.size();
618  max_lookahead_level = -1;
619  opath = 0;
620 }
621 
622 //==============================================================================
623 template<typename BV>
625 {
626  int num_leaves = lend - lbeg;
627  if(num_leaves > 1)
628  {
629  if(bits > 0)
630  {
631  NodeType dummy;
632  dummy.code = split;
633  NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
634 
635  if(lcenter == lbeg)
636  {
637  uint32 split2 = split | (1 << (bits - 1));
638  return mortonRecurse_0(lbeg, lend, split2, bits - 1);
639  }
640  else if(lcenter == lend)
641  {
642  uint32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
643  return mortonRecurse_0(lbeg, lend, split1, bits - 1);
644  }
645  else
646  {
647  uint32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
648  uint32 split2 = split | (1 << (bits - 1));
649 
650  NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
651  NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
652  NodeType* node = createNode(nullptr, nullptr);
653  node->children[0] = child1;
654  node->children[1] = child2;
655  child1->parent = node;
656  child2->parent = node;
657  return node;
658  }
659  }
660  else
661  {
662  NodeType* node = topdown(lbeg, lend);
663  return node;
664  }
665  }
666  else
667  return *lbeg;
668 }
669 
670 //==============================================================================
671 template<typename BV>
673 {
674  int num_leaves = lend - lbeg;
675  if(num_leaves > 1)
676  {
677  if(bits > 0)
678  {
679  NodeType dummy;
680  dummy.code = split;
681  NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton());
682 
683  if(lcenter == lbeg)
684  {
685  uint32 split2 = split | (1 << (bits - 1));
686  return mortonRecurse_1(lbeg, lend, split2, bits - 1);
687  }
688  else if(lcenter == lend)
689  {
690  uint32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
691  return mortonRecurse_1(lbeg, lend, split1, bits - 1);
692  }
693  else
694  {
695  uint32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
696  uint32 split2 = split | (1 << (bits - 1));
697 
698  NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
699  NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
700  NodeType* node = createNode(nullptr, nullptr);
701  node->children[0] = child1;
702  node->children[1] = child2;
703  child1->parent = node;
704  child2->parent = node;
705  return node;
706  }
707  }
708  else
709  {
710  NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
711  NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
712  NodeType* node = createNode(nullptr, nullptr);
713  node->children[0] = child1;
714  node->children[1] = child2;
715  child1->parent = node;
716  child2->parent = node;
717  return node;
718  }
719  }
720  else
721  return *lbeg;
722 }
723 
724 //==============================================================================
725 template<typename BV>
727 {
728  int num_leaves = lend - lbeg;
729  if(num_leaves > 1)
730  {
731  NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
732  NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
733  NodeType* node = createNode(nullptr, nullptr);
734  node->children[0] = child1;
735  node->children[1] = child2;
736  child1->parent = node;
737  child2->parent = node;
738  return node;
739  }
740  else
741  return *lbeg;
742 }
743 
744 //==============================================================================
745 template<typename BV>
746 void HierarchyTree<BV>::update_(NodeType* leaf, const BV& bv)
747 {
748  NodeType* root = removeLeaf(leaf);
749  if(root)
750  {
751  if(max_lookahead_level >= 0)
752  {
753  for(int i = 0; (i < max_lookahead_level) && root->parent; ++i)
754  root = root->parent;
755  }
756  else
757  root = root_node;
758  }
759 
760  leaf->bv = bv;
761  insertLeaf(root, leaf);
762 }
763 
764 //==============================================================================
765 template<typename BV>
767 {
768  NodeType* p = n->parent;
769  if(p > n)
770  {
771  int i = indexOf(n);
772  int j = 1 - i;
773  NodeType* s = p->children[j];
774  NodeType* q = p->parent;
775  if(q) q->children[indexOf(p)] = n; else r = n;
776  s->parent = n;
777  p->parent = n;
778  n->parent = q;
779  p->children[0] = n->children[0];
780  p->children[1] = n->children[1];
781  n->children[0]->parent = p;
782  n->children[1]->parent = p;
783  n->children[i] = p;
784  n->children[j] = s;
785  std::swap(p->bv, n->bv);
786  return p;
787  }
788  return n;
789 }
790 
791 //==============================================================================
792 template<typename BV>
794  NodeType* const leaf)
795 // Attempts to insert `leaf` into a subtree rooted at `sub_root`.
796 // 1. If the whole tree is empty, then `leaf` simply becomes the tree.
797 // 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
798 // `sub_root` is selected (the selection criteria is a black box for this
799 // algorithm), and the `sibling` leaf is replaced by an internal node with
800 // two children, `sibling` and `leaf`. The bounding volumes are updated as
801 // necessary.
802 {
803  if(!root_node)
804  {
805  // If the entire tree is empty, the node pointed by `leaf` variable will
806  // become the root of the tree.
807  root_node = leaf;
808  leaf->parent = nullptr;
809  return;
810  }
811  // Traverse the tree from the given `sub_root` down to an existing leaf
812  // node that we call `sibling`. The `sibling` node will eventually become
813  // the sibling of the given `leaf` node.
814  NodeType* sibling = sub_root;
815  while(!sibling->isLeaf())
816  {
817  sibling = sibling->children[select(*leaf, *(sibling->children[0]),
818  *(sibling->children[1]))];
819  }
820  NodeType* prev = sibling->parent;
821  // Create a new `node` that later will become the new parent of both the
822  // `sibling` and the given `leaf`.
823  NodeType* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
824  if(prev)
825  // If the parent `prev` of the `sibling` is an interior node, we will
826  // replace the `sibling` with the subtree {node {`sibling`, leaf}} like
827  // this:
828  // Before After
829  //
830  // ╱ ╱
831  // prev prev
832  // ╱ ╲ ─> ╱ ╲
833  // sibling ... node ...
834  // ╱ ╲
835  // sibling leaf
836  {
837  prev->children[indexOf(sibling)] = node;
838  node->children[0] = sibling; sibling->parent = node;
839  node->children[1] = leaf; leaf->parent = node;
840  // Now that we've inserted `leaf` some of the existing bounding
841  // volumes might not fully enclose their children. Walk up the tree
842  // looking for parents that don't already enclose their children
843  // and create a new tight-fitting bounding volume for those.
844  do
845  {
846  if(!prev->bv.contain(node->bv))
847  prev->bv = prev->children[0]->bv + prev->children[1]->bv;
848  else
849  break;
850  node = prev;
851  } while (nullptr != (prev = node->parent));
852  }
853  else
854  // If the `sibling` has no parent, i.e., the tree is a singleton,
855  // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
856  // this:
857  //
858  // node
859  // ╱ ╲
860  // sibling leaf
861  {
862  node->children[0] = sibling; sibling->parent = node;
863  node->children[1] = leaf; leaf->parent = node;
864  root_node = node;
865  }
866 
867  // Note that the above algorithm always adds the new `leaf` node as the right
868  // child, i.e., children[1]. Calling removeLeaf(l) followed by calling
869  // this function insertLeaf(l) where l is a left child will result in
870  // switching l and its sibling even if no object's pose has changed.
871 }
872 
873 //==============================================================================
874 template <typename BV>
877 // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
878 // and its parent with just its sibling. It then "tightens" the ancestor
879 // bounding volumes. Returns a pointer to the parent of the highest node whose
880 // BV changed due to the removal.
881  if(leaf == root_node)
882  {
883  // If the `leaf` node is the only node in the tree, the tree becomes empty.
884  root_node = nullptr;
885  return nullptr;
886  }
887  NodeType* parent = leaf->parent;
888  NodeType* prev = parent->parent;
889  NodeType* sibling = parent->children[1-indexOf(leaf)];
890  if(prev)
891  {
892  // If the parent node is not the root node, the sibling node will
893  // replace the parent node like this:
894  //
895  // Before After
896  // ... ...
897  // ╱ ╱
898  // prev prev
899  // ╱ ╲ ╱ ╲
900  // parent ... ─> sibling ...
901  // ╱ ╲ ╱╲
902  // leaf sibling ...
903  // ╱╲
904  // ...
905  //
906  // Step 1: replace the subtree {parent {leaf, sibling {...}}} with
907  // {sibling {...}}.
908  prev->children[indexOf(parent)] = sibling;
909  sibling->parent = prev;
910  deleteNode(parent);
911  // Step 2: tighten up the BVs of the ancestor nodes.
912  while(prev)
913  {
914  BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
915  if(!new_bv.equal(prev->bv))
916  {
917  prev->bv = new_bv;
918  prev = prev->parent;
919  }
920  else break;
921  }
922 
923  return prev ? prev : root_node;
924  }
925  else
926  {
927  // If the parent node is the root node, the sibling node will become the
928  // root of the whole tree like this:
929  //
930  // Before After
931  //
932  // parent
933  // ╱ ╲
934  // leaf sibling ─> sibling
935  // ╱╲ ╱╲
936  // ... ...
937  root_node = sibling;
938  sibling->parent = nullptr;
939  deleteNode(parent);
940  return root_node;
941  }
942 }
943 
944 //==============================================================================
945 template<typename BV>
946 void HierarchyTree<BV>::fetchLeaves(NodeType* root, std::vector<NodeType*>& leaves, int depth)
947 {
948  if((!root->isLeaf()) && depth)
949  {
950  fetchLeaves(root->children[0], leaves, depth-1);
951  fetchLeaves(root->children[1], leaves, depth-1);
952  deleteNode(root);
953  }
954  else
955  {
956  leaves.push_back(root);
957  }
958 }
959 
960 //==============================================================================
961 template<typename BV>
963 {
964  // node cannot be nullptr
965  return (node->parent->children[1] == node);
966 }
967 
968 //==============================================================================
969 template<typename BV>
971  const BV& bv,
972  void* data)
973 {
974  NodeType* node = createNode(parent, data);
975  node->bv = bv;
976  return node;
977 }
978 
979 //==============================================================================
980 template<typename BV>
982  const BV& bv1,
983  const BV& bv2,
984  void* data)
985 {
986  NodeType* node = createNode(parent, data);
987  node->bv = bv1 + bv2;
988  return node;
989 }
990 
991 //==============================================================================
992 template<typename BV>
994 {
995  NodeType* node = nullptr;
996  if(free_node)
997  {
998  node = free_node;
999  free_node = nullptr;
1000  }
1001  else
1002  node = new NodeType();
1003  node->parent = parent;
1004  node->data = data;
1005  node->children[1] = 0;
1006  return node;
1007 }
1008 
1009 //==============================================================================
1010 template<typename BV>
1012 {
1013  if(free_node != node)
1014  {
1015  delete free_node;
1016  free_node = node;
1017  }
1018 }
1019 
1020 //==============================================================================
1021 template<typename BV>
1023 {
1024  if(!node->isLeaf())
1025  {
1026  recurseDeleteNode(node->children[0]);
1027  recurseDeleteNode(node->children[1]);
1028  }
1029 
1030  if(node == root_node) root_node = nullptr;
1031  deleteNode(node);
1032 }
1033 
1034 //==============================================================================
1035 template<typename BV>
1037 {
1038  if(!node->isLeaf())
1039  {
1040  recurseRefit(node->children[0]);
1041  recurseRefit(node->children[1]);
1042  node->bv = node->children[0]->bv + node->children[1]->bv;
1043  }
1044  else
1045  return;
1046 }
1047 
1048 //==============================================================================
1049 template<typename BV>
1051 {
1052  if(a->bv.center()[d] < b->bv.center()[d]) return true;
1053  return false;
1054 }
1055 
1056 //==============================================================================
1057 template <typename S, typename BV>
1059 {
1060  static std::size_t run(
1061  const NodeBase<BV>& /*query*/,
1062  const NodeBase<BV>& /*node1*/,
1063  const NodeBase<BV>& /*node2*/)
1064  {
1065  return 0;
1066  }
1067 
1068  static std::size_t run(
1069  const BV& /*query*/,
1070  const NodeBase<BV>& /*node1*/,
1071  const NodeBase<BV>& /*node2*/)
1072  {
1073  return 0;
1074  }
1075 };
1076 
1077 //==============================================================================
1078 template<typename BV>
1079 size_t select(
1080  const NodeBase<BV>& query,
1081  const NodeBase<BV>& node1,
1082  const NodeBase<BV>& node2)
1083 {
1084  return SelectImpl<typename BV::S, BV>::run(query, node1, node2);
1085 }
1086 
1087 //==============================================================================
1088 template<typename BV>
1089 size_t select(
1090  const BV& query,
1091  const NodeBase<BV>& node1,
1092  const NodeBase<BV>& node2)
1093 {
1094  return SelectImpl<typename BV::S, BV>::run(query, node1, node2);
1095 }
1096 
1097 //==============================================================================
1098 template <typename S>
1099 struct SelectImpl<S, AABB<S>>
1100 {
1101  static std::size_t run(
1102  const NodeBase<AABB<S>>& node,
1103  const NodeBase<AABB<S>>& node1,
1104  const NodeBase<AABB<S>>& node2)
1105  {
1106  const AABB<S>& bv = node.bv;
1107  const AABB<S>& bv1 = node1.bv;
1108  const AABB<S>& bv2 = node2.bv;
1109  Vector3<S> v = bv.min_ + bv.max_;
1110  Vector3<S> v1 = v - (bv1.min_ + bv1.max_);
1111  Vector3<S> v2 = v - (bv2.min_ + bv2.max_);
1112  S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
1113  S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
1114  return (d1 < d2) ? 0 : 1;
1115  }
1116 
1117  static std::size_t run(
1118  const AABB<S>& query,
1119  const NodeBase<AABB<S>>& node1,
1120  const NodeBase<AABB<S>>& node2)
1121  {
1122  const AABB<S>& bv = query;
1123  const AABB<S>& bv1 = node1.bv;
1124  const AABB<S>& bv2 = node2.bv;
1125  Vector3<S> v = bv.min_ + bv.max_;
1126  Vector3<S> v1 = v - (bv1.min_ + bv1.max_);
1127  Vector3<S> v2 = v - (bv2.min_ + bv2.max_);
1128  S d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
1129  S d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
1130  return (d1 < d2) ? 0 : 1;
1131  }
1132 };
1133 
1134 } // namespace detail
1135 } // namespace fcl
1136 
1137 #endif
fcl::detail::HierarchyTree::init
void init(std::vector< NodeType * > &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree-inl.h:71
fcl::detail::HierarchyTree::insert
NodeType * insert(const BV &bv, void *data)
Insest a node.
Definition: hierarchy_tree-inl.h:94
fcl::detail::HierarchyTree::topdown_0
NodeType * topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. During construction...
Definition: hierarchy_tree-inl.h:425
fcl::detail::select
size_t select(const BV &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for no...
Definition: hierarchy_tree-inl.h:1089
fcl::detail::HierarchyTree::balanceIncremental
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree-inl.h:263
fcl::detail::HierarchyTree::getMaxDepth
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree-inl.h:225
fcl::uint32
std::uint32_t uint32
Definition: types.h:62
fcl::detail::NodeBase::parent
NodeBase< BV > * parent
pointer to parent node
Definition: node_base.h:57
fcl::detail::HierarchyTree< fcl::AABB< S > >::S
typename fcl::AABB< S > ::S S
Definition: hierarchy_tree.h:62
swap
static void swap(T &x, T &y)
Definition: svm.cpp:54
fcl::detail::SelectImpl::run
static std::size_t run(const BV &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:1068
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::HierarchyTree::remove
void remove(NodeType *leaf)
Remove a leaf node.
Definition: hierarchy_tree-inl.h:104
hierarchy_tree.h
fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:49
fcl::detail::NodeBase::children
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:68
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::HierarchyTree::print
void print(NodeType *root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree-inl.h:327
fcl::detail::HierarchyTree::clear
void clear()
Clear the tree.
Definition: hierarchy_tree-inl.h:113
fcl::detail::SelectImpl
Definition: hierarchy_tree-inl.h:1058
fcl::detail::NodeBase::data
void * data
Definition: node_base.h:69
fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:51
fcl::AABB::max_
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
fcl::detail::HierarchyTree::update_
void update_(NodeType *leaf, const BV &bv)
update one leaf node's bounding volume
Definition: hierarchy_tree-inl.h:746
fcl::detail::SelectImpl< S, AABB< S > >::run
static std::size_t run(const NodeBase< AABB< S >> &node, const NodeBase< AABB< S >> &node1, const NodeBase< AABB< S >> &node2)
Definition: hierarchy_tree-inl.h:1101
prev
EndPoint * prev[3]
the previous end point in the end point list
Definition: broadphase_SaP.h:187
fcl::AABB::min_
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
fcl::detail::HierarchyTree::getRoot
NodeType * getRoot() const
get the root of the tree
Definition: hierarchy_tree-inl.h:313
fcl::detail::UpdateImpl::run
static bool run(const HierarchyTree< BV > &tree, typename HierarchyTree< BV >::NodeType *leaf, const BV &bv, const Vector3< S > &)
Definition: hierarchy_tree-inl.h:188
fcl::detail::HierarchyTree::balanceTopdown
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree-inl.h:250
fcl::detail::nodeBaseLess
bool nodeBaseLess(NodeBase< BV > *a, NodeBase< BV > *b, int d)
Compare two nodes accoording to the d-th dimension of node center.
Definition: hierarchy_tree-inl.h:1050
fcl::detail::UpdateImpl::run
static bool run(const HierarchyTree< BV > &tree, typename HierarchyTree< BV >::NodeType *leaf, const BV &bv, const Vector3< S > &, S)
Definition: hierarchy_tree-inl.h:176
r
S r
Definition: test_sphere_box.cpp:171
fcl::detail::HierarchyTree::size
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree-inl.h:306
fcl::detail::HierarchyTree::balanceBottomup
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree-inl.h:236
fcl::detail::NodeBase::code
uint32 code
morton code for current BV
Definition: node_base.h:73
fcl::detail::HierarchyTree< fcl::AABB< S > >::NodeVecIterator
std::vector< NodeBase< fcl::AABB< S > > * >::iterator NodeVecIterator
Definition: hierarchy_tree.h:145
fcl::detail::NodeBase::bv
BV bv
the bounding volume for the node
Definition: node_base.h:54
fcl::detail::SelectImpl< S, AABB< S > >::run
static std::size_t run(const AABB< S > &query, const NodeBase< AABB< S >> &node1, const NodeBase< AABB< S >> &node2)
Definition: hierarchy_tree-inl.h:1117
fcl::detail::UpdateImpl
Definition: hierarchy_tree-inl.h:174
fcl::detail::HierarchyTree::extractLeaves
void extractLeaves(const NodeType *root, std::vector< NodeType * > &leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree-inl.h:293
fcl::detail::NodeBase::isLeaf
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:57
fcl::detail::HierarchyTree::topdown
NodeType * topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from top
Definition: hierarchy_tree-inl.h:381
fcl::detail::HierarchyTree::update
void update(NodeType *leaf, int lookahead_level=-1)
Updates a leaf node. A use case is when the bounding volume of an object changes. Ensure every parent...
Definition: hierarchy_tree-inl.h:133
fcl::detail::HierarchyTree::~HierarchyTree
~HierarchyTree()
Definition: hierarchy_tree-inl.h:64
fcl::detail::HierarchyTree::HierarchyTree
HierarchyTree(int bu_threshold_=16, int topdown_level_=0)
Create hierarchy tree with suitable setting. bu_threshold decides the height of tree node to start bo...
Definition: hierarchy_tree-inl.h:51
fcl::detail::SelectImpl::run
static std::size_t run(const NodeBase< BV > &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:1060
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::HierarchyTree::bottomup
void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from bottom – very heavy way
Definition: hierarchy_tree-inl.h:344
fcl::detail::HierarchyTree::refit
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
Definition: hierarchy_tree-inl.h:285
fcl::detail::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:58
fcl::detail::HierarchyTree::getMaxHeight
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree-inl.h:216
fcl::detail::HierarchyTree::empty
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree-inl.h:126


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:51