hpp-fcl 2.4.4
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
43namespace hpp {
44namespace fcl {
45
46namespace detail {
47
48//==============================================================================
49template <typename BV>
50HierarchyTree<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//==============================================================================
61template <typename BV>
63 clear();
64}
65
66//==============================================================================
67template <typename BV>
68void 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//==============================================================================
88template <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//==============================================================================
98template <typename BV>
100 removeLeaf(leaf);
101 deleteNode(leaf);
102 --n_leaves;
103}
104
105//==============================================================================
106template <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//==============================================================================
117template <typename BV>
119 return (nullptr == root_node);
120}
121
122//==============================================================================
123template <typename BV>
124void 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//==============================================================================
151template <typename BV>
152bool 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//==============================================================================
159template <typename S, typename BV>
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//==============================================================================
179template <typename BV>
180bool 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//==============================================================================
186template <typename BV>
187bool 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//==============================================================================
192template <typename BV>
194 if (!root_node) return 0;
195 return getMaxHeight(root_node);
196}
197
198//==============================================================================
199template <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//==============================================================================
209template <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//==============================================================================
221template <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//==============================================================================
232template <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//==============================================================================
250template <typename BV>
252 if (root_node) recurseRefit(root_node);
253}
254
255//==============================================================================
256template <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//==============================================================================
267template <typename BV>
269 return n_leaves;
270}
271
272//==============================================================================
273template <typename BV>
275 return root_node;
276}
277
278//==============================================================================
279template <typename BV>
281 return root_node;
282}
283
284//==============================================================================
285template <typename BV>
286void 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//==============================================================================
299template <typename BV>
300void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg,
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//==============================================================================
333template <typename BV>
334typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown(
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//==============================================================================
349template <typename BV>
350size_t HierarchyTree<BV>::getMaxHeight(Node* node) const {
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//==============================================================================
360template <typename BV>
361void 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//==============================================================================
371template <typename BV>
372typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0(
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//==============================================================================
406template <typename BV>
407typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1(
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//==============================================================================
466template <typename BV>
467void 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//==============================================================================
476template <typename BV>
477void 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//==============================================================================
500template <typename BV>
501void 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//==============================================================================
524template <typename BV>
525void 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//==============================================================================
547template <typename BV>
548typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0(
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//==============================================================================
587template <typename BV>
588typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1(
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//==============================================================================
633template <typename BV>
634typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2(
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//==============================================================================
651template <typename BV>
652void 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//==============================================================================
667template <typename BV>
668typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) {
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//==============================================================================
695template <typename BV>
696void 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//==============================================================================
776template <typename BV>
777typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf(
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//==============================================================================
841template <typename BV>
842void 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//==============================================================================
854template <typename BV>
855size_t HierarchyTree<BV>::indexOf(Node* node) {
856 // node cannot be nullptr
857 return (node->parent->children[1] == node);
858}
859
860//==============================================================================
861template <typename BV>
862typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
863 const BV& bv,
864 void* data) {
865 Node* node = createNode(parent, data);
866 node->bv = bv;
867 return node;
868}
869
870//==============================================================================
871template <typename BV>
872typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
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//==============================================================================
882template <typename BV>
883typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent,
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//==============================================================================
898template <typename BV>
899void HierarchyTree<BV>::deleteNode(Node* node) {
900 if (free_node != node) {
901 delete free_node;
902 free_node = node;
903 }
904}
905
906//==============================================================================
907template <typename BV>
908void HierarchyTree<BV>::recurseDeleteNode(Node* node) {
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//==============================================================================
919template <typename BV>
920void HierarchyTree<BV>::recurseRefit(Node* node) {
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//==============================================================================
930template <typename BV>
932 if (a->bv.center()[d] < b->bv.center()[d]) return true;
933 return false;
934}
935
936//==============================================================================
937template <typename S, typename BV>
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//==============================================================================
952template <typename BV>
953size_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//==============================================================================
959template <typename BV>
960size_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//==============================================================================
966template <typename S>
967struct 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
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
~HierarchyTree()
Definition: hierarchy_tree-inl.h:62
void clear()
Clear the tree.
Definition: hierarchy_tree-inl.h:107
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
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
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree-inl.h:193
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree-inl.h:268
void extractLeaves(const Node *root, std::vector< Node * > &leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree-inl.h:257
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree-inl.h:200
Node * insert(const BV &bv, void *data)
Insest a node.
Definition: hierarchy_tree-inl.h:89
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree-inl.h:233
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree-inl.h:210
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
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
Node * getRoot() const
get the root of the tree
Definition: hierarchy_tree-inl.h:274
void remove(Node *leaf)
Remove a leaf node.
Definition: hierarchy_tree-inl.h:99
NodeBase< BV > Node
Definition: hierarchy_tree.h:59
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree-inl.h:222
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree-inl.h:118
void print(Node *root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree-inl.h:286
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
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
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
Definition: hierarchy_tree-inl.h:953
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
dynamic AABB tree node
Definition: node_base.h:50
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
NodeBase< BV > * parent
pointer to parent node
Definition: node_base.h:55
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
BV bv
the bounding volume for the node
Definition: node_base.h:52
static std::size_t run(const AABB &query, const NodeBase< AABB > &node1, const NodeBase< AABB > &node2)
Definition: hierarchy_tree-inl.h:982
static std::size_t run(const NodeBase< AABB > &node, const NodeBase< AABB > &node1, const NodeBase< AABB > &node2)
Definition: hierarchy_tree-inl.h:968
Definition: hierarchy_tree-inl.h:938
static std::size_t run(const BV &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:945
static std::size_t run(const NodeBase< BV > &, const NodeBase< BV > &, const NodeBase< BV > &)
Definition: hierarchy_tree-inl.h:939
Definition: hierarchy_tree-inl.h:160
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
static bool run(const HierarchyTree< BV > &tree, typename HierarchyTree< BV >::Node *leaf, const BV &bv, const Vec3f &)
Definition: hierarchy_tree-inl.h:169