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 HPP_FCL_HIERARCHY_TREE_INL_H
39 #define HPP_FCL_HIERARCHY_TREE_INL_H
40 
42 
43 namespace hpp {
44 namespace fcl {
45 
46 namespace detail {
47 
48 //==============================================================================
49 template <typename BV>
50 HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
51  root_node = nullptr;
52  n_leaves = 0;
53  free_node = nullptr;
54  max_lookahead_level = -1;
55  opath = 0;
56  bu_threshold = bu_threshold_;
57  topdown_level = topdown_level_;
58 }
59 
60 //==============================================================================
61 template <typename BV>
63  clear();
64 }
65 
66 //==============================================================================
67 template <typename BV>
68 void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) {
69  switch (level) {
70  case 0:
71  init_0(leaves);
72  break;
73  case 1:
74  init_1(leaves);
75  break;
76  case 2:
77  init_2(leaves);
78  break;
79  case 3:
80  init_3(leaves);
81  break;
82  default:
83  init_0(leaves);
84  }
85 }
86 
87 //==============================================================================
88 template <typename BV>
90  void* data) {
91  Node* leaf = createNode(nullptr, bv, data);
92  insertLeaf(root_node, leaf);
93  ++n_leaves;
94  return leaf;
95 }
96 
97 //==============================================================================
98 template <typename BV>
100  removeLeaf(leaf);
101  deleteNode(leaf);
102  --n_leaves;
103 }
104 
105 //==============================================================================
106 template <typename BV>
108  if (root_node) recurseDeleteNode(root_node);
109  n_leaves = 0;
110  delete free_node;
111  free_node = nullptr;
112  max_lookahead_level = -1;
113  opath = 0;
114 }
115 
116 //==============================================================================
117 template <typename BV>
119  return (nullptr == root_node);
120 }
121 
122 //==============================================================================
123 template <typename BV>
124 void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) {
125  // TODO(DamrongGuoy): Since we update a leaf node by removing and
126  // inserting the same leaf node, it is likely to change the structure of
127  // the tree even if no object's pose has changed. In the future,
128  // find a way to preserve the structure of the tree to solve this issue:
129  // https://github.com/flexible-collision-library/fcl/issues/368
130 
131  // First we remove the leaf node pointed by `leaf` variable from the tree.
132  // The `sub_root` variable is the root of the subtree containing nodes
133  // whose BVs were adjusted due to node removal.
134  typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf);
135  if (sub_root) {
136  if (lookahead_level > 0) {
137  // For positive `lookahead_level`, we move the `sub_root` variable up.
138  for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i)
139  sub_root = sub_root->parent;
140  } else
141  // By default, lookahead_level = -1, and we reset the `sub_root` variable
142  // to the root of the entire tree.
143  sub_root = root_node;
144  }
145  // Then we insert the node pointed by `leaf` variable back into the
146  // subtree rooted at `sub_root` variable.
147  insertLeaf(sub_root, leaf);
148 }
149 
150 //==============================================================================
151 template <typename BV>
152 bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) {
153  if (leaf->bv.contain(bv)) return false;
154  update_(leaf, bv);
155  return true;
156 }
157 
158 //==============================================================================
159 template <typename S, typename BV>
160 struct UpdateImpl {
161  static bool run(const HierarchyTree<BV>& tree,
162  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
163  const Vec3f& /*vel*/, FCL_REAL /*margin*/) {
164  if (leaf->bv.contain(bv)) return false;
165  tree.update_(leaf, bv);
166  return true;
167  }
168 
169  static bool run(const HierarchyTree<BV>& tree,
170  typename HierarchyTree<BV>::Node* leaf, const BV& bv,
171  const Vec3f& /*vel*/) {
172  if (leaf->bv.contain(bv)) return false;
173  tree.update_(leaf, bv);
174  return true;
175  }
176 };
177 
178 //==============================================================================
179 template <typename BV>
180 bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel,
181  FCL_REAL margin) {
182  return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel, margin);
183 }
184 
185 //==============================================================================
186 template <typename BV>
187 bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3f& vel) {
188  return UpdateImpl<FCL_REAL, BV>::run(*this, leaf, bv, vel);
189 }
190 
191 //==============================================================================
192 template <typename BV>
194  if (!root_node) return 0;
195  return getMaxHeight(root_node);
196 }
197 
198 //==============================================================================
199 template <typename BV>
201  if (!root_node) return 0;
202 
203  size_t max_depth;
204  getMaxDepth(root_node, 0, max_depth);
205  return max_depth;
206 }
207 
208 //==============================================================================
209 template <typename BV>
211  if (root_node) {
212  std::vector<Node*> leaves;
213  leaves.reserve(n_leaves);
214  fetchLeaves(root_node, leaves);
215  bottomup(leaves.begin(), leaves.end());
216  root_node = leaves[0];
217  }
218 }
219 
220 //==============================================================================
221 template <typename BV>
223  if (root_node) {
224  std::vector<Node*> leaves;
225  leaves.reserve(n_leaves);
226  fetchLeaves(root_node, leaves);
227  root_node = topdown(leaves.begin(), leaves.end());
228  }
229 }
230 
231 //==============================================================================
232 template <typename BV>
234  if (iterations < 0) iterations = (int)n_leaves;
235  if (root_node && (iterations > 0)) {
236  for (int i = 0; i < iterations; ++i) {
237  Node* node = root_node;
238  unsigned int bit = 0;
239  while (!node->isLeaf()) {
240  node = sort(node, root_node)->children[(opath >> bit) & 1];
241  bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
242  }
243  update(node);
244  ++opath;
245  }
246  }
247 }
248 
249 //==============================================================================
250 template <typename BV>
252  if (root_node) recurseRefit(root_node);
253 }
254 
255 //==============================================================================
256 template <typename BV>
258  std::vector<Node*>& leaves) const {
259  if (!root->isLeaf()) {
260  extractLeaves(root->children[0], leaves);
261  extractLeaves(root->children[1], leaves);
262  } else
263  leaves.push_back(root);
264 }
265 
266 //==============================================================================
267 template <typename BV>
268 size_t HierarchyTree<BV>::size() const {
269  return n_leaves;
270 }
271 
272 //==============================================================================
273 template <typename BV>
275  return root_node;
276 }
277 
278 //==============================================================================
279 template <typename BV>
281  return root_node;
282 }
283 
284 //==============================================================================
285 template <typename BV>
286 void HierarchyTree<BV>::print(Node* root, int depth) {
287  for (int i = 0; i < depth; ++i) std::cout << " ";
288  std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", "
289  << root->bv.min_[2] << "; " << root->bv.max_[0] << ", "
290  << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl;
291  if (root->isLeaf()) {
292  } else {
293  print(root->children[0], depth + 1);
294  print(root->children[1], depth + 1);
295  }
296 }
297 
298 //==============================================================================
299 template <typename BV>
301  const NodeVecIterator lend) {
302  NodeVecIterator lcur_end = lend;
303  while (lbeg < lcur_end - 1) {
304  NodeVecIterator min_it1 = lbeg;
305  NodeVecIterator min_it2 = lbeg + 1;
306  FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)();
307  for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) {
308  for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) {
309  FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size();
310  if (cur_size < min_size) {
311  min_size = cur_size;
312  min_it1 = it1;
313  min_it2 = it2;
314  }
315  }
316  }
317 
318  Node* n[2] = {*min_it1, *min_it2};
319  Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr);
320  p->children[0] = n[0];
321  p->children[1] = n[1];
322  n[0]->parent = p;
323  n[1]->parent = p;
324  *min_it1 = p;
325  Node* tmp = *min_it2;
326  lcur_end--;
327  *min_it2 = *lcur_end;
328  *lcur_end = tmp;
329  }
330 }
331 
332 //==============================================================================
333 template <typename BV>
335  const NodeVecIterator lbeg, const NodeVecIterator lend) {
336  switch (topdown_level) {
337  case 0:
338  return topdown_0(lbeg, lend);
339  break;
340  case 1:
341  return topdown_1(lbeg, lend);
342  break;
343  default:
344  return topdown_0(lbeg, lend);
345  }
346 }
347 
348 //==============================================================================
349 template <typename BV>
351  if (!node->isLeaf()) {
352  size_t height1 = getMaxHeight(node->children[0]);
353  size_t height2 = getMaxHeight(node->children[1]);
354  return std::max(height1, height2) + 1;
355  } else
356  return 0;
357 }
358 
359 //==============================================================================
360 template <typename BV>
361 void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth,
362  size_t& max_depth) const {
363  if (!node->isLeaf()) {
364  getMaxDepth(node->children[0], depth + 1, max_depth);
365  getMaxDepth(node->children[1], depth + 1, max_depth);
366  } else
367  max_depth = std::max(max_depth, depth);
368 }
369 
370 //==============================================================================
371 template <typename BV>
373  const NodeVecIterator lbeg, const NodeVecIterator lend) {
374  long num_leaves = lend - lbeg;
375  if (num_leaves > 1) {
376  if (num_leaves > bu_threshold) {
377  BV vol = (*lbeg)->bv;
378  for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv;
379 
380  int best_axis = 0;
381  FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
382  if (extent[1] > extent[0]) best_axis = 1;
383  if (extent[2] > extent[best_axis]) best_axis = 2;
384 
385  // compute median
386  NodeVecIterator lcenter = lbeg + num_leaves / 2;
387  std::nth_element(lbeg, lcenter, lend,
388  std::bind(&nodeBaseLess<BV>, std::placeholders::_1,
389  std::placeholders::_2, std::ref(best_axis)));
390 
391  Node* node = createNode(nullptr, vol, nullptr);
392  node->children[0] = topdown_0(lbeg, lcenter);
393  node->children[1] = topdown_0(lcenter, lend);
394  node->children[0]->parent = node;
395  node->children[1]->parent = node;
396  return node;
397  } else {
398  bottomup(lbeg, lend);
399  return *lbeg;
400  }
401  }
402  return *lbeg;
403 }
404 
405 //==============================================================================
406 template <typename BV>
408  const NodeVecIterator lbeg, const NodeVecIterator lend) {
409  long num_leaves = lend - lbeg;
410  if (num_leaves > 1) {
411  if (num_leaves > bu_threshold) {
412  Vec3f split_p = (*lbeg)->bv.center();
413  BV vol = (*lbeg)->bv;
414  NodeVecIterator it;
415  for (it = lbeg + 1; it < lend; ++it) {
416  split_p += (*it)->bv.center();
417  vol += (*it)->bv;
418  }
419  split_p /= static_cast<FCL_REAL>(num_leaves);
420  int best_axis = -1;
421  long bestmidp = num_leaves;
422  int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
423  for (it = lbeg; it < lend; ++it) {
424  Vec3f x = (*it)->bv.center() - split_p;
425  for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
426  }
427 
428  for (int i = 0; i < 3; ++i) {
429  if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
430  long midp = std::abs(splitcount[i][0] - splitcount[i][1]);
431  if (midp < bestmidp) {
432  best_axis = i;
433  bestmidp = midp;
434  }
435  }
436  }
437 
438  if (best_axis < 0) best_axis = 0;
439 
440  FCL_REAL split_value = split_p[best_axis];
441  NodeVecIterator lcenter = lbeg;
442  for (it = lbeg; it < lend; ++it) {
443  if ((*it)->bv.center()[best_axis] < split_value) {
444  Node* temp = *it;
445  *it = *lcenter;
446  *lcenter = temp;
447  ++lcenter;
448  }
449  }
450 
451  Node* node = createNode(nullptr, vol, nullptr);
452  node->children[0] = topdown_1(lbeg, lcenter);
453  node->children[1] = topdown_1(lcenter, lend);
454  node->children[0]->parent = node;
455  node->children[1]->parent = node;
456  return node;
457  } else {
458  bottomup(lbeg, lend);
459  return *lbeg;
460  }
461  }
462  return *lbeg;
463 }
464 
465 //==============================================================================
466 template <typename BV>
467 void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) {
468  clear();
469  root_node = topdown(leaves.begin(), leaves.end());
470  n_leaves = leaves.size();
471  max_lookahead_level = -1;
472  opath = 0;
473 }
474 
475 //==============================================================================
476 template <typename BV>
477 void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) {
478  clear();
479 
480  BV bound_bv;
481  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
482  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
483 
484  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
485  for (size_t i = 0; i < leaves.size(); ++i)
486  leaves[i]->code = coder(leaves[i]->bv.center());
487 
488  std::sort(leaves.begin(), leaves.end(), SortByMorton());
489 
490  root_node = mortonRecurse_0(leaves.begin(), leaves.end(),
491  (1 << (coder.bits() - 1)), coder.bits() - 1);
492 
493  refit();
494  n_leaves = leaves.size();
495  max_lookahead_level = -1;
496  opath = 0;
497 }
498 
499 //==============================================================================
500 template <typename BV>
501 void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) {
502  clear();
503 
504  BV bound_bv;
505  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
506  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
507 
508  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
509  for (size_t i = 0; i < leaves.size(); ++i)
510  leaves[i]->code = coder(leaves[i]->bv.center());
511 
512  std::sort(leaves.begin(), leaves.end(), SortByMorton());
513 
514  root_node = mortonRecurse_1(leaves.begin(), leaves.end(),
515  (1 << (coder.bits() - 1)), coder.bits() - 1);
516 
517  refit();
518  n_leaves = leaves.size();
519  max_lookahead_level = -1;
520  opath = 0;
521 }
522 
523 //==============================================================================
524 template <typename BV>
525 void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) {
526  clear();
527 
528  BV bound_bv;
529  if (leaves.size() > 0) bound_bv = leaves[0]->bv;
530  for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv;
531 
532  morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
533  for (size_t i = 0; i < leaves.size(); ++i)
534  leaves[i]->code = coder(leaves[i]->bv.center());
535 
536  std::sort(leaves.begin(), leaves.end(), SortByMorton());
537 
538  root_node = mortonRecurse_2(leaves.begin(), leaves.end());
539 
540  refit();
541  n_leaves = leaves.size();
542  max_lookahead_level = -1;
543  opath = 0;
544 }
545 
546 //==============================================================================
547 template <typename BV>
549  const NodeVecIterator lbeg, const NodeVecIterator lend,
550  const uint32_t& split, int bits) {
551  long num_leaves = lend - lbeg;
552  if (num_leaves > 1) {
553  if (bits > 0) {
554  Node dummy;
555  dummy.code = split;
556  NodeVecIterator lcenter =
557  std::lower_bound(lbeg, lend, &dummy, SortByMorton());
558 
559  if (lcenter == lbeg) {
560  uint32_t split2 = split | (1 << (bits - 1));
561  return mortonRecurse_0(lbeg, lend, split2, bits - 1);
562  } else if (lcenter == lend) {
563  uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
564  return mortonRecurse_0(lbeg, lend, split1, bits - 1);
565  } else {
566  uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
567  uint32_t split2 = split | (1 << (bits - 1));
568 
569  Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
570  Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
571  Node* node = createNode(nullptr, nullptr);
572  node->children[0] = child1;
573  node->children[1] = child2;
574  child1->parent = node;
575  child2->parent = node;
576  return node;
577  }
578  } else {
579  Node* node = topdown(lbeg, lend);
580  return node;
581  }
582  } else
583  return *lbeg;
584 }
585 
586 //==============================================================================
587 template <typename BV>
589  const NodeVecIterator lbeg, const NodeVecIterator lend,
590  const uint32_t& split, int bits) {
591  long num_leaves = lend - lbeg;
592  if (num_leaves > 1) {
593  if (bits > 0) {
594  Node dummy;
595  dummy.code = split;
596  NodeVecIterator lcenter =
597  std::lower_bound(lbeg, lend, &dummy, SortByMorton());
598 
599  if (lcenter == lbeg) {
600  uint32_t split2 = split | (1 << (bits - 1));
601  return mortonRecurse_1(lbeg, lend, split2, bits - 1);
602  } else if (lcenter == lend) {
603  uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
604  return mortonRecurse_1(lbeg, lend, split1, bits - 1);
605  } else {
606  uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
607  uint32_t split2 = split | (1 << (bits - 1));
608 
609  Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
610  Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
611  Node* node = createNode(nullptr, nullptr);
612  node->children[0] = child1;
613  node->children[1] = child2;
614  child1->parent = node;
615  child2->parent = node;
616  return node;
617  }
618  } else {
619  Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
620  Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
621  Node* node = createNode(nullptr, nullptr);
622  node->children[0] = child1;
623  node->children[1] = child2;
624  child1->parent = node;
625  child2->parent = node;
626  return node;
627  }
628  } else
629  return *lbeg;
630 }
631 
632 //==============================================================================
633 template <typename BV>
635  const NodeVecIterator lbeg, const NodeVecIterator lend) {
636  long num_leaves = lend - lbeg;
637  if (num_leaves > 1) {
638  Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
639  Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
640  Node* node = createNode(nullptr, nullptr);
641  node->children[0] = child1;
642  node->children[1] = child2;
643  child1->parent = node;
644  child2->parent = node;
645  return node;
646  } else
647  return *lbeg;
648 }
649 
650 //==============================================================================
651 template <typename BV>
652 void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) {
653  Node* root = removeLeaf(leaf);
654  if (root) {
655  if (max_lookahead_level >= 0) {
656  for (int i = 0; (i < max_lookahead_level) && root->parent; ++i)
657  root = root->parent;
658  } else
659  root = root_node;
660  }
661 
662  leaf->bv = bv;
663  insertLeaf(root, leaf);
664 }
665 
666 //==============================================================================
667 template <typename BV>
669  Node* p = n->parent;
670  if (p > n) {
671  size_t i = indexOf(n);
672  size_t j = 1 - i;
673  Node* s = p->children[j];
674  Node* q = p->parent;
675  if (q)
676  q->children[indexOf(p)] = n;
677  else
678  r = n;
679  s->parent = n;
680  p->parent = n;
681  n->parent = q;
682  p->children[0] = n->children[0];
683  p->children[1] = n->children[1];
684  n->children[0]->parent = p;
685  n->children[1]->parent = p;
686  n->children[i] = p;
687  n->children[j] = s;
688  std::swap(p->bv, n->bv);
689  return p;
690  }
691  return n;
692 }
693 
694 //==============================================================================
695 template <typename BV>
696 void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf)
697 // Attempts to insert `leaf` into a subtree rooted at `sub_root`.
698 // 1. If the whole tree is empty, then `leaf` simply becomes the tree.
699 // 2. Otherwise, a leaf node called `sibling` of the subtree rooted at
700 // `sub_root` is selected (the selection criteria is a black box for this
701 // algorithm), and the `sibling` leaf is replaced by an internal node with
702 // two children, `sibling` and `leaf`. The bounding volumes are updated as
703 // necessary.
704 {
705  if (!root_node) {
706  // If the entire tree is empty, the node pointed by `leaf` variable will
707  // become the root of the tree.
708  root_node = leaf;
709  leaf->parent = nullptr;
710  return;
711  }
712  // Traverse the tree from the given `sub_root` down to an existing leaf
713  // node that we call `sibling`. The `sibling` node will eventually become
714  // the sibling of the given `leaf` node.
715  Node* sibling = sub_root;
716  while (!sibling->isLeaf()) {
717  sibling = sibling->children[select(*leaf, *(sibling->children[0]),
718  *(sibling->children[1]))];
719  }
720  Node* prev = sibling->parent;
721  // Create a new `node` that later will become the new parent of both the
722  // `sibling` and the given `leaf`.
723  Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr);
724  if (prev)
725  // If the parent `prev` of the `sibling` is an interior node, we will
726  // replace the `sibling` with the subtree {node {`sibling`, leaf}} like
727  // this:
728  // Before After
729  //
730  // ╱ ╱
731  // prev prev
732  // ╱ ╲ ─> ╱ ╲
733  // sibling ... node ...
734  // ╱ ╲
735  // sibling leaf
736  {
737  prev->children[indexOf(sibling)] = node;
738  node->children[0] = sibling;
739  sibling->parent = node;
740  node->children[1] = leaf;
741  leaf->parent = node;
742  // Now that we've inserted `leaf` some of the existing bounding
743  // volumes might not fully enclose their children. Walk up the tree
744  // looking for parents that don't already enclose their children
745  // and create a new tight-fitting bounding volume for those.
746  do {
747  if (!prev->bv.contain(node->bv))
748  prev->bv = prev->children[0]->bv + prev->children[1]->bv;
749  else
750  break;
751  node = prev;
752  } while (nullptr != (prev = node->parent));
753  } else
754  // If the `sibling` has no parent, i.e., the tree is a singleton,
755  // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like
756  // this:
757  //
758  // node
759  // ╱ ╲
760  // sibling leaf
761  {
762  node->children[0] = sibling;
763  sibling->parent = node;
764  node->children[1] = leaf;
765  leaf->parent = node;
766  root_node = node;
767  }
768 
769  // Note that the above algorithm always adds the new `leaf` node as the right
770  // child, i.e., children[1]. Calling removeLeaf(l) followed by calling
771  // this function insertLeaf(l) where l is a left child will result in
772  // switching l and its sibling even if no object's pose has changed.
773 }
774 
775 //==============================================================================
776 template <typename BV>
778  Node* const leaf) {
779  // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling,
780  // and its parent with just its sibling. It then "tightens" the ancestor
781  // bounding volumes. Returns a pointer to the parent of the highest node whose
782  // BV changed due to the removal.
783  if (leaf == root_node) {
784  // If the `leaf` node is the only node in the tree, the tree becomes empty.
785  root_node = nullptr;
786  return nullptr;
787  }
788  Node* parent = leaf->parent;
789  Node* prev = parent->parent;
790  Node* sibling = parent->children[1 - indexOf(leaf)];
791  if (prev) {
792  // If the parent node is not the root node, the sibling node will
793  // replace the parent node like this:
794  //
795  // Before After
796  // ... ...
797  // ╱ ╱
798  // prev prev
799  // ╱ ╲ ╱ ╲
800  // parent ... ─> sibling ...
801  // ╱ ╲ ╱╲
802  // leaf sibling ...
803  // ╱╲
804  // ...
805  //
806  // Step 1: replace the subtree {parent {leaf, sibling {...}}} with
807  // {sibling {...}}.
808  prev->children[indexOf(parent)] = sibling;
809  sibling->parent = prev;
810  deleteNode(parent);
811  // Step 2: tighten up the BVs of the ancestor nodes.
812  while (prev) {
813  BV new_bv = prev->children[0]->bv + prev->children[1]->bv;
814  if (!(new_bv == prev->bv)) {
815  prev->bv = new_bv;
816  prev = prev->parent;
817  } else
818  break;
819  }
820 
821  return prev ? prev : root_node;
822  } else {
823  // If the parent node is the root node, the sibling node will become the
824  // root of the whole tree like this:
825  //
826  // Before After
827  //
828  // parent
829  // ╱ ╲
830  // leaf sibling ─> sibling
831  // ╱╲ ╱╲
832  // ... ...
833  root_node = sibling;
834  sibling->parent = nullptr;
835  deleteNode(parent);
836  return root_node;
837  }
838 }
839 
840 //==============================================================================
841 template <typename BV>
842 void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves,
843  int depth) {
844  if ((!root->isLeaf()) && depth) {
845  fetchLeaves(root->children[0], leaves, depth - 1);
846  fetchLeaves(root->children[1], leaves, depth - 1);
847  deleteNode(root);
848  } else {
849  leaves.push_back(root);
850  }
851 }
852 
853 //==============================================================================
854 template <typename BV>
856  // node cannot be nullptr
857  return (node->parent->children[1] == node);
858 }
859 
860 //==============================================================================
861 template <typename BV>
863  const BV& bv,
864  void* data) {
865  Node* node = createNode(parent, data);
866  node->bv = bv;
867  return node;
868 }
869 
870 //==============================================================================
871 template <typename BV>
873  const BV& bv1,
874  const BV& bv2,
875  void* data) {
876  Node* node = createNode(parent, data);
877  node->bv = bv1 + bv2;
878  return node;
879 }
880 
881 //==============================================================================
882 template <typename BV>
884  void* data) {
885  Node* node = nullptr;
886  if (free_node) {
887  node = free_node;
888  free_node = nullptr;
889  } else
890  node = new Node();
891  node->parent = parent;
892  node->data = data;
893  node->children[1] = 0;
894  return node;
895 }
896 
897 //==============================================================================
898 template <typename BV>
900  if (free_node != node) {
901  delete free_node;
902  free_node = node;
903  }
904 }
905 
906 //==============================================================================
907 template <typename BV>
909  if (!node->isLeaf()) {
910  recurseDeleteNode(node->children[0]);
911  recurseDeleteNode(node->children[1]);
912  }
913 
914  if (node == root_node) root_node = nullptr;
915  deleteNode(node);
916 }
917 
918 //==============================================================================
919 template <typename BV>
921  if (!node->isLeaf()) {
922  recurseRefit(node->children[0]);
923  recurseRefit(node->children[1]);
924  node->bv = node->children[0]->bv + node->children[1]->bv;
925  } else
926  return;
927 }
928 
929 //==============================================================================
930 template <typename BV>
932  if (a->bv.center()[d] < b->bv.center()[d]) return true;
933  return false;
934 }
935 
936 //==============================================================================
937 template <typename S, typename BV>
938 struct SelectImpl {
939  static std::size_t run(const NodeBase<BV>& /*query*/,
940  const NodeBase<BV>& /*node1*/,
941  const NodeBase<BV>& /*node2*/) {
942  return 0;
943  }
944 
945  static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/,
946  const NodeBase<BV>& /*node2*/) {
947  return 0;
948  }
949 };
950 
951 //==============================================================================
952 template <typename BV>
953 size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
954  const NodeBase<BV>& node2) {
955  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
956 }
957 
958 //==============================================================================
959 template <typename BV>
960 size_t select(const BV& query, const NodeBase<BV>& node1,
961  const NodeBase<BV>& node2) {
962  return SelectImpl<FCL_REAL, BV>::run(query, node1, node2);
963 }
964 
965 //==============================================================================
966 template <typename S>
967 struct SelectImpl<S, AABB> {
968  static std::size_t run(const NodeBase<AABB>& node,
969  const NodeBase<AABB>& node1,
970  const NodeBase<AABB>& node2) {
971  const AABB& bv = node.bv;
972  const AABB& bv1 = node1.bv;
973  const AABB& bv2 = node2.bv;
974  Vec3f v = bv.min_ + bv.max_;
975  Vec3f v1 = v - (bv1.min_ + bv1.max_);
976  Vec3f v2 = v - (bv2.min_ + bv2.max_);
977  FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
978  FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
979  return (d1 < d2) ? 0 : 1;
980  }
981 
982  static std::size_t run(const AABB& query, const NodeBase<AABB>& node1,
983  const NodeBase<AABB>& node2) {
984  const AABB& bv = query;
985  const AABB& bv1 = node1.bv;
986  const AABB& bv2 = node2.bv;
987  Vec3f v = bv.min_ + bv.max_;
988  Vec3f v1 = v - (bv1.min_ + bv1.max_);
989  Vec3f v2 = v - (bv2.min_ + bv2.max_);
990  FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
991  FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
992  return (d1 < d2) ? 0 : 1;
993  }
994 };
995 
996 } // namespace detail
997 } // namespace fcl
998 } // namespace hpp
999 
1000 #endif
hpp::fcl::detail::NodeBase::data
void * data
Definition: node_base.h:66
hpp::fcl::detail::NodeBase::parent
NodeBase< BV > * parent
pointer to parent node
Definition: node_base.h:55
hpp::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:300
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::detail::HierarchyTree::balanceIncremental
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree-inl.h:233
hpp::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:251
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
hpp::fcl::detail::SelectImpl::run
static std::size_t run(const NodeBase< BV > &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:939
hpp::fcl::detail::NodeBase::code
uint32_t code
morton code for current BV
Definition: node_base.h:70
hpp::fcl::detail::NodeBase::isLeaf
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
hpp::fcl::detail::SelectImpl< S, AABB >::run
static std::size_t run(const NodeBase< AABB > &node, const NodeBase< AABB > &node1, const NodeBase< AABB > &node2)
Definition: hierarchy_tree-inl.h:968
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
data
data
hpp::fcl::detail::HierarchyTree< hpp::fcl::AABB >::NodeVecIterator
std::vector< NodeBase< hpp::fcl::AABB > * >::iterator NodeVecIterator
Definition: hierarchy_tree.h:143
hpp::fcl::detail::UpdateImpl::run
static bool run(const HierarchyTree< BV > &tree, typename HierarchyTree< BV >::Node *leaf, const BV &bv, const Vec3f &, FCL_REAL)
Definition: hierarchy_tree-inl.h:161
hpp::fcl::detail::HierarchyTree::extractLeaves
void extractLeaves(const Node *root, std::vector< Node * > &leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree-inl.h:257
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
hpp::fcl::detail::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
octree.r
r
Definition: octree.py:9
hierarchy_tree.h
hpp::fcl::detail::SelectImpl
Definition: hierarchy_tree-inl.h:938
print
void print(const Tensor &tensor)
hpp::fcl::detail::SelectImpl< S, AABB >::run
static std::size_t run(const AABB &query, const NodeBase< AABB > &node1, const NodeBase< AABB > &node2)
Definition: hierarchy_tree-inl.h:982
hpp::fcl::detail::HierarchyTree::topdown
Node * topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from top
Definition: hierarchy_tree-inl.h:334
a
list a
hpp::fcl::detail::NodeBase::bv
BV bv
the bounding volume for the node
Definition: node_base.h:52
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::detail::HierarchyTree::getMaxHeight
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree-inl.h:193
hpp::fcl::detail::HierarchyTree::clear
void clear()
Clear the tree.
Definition: hierarchy_tree-inl.h:107
hpp::fcl::detail::HierarchyTree::balanceTopdown
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree-inl.h:222
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::detail::HierarchyTree::print
void print(Node *root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree-inl.h:286
x
x
hpp::fcl::detail::UpdateImpl
Definition: hierarchy_tree-inl.h:160
hpp::fcl::detail::HierarchyTree::size
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree-inl.h:268
hpp::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:50
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: omniidl_be_python_with_docstring.py:140
q
q
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
hpp::fcl::detail::UpdateImpl::run
static bool run(const HierarchyTree< BV > &tree, typename HierarchyTree< BV >::Node *leaf, const BV &bv, const Vec3f &)
Definition: hierarchy_tree-inl.h:169
hpp::fcl::detail::SelectImpl::run
static std::size_t run(const BV &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:945
hpp::fcl::detail::HierarchyTree::update
void update(Node *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:124
hpp::fcl::detail::HierarchyTree::remove
void remove(Node *leaf)
Remove a leaf node.
Definition: hierarchy_tree-inl.h:99
hpp::fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:50
hpp::fcl::detail::HierarchyTree::topdown_0
Node * 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:372
hpp::fcl::detail::HierarchyTree::getRoot
Node * getRoot() const
get the root of the tree
Definition: hierarchy_tree-inl.h:274
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::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:931
hpp::fcl::detail::HierarchyTree::insert
Node * insert(const BV &bv, void *data)
Insest a node.
Definition: hierarchy_tree-inl.h:89
hpp::fcl::detail::HierarchyTree::~HierarchyTree
~HierarchyTree()
Definition: hierarchy_tree-inl.h:62
hpp::fcl::detail::HierarchyTree::balanceBottomup
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree-inl.h:210
hpp::fcl::detail::HierarchyTree::empty
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree-inl.h:118
hpp::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:960
hpp::fcl::detail::NodeBase::children
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
hpp::fcl::detail::HierarchyTree::getMaxDepth
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree-inl.h:200
hpp::fcl::detail::HierarchyTree::init
void init(std::vector< Node * > &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree-inl.h:68
obb.v
list v
Definition: obb.py:48
hpp::fcl::detail::HierarchyTree::update_
void update_(Node *leaf, const BV &bv)
update one leaf node's bounding volume
Definition: hierarchy_tree-inl.h:652


hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:13