hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
hierarchy_tree_array-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_ARRAY_INL_H
39#define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H
40
42
43#include <algorithm>
44#include <iostream>
45
46namespace hpp {
47namespace fcl {
48
49namespace detail {
50
51namespace implementation_array {
52
53//==============================================================================
54template <typename BV>
55HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
56 root_node = NULL_NODE;
57 n_nodes = 0;
58 n_nodes_alloc = 16;
59 nodes = new Node[n_nodes_alloc];
60 for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
61 nodes[n_nodes_alloc - 1].next = NULL_NODE;
62 n_leaves = 0;
63 freelist = 0;
64 opath = 0;
65 max_lookahead_level = -1;
66 bu_threshold = bu_threshold_;
67 topdown_level = topdown_level_;
68}
69
70//==============================================================================
71template <typename BV>
73 delete[] nodes;
74}
75
76//==============================================================================
77template <typename BV>
78void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) {
79 switch (level) {
80 case 0:
81 init_0(leaves, n_leaves_);
82 break;
83 case 1:
84 init_1(leaves, n_leaves_);
85 break;
86 case 2:
87 init_2(leaves, n_leaves_);
88 break;
89 case 3:
90 init_3(leaves, n_leaves_);
91 break;
92 default:
93 init_0(leaves, n_leaves_);
94 }
96
97//==============================================================================
98template <typename BV>
99void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) {
100 clear();
101
102 n_leaves = (size_t)n_leaves_;
103 root_node = NULL_NODE;
104 nodes = new Node[n_leaves * 2];
105 std::copy(leaves, leaves + n_leaves, nodes);
106 freelist = n_leaves;
107 n_nodes = n_leaves;
108 n_nodes_alloc = 2 * n_leaves;
109 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
110 nodes[n_nodes_alloc - 1].next = NULL_NODE;
112 size_t* ids = new size_t[n_leaves];
113 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
114
115 root_node = topdown(ids, ids + n_leaves);
116 delete[] ids;
117
118 opath = 0;
119 max_lookahead_level = -1;
120}
122//==============================================================================
123template <typename BV>
124void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) {
125 clear();
126
127 n_leaves = (size_t)n_leaves_;
128 root_node = NULL_NODE;
129 nodes = new Node[n_leaves * 2];
130 std::copy(leaves, leaves + n_leaves, nodes);
131 freelist = n_leaves;
132 n_nodes = n_leaves;
133 n_nodes_alloc = 2 * n_leaves;
134 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
135 nodes[n_nodes_alloc - 1].next = NULL_NODE;
137 BV bound_bv;
138 if (n_leaves > 0) bound_bv = nodes[0].bv;
139 for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
141 morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
142 for (size_t i = 0; i < n_leaves; ++i)
143 nodes[i].code = coder(nodes[i].bv.center());
144
145 size_t* ids = new size_t[n_leaves];
146 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
147
148 const SortByMorton comp{nodes};
149 std::sort(ids, ids + n_leaves, comp);
150 root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
151 coder.bits() - 1);
152 delete[] ids;
153
154 refit();
156 opath = 0;
157 max_lookahead_level = -1;
158}
159
160//==============================================================================
161template <typename BV>
162void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) {
163 clear();
164
165 n_leaves = (size_t)n_leaves_;
166 root_node = NULL_NODE;
167 nodes = new Node[n_leaves * 2];
168 std::copy(leaves, leaves + n_leaves, nodes);
169 freelist = n_leaves;
170 n_nodes = n_leaves;
171 n_nodes_alloc = 2 * n_leaves;
172 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
173 nodes[n_nodes_alloc - 1].next = NULL_NODE;
174
175 BV bound_bv;
176 if (n_leaves > 0) bound_bv = nodes[0].bv;
177 for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
178
179 morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
180 for (size_t i = 0; i < n_leaves; ++i)
181 nodes[i].code = coder(nodes[i].bv.center());
182
183 size_t* ids = new size_t[n_leaves];
184 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
185
186 const SortByMorton comp{nodes};
187 std::sort(ids, ids + n_leaves, comp);
188 root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
189 coder.bits() - 1);
190 delete[] ids;
191
192 refit();
193
194 opath = 0;
195 max_lookahead_level = -1;
196}
197
198//==============================================================================
199template <typename BV>
200void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) {
201 clear();
202
203 n_leaves = (size_t)n_leaves_;
204 root_node = NULL_NODE;
205 nodes = new Node[n_leaves * 2];
206 std::copy(leaves, leaves + n_leaves, nodes);
207 freelist = n_leaves;
208 n_nodes = n_leaves;
209 n_nodes_alloc = 2 * n_leaves;
210 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
211 nodes[n_nodes_alloc - 1].next = NULL_NODE;
212
213 BV bound_bv;
214 if (n_leaves > 0) bound_bv = nodes[0].bv;
215 for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
216
217 morton_functor<FCL_REAL, uint32_t> coder(bound_bv);
218 for (size_t i = 0; i < n_leaves; ++i)
219 nodes[i].code = coder(nodes[i].bv.center());
220
221 size_t* ids = new size_t[n_leaves];
222 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
223
224 const SortByMorton comp{nodes};
225 std::sort(ids, ids + n_leaves, comp);
226 root_node = mortonRecurse_2(ids, ids + n_leaves);
227 delete[] ids;
228
229 refit();
230
231 opath = 0;
232 max_lookahead_level = -1;
233}
234
235//==============================================================================
236template <typename BV>
237size_t HierarchyTree<BV>::insert(const BV& bv, void* data) {
238 size_t node = createNode(NULL_NODE, bv, data);
239 insertLeaf(root_node, node);
240 ++n_leaves;
241 return node;
242}
243
244//==============================================================================
245template <typename BV>
246void HierarchyTree<BV>::remove(size_t leaf) {
247 removeLeaf(leaf);
248 deleteNode(leaf);
249 --n_leaves;
250}
251
252//==============================================================================
253template <typename BV>
255 delete[] nodes;
256 root_node = NULL_NODE;
257 n_nodes = 0;
258 n_nodes_alloc = 16;
259 nodes = new Node[n_nodes_alloc];
260 for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
261 nodes[n_nodes_alloc - 1].next = NULL_NODE;
262 n_leaves = 0;
263 freelist = 0;
264 opath = 0;
265 max_lookahead_level = -1;
266}
267
268//==============================================================================
269template <typename BV>
271 return (n_nodes == 0);
272}
273
274//==============================================================================
275template <typename BV>
276void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) {
277 size_t root = removeLeaf(leaf);
278 if (root != NULL_NODE) {
279 if (lookahead_level > 0) {
280 for (int i = 0;
281 (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
282 root = nodes[root].parent;
283 } else
284 root = root_node;
285 }
286 insertLeaf(root, leaf);
287}
288
289//==============================================================================
290template <typename BV>
291bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
292 if (nodes[leaf].bv.contain(bv)) return false;
293 update_(leaf, bv);
294 return true;
295}
296
297//==============================================================================
298template <typename BV>
299bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel,
300 FCL_REAL margin) {
304
305 if (nodes[leaf].bv.contain(bv)) return false;
306 update_(leaf, bv);
307 return true;
308}
309
310//==============================================================================
311template <typename BV>
312bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel) {
314
315 if (nodes[leaf].bv.contain(bv)) return false;
316 update_(leaf, bv);
317 return true;
318}
319
320//==============================================================================
321template <typename BV>
323 if (root_node == NULL_NODE) return 0;
324
325 return getMaxHeight(root_node);
326}
327
328//==============================================================================
329template <typename BV>
331 if (root_node == NULL_NODE) return 0;
332
333 size_t max_depth;
334 getMaxDepth(root_node, 0, max_depth);
335 return max_depth;
336}
337
338//==============================================================================
339template <typename BV>
341 if (root_node != NULL_NODE) {
342 Node* leaves = new Node[n_leaves];
343 Node* leaves_ = leaves;
344 extractLeaves(root_node, leaves_);
345 root_node = NULL_NODE;
346 std::copy(leaves, leaves + n_leaves, nodes);
347 freelist = n_leaves;
348 n_nodes = n_leaves;
349 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
350 nodes[n_nodes_alloc - 1].next = NULL_NODE;
351
352 size_t* ids = new size_t[n_leaves];
353 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
354
355 bottomup(ids, ids + n_leaves);
356 root_node = *ids;
357
358 delete[] ids;
359 }
360}
361
362//==============================================================================
363template <typename BV>
365 if (root_node != NULL_NODE) {
366 Node* leaves = new Node[n_leaves];
367 Node* leaves_ = leaves;
368 extractLeaves(root_node, leaves_);
369 root_node = NULL_NODE;
370 std::copy(leaves, leaves + n_leaves, nodes);
371 freelist = n_leaves;
372 n_nodes = n_leaves;
373 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
374 nodes[n_nodes_alloc - 1].next = NULL_NODE;
375
376 size_t* ids = new size_t[n_leaves];
377 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
378
379 root_node = topdown(ids, ids + n_leaves);
380 delete[] ids;
381 }
382}
383
384//==============================================================================
385template <typename BV>
387 if (iterations < 0) iterations = (int)n_leaves;
388 if ((root_node != NULL_NODE) && (iterations > 0)) {
389 for (int i = 0; i < iterations; ++i) {
390 size_t node = root_node;
391 unsigned int bit = 0;
392 while (!nodes[node].isLeaf()) {
393 node = nodes[node].children[(opath >> bit) & 1];
394 bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
395 }
396 update(node);
397 ++opath;
398 }
399 }
400}
401
402//==============================================================================
403template <typename BV>
405 if (root_node != NULL_NODE) recurseRefit(root_node);
406}
407
408//==============================================================================
409template <typename BV>
410void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const {
411 if (!nodes[root].isLeaf()) {
412 extractLeaves(nodes[root].children[0], leaves);
413 extractLeaves(nodes[root].children[1], leaves);
414 } else {
415 *leaves = nodes[root];
416 leaves++;
417 }
418}
419
420//==============================================================================
421template <typename BV>
423 return n_leaves;
424}
425
426//==============================================================================
427template <typename BV>
429 return root_node;
430}
431
432//==============================================================================
433template <typename BV>
435 return nodes;
436}
437
438//==============================================================================
439template <typename BV>
440void HierarchyTree<BV>::print(size_t root, int depth) {
441 for (int i = 0; i < depth; ++i) std::cout << " ";
442 Node* n = nodes + root;
443 std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", "
444 << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1]
445 << ", " << n->bv.max_[2] << ")" << std::endl;
446 if (n->isLeaf()) {
447 } else {
448 print(n->children[0], depth + 1);
449 print(n->children[1], depth + 1);
450 }
451}
452
453//==============================================================================
454template <typename BV>
455size_t HierarchyTree<BV>::getMaxHeight(size_t node) const {
456 if (!nodes[node].isLeaf()) {
457 size_t height1 = getMaxHeight(nodes[node].children[0]);
458 size_t height2 = getMaxHeight(nodes[node].children[1]);
459 return std::max(height1, height2) + 1;
460 } else
461 return 0;
462}
463
464//==============================================================================
465template <typename BV>
466void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth,
467 size_t& max_depth) const {
468 if (!nodes[node].isLeaf()) {
469 getMaxDepth(nodes[node].children[0], depth + 1, max_depth);
470 getmaxDepth(nodes[node].children[1], depth + 1, max_depth);
471 } else
472 max_depth = std::max(max_depth, depth);
473}
474
475//==============================================================================
476template <typename BV>
477void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) {
478 size_t* lcur_end = lend;
479 while (lbeg < lcur_end - 1) {
480 size_t *min_it1 = nullptr, *min_it2 = nullptr;
481 FCL_REAL min_size = (std::numeric_limits<FCL_REAL>::max)();
482 for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
483 for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
484 FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
485 if (cur_size < min_size) {
486 min_size = cur_size;
487 min_it1 = it1;
488 min_it2 = it2;
489 }
490 }
491 }
492
493 size_t p =
494 createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr);
495 nodes[p].children[0] = *min_it1;
496 nodes[p].children[1] = *min_it2;
497 nodes[*min_it1].parent = p;
498 nodes[*min_it2].parent = p;
499 *min_it1 = p;
500 size_t tmp = *min_it2;
501 lcur_end--;
502 *min_it2 = *lcur_end;
503 *lcur_end = tmp;
504 }
505}
506
507//==============================================================================
508template <typename BV>
509size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) {
510 switch (topdown_level) {
511 case 0:
512 return topdown_0(lbeg, lend);
513 break;
514 case 1:
515 return topdown_1(lbeg, lend);
516 break;
517 default:
518 return topdown_0(lbeg, lend);
519 }
520}
521
522//==============================================================================
523template <typename BV>
524size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) {
525 long num_leaves = lend - lbeg;
526 if (num_leaves > 1) {
527 if (num_leaves > bu_threshold) {
528 BV vol = nodes[*lbeg].bv;
529 for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv;
530
531 size_t best_axis = 0;
532 FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()};
533 if (extent[1] > extent[0]) best_axis = 1;
534 if (extent[2] > extent[best_axis]) best_axis = 2;
535
536 nodeBaseLess<BV> comp(nodes, best_axis);
537 size_t* lcenter = lbeg + num_leaves / 2;
538 std::nth_element(lbeg, lcenter, lend, comp);
539
540 size_t node = createNode(NULL_NODE, vol, nullptr);
541 nodes[node].children[0] = topdown_0(lbeg, lcenter);
542 nodes[node].children[1] = topdown_0(lcenter, lend);
543 nodes[nodes[node].children[0]].parent = node;
544 nodes[nodes[node].children[1]].parent = node;
545 return node;
546 } else {
547 bottomup(lbeg, lend);
548 return *lbeg;
549 }
550 }
551 return *lbeg;
552}
553
554//==============================================================================
555template <typename BV>
556size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
557 long num_leaves = lend - lbeg;
558 if (num_leaves > 1) {
559 if (num_leaves > bu_threshold) {
560 Vec3f split_p = nodes[*lbeg].bv.center();
561 BV vol = nodes[*lbeg].bv;
562 for (size_t* i = lbeg + 1; i < lend; ++i) {
563 split_p += nodes[*i].bv.center();
564 vol += nodes[*i].bv;
565 }
566 split_p /= static_cast<FCL_REAL>(num_leaves);
567 int best_axis = -1;
568 int bestmidp = (int)num_leaves;
569 int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
570 for (size_t* i = lbeg; i < lend; ++i) {
571 Vec3f x = nodes[*i].bv.center() - split_p;
572 for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
573 }
574
575 for (size_t i = 0; i < 3; ++i) {
576 if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
577 int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
578 if (midp < bestmidp) {
579 best_axis = (int)i;
580 bestmidp = midp;
581 }
582 }
583 }
584
585 if (best_axis < 0) best_axis = 0;
586
587 FCL_REAL split_value = split_p[best_axis];
588 size_t* lcenter = lbeg;
589 for (size_t* i = lbeg; i < lend; ++i) {
590 if (nodes[*i].bv.center()[best_axis] < split_value) {
591 size_t temp = *i;
592 *i = *lcenter;
593 *lcenter = temp;
594 ++lcenter;
595 }
596 }
597
598 size_t node = createNode(NULL_NODE, vol, nullptr);
599 nodes[node].children[0] = topdown_1(lbeg, lcenter);
600 nodes[node].children[1] = topdown_1(lcenter, lend);
601 nodes[nodes[node].children[0]].parent = node;
602 nodes[nodes[node].children[1]].parent = node;
603 return node;
604 } else {
605 bottomup(lbeg, lend);
606 return *lbeg;
607 }
608 }
609 return *lbeg;
610}
611
612//==============================================================================
613template <typename BV>
614size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend,
615 const uint32_t& split, int bits) {
616 long num_leaves = lend - lbeg;
617 if (num_leaves > 1) {
618 if (bits > 0) {
619 const SortByMorton comp{nodes, split};
620 size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
621
622 if (lcenter == lbeg) {
623 uint32_t split2 = split | (1 << (bits - 1));
624 return mortonRecurse_0(lbeg, lend, split2, bits - 1);
625 } else if (lcenter == lend) {
626 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
627 return mortonRecurse_0(lbeg, lend, split1, bits - 1);
628 } else {
629 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
630 uint32_t split2 = split | (1 << (bits - 1));
631
632 size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
633 size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
634 size_t node = createNode(NULL_NODE, nullptr);
635 nodes[node].children[0] = child1;
636 nodes[node].children[1] = child2;
637 nodes[child1].parent = node;
638 nodes[child2].parent = node;
639 return node;
640 }
641 } else {
642 size_t node = topdown(lbeg, lend);
643 return node;
644 }
645 } else
646 return *lbeg;
647}
648
649//==============================================================================
650template <typename BV>
651size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend,
652 const uint32_t& split, int bits) {
653 long num_leaves = lend - lbeg;
654 if (num_leaves > 1) {
655 if (bits > 0) {
656 const SortByMorton comp{nodes, split};
657 size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
658
659 if (lcenter == lbeg) {
660 uint32_t split2 = split | (1 << (bits - 1));
661 return mortonRecurse_1(lbeg, lend, split2, bits - 1);
662 } else if (lcenter == lend) {
663 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
664 return mortonRecurse_1(lbeg, lend, split1, bits - 1);
665 } else {
666 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
667 uint32_t split2 = split | (1 << (bits - 1));
668
669 size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
670 size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
671 size_t node = createNode(NULL_NODE, nullptr);
672 nodes[node].children[0] = child1;
673 nodes[node].children[1] = child2;
674 nodes[child1].parent = node;
675 nodes[child2].parent = node;
676 return node;
677 }
678 } else {
679 size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
680 size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
681 size_t node = createNode(NULL_NODE, nullptr);
682 nodes[node].children[0] = child1;
683 nodes[node].children[1] = child2;
684 nodes[child1].parent = node;
685 nodes[child2].parent = node;
686 return node;
687 }
688 } else
689 return *lbeg;
690}
691
692//==============================================================================
693template <typename BV>
694size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) {
695 long num_leaves = lend - lbeg;
696 if (num_leaves > 1) {
697 size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
698 size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
699 size_t node = createNode(NULL_NODE, nullptr);
700 nodes[node].children[0] = child1;
701 nodes[node].children[1] = child2;
702 nodes[child1].parent = node;
703 nodes[child2].parent = node;
704 return node;
705 } else
706 return *lbeg;
707}
708
709//==============================================================================
710template <typename BV>
711void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) {
712 if (root_node == NULL_NODE) {
713 root_node = leaf;
714 nodes[leaf].parent = NULL_NODE;
715 } else {
716 if (!nodes[root].isLeaf()) {
717 do {
718 root = nodes[root].children[select(leaf, nodes[root].children[0],
719 nodes[root].children[1], nodes)];
720 } while (!nodes[root].isLeaf());
721 }
722
723 size_t prev = nodes[root].parent;
724 size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr);
725 if (prev != NULL_NODE) {
726 nodes[prev].children[indexOf(root)] = node;
727 nodes[node].children[0] = root;
728 nodes[root].parent = node;
729 nodes[node].children[1] = leaf;
730 nodes[leaf].parent = node;
731 do {
732 if (!nodes[prev].bv.contain(nodes[node].bv))
733 nodes[prev].bv = nodes[nodes[prev].children[0]].bv +
734 nodes[nodes[prev].children[1]].bv;
735 else
736 break;
737 node = prev;
738 } while (NULL_NODE != (prev = nodes[node].parent));
739 } else {
740 nodes[node].children[0] = root;
741 nodes[root].parent = node;
742 nodes[node].children[1] = leaf;
743 nodes[leaf].parent = node;
744 root_node = node;
745 }
746 }
747}
748
749//==============================================================================
750template <typename BV>
751size_t HierarchyTree<BV>::removeLeaf(size_t leaf) {
752 if (leaf == root_node) {
753 root_node = NULL_NODE;
754 return NULL_NODE;
755 } else {
756 size_t parent = nodes[leaf].parent;
757 size_t prev = nodes[parent].parent;
758 size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
759
760 if (prev != NULL_NODE) {
761 nodes[prev].children[indexOf(parent)] = sibling;
762 nodes[sibling].parent = prev;
763 deleteNode(parent);
764 while (prev != NULL_NODE) {
765 BV new_bv = nodes[nodes[prev].children[0]].bv +
766 nodes[nodes[prev].children[1]].bv;
767 if (!(new_bv == nodes[prev].bv)) {
768 nodes[prev].bv = new_bv;
769 prev = nodes[prev].parent;
770 } else
771 break;
772 }
773
774 return (prev != NULL_NODE) ? prev : root_node;
775 } else {
776 root_node = sibling;
777 nodes[sibling].parent = NULL_NODE;
778 deleteNode(parent);
779 return root_node;
780 }
781 }
782}
783
784//==============================================================================
785template <typename BV>
786void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) {
787 size_t root = removeLeaf(leaf);
788 if (root != NULL_NODE) {
789 if (max_lookahead_level >= 0) {
790 for (int i = 0;
791 (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
792 root = nodes[root].parent;
793 }
794
795 nodes[leaf].bv = bv;
796 insertLeaf(root, leaf);
797 }
798}
799
800//==============================================================================
801template <typename BV>
802size_t HierarchyTree<BV>::indexOf(size_t node) {
803 return (nodes[nodes[node].parent].children[1] == node);
804}
805
806//==============================================================================
807template <typename BV>
808size_t HierarchyTree<BV>::allocateNode() {
809 if (freelist == NULL_NODE) {
810 Node* old_nodes = nodes;
811 n_nodes_alloc *= 2;
812 nodes = new Node[n_nodes_alloc];
813 std::copy(old_nodes, old_nodes + n_nodes, nodes);
814 delete[] old_nodes;
815
816 for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
817 nodes[n_nodes_alloc - 1].next = NULL_NODE;
818 freelist = n_nodes;
819 }
820
821 size_t node_id = freelist;
822 freelist = nodes[node_id].next;
823 nodes[node_id].parent = NULL_NODE;
824 nodes[node_id].children[0] = NULL_NODE;
825 nodes[node_id].children[1] = NULL_NODE;
826 ++n_nodes;
827 return node_id;
828}
829
830//==============================================================================
831template <typename BV>
832size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1,
833 const BV& bv2, void* data) {
834 size_t node = allocateNode();
835 nodes[node].parent = parent;
836 nodes[node].data = data;
837 nodes[node].bv = bv1 + bv2;
838 return node;
839}
840
841//==============================================================================
842template <typename BV>
843size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) {
844 size_t node = allocateNode();
845 nodes[node].parent = parent;
846 nodes[node].data = data;
847 nodes[node].bv = bv;
848 return node;
849}
850
851//==============================================================================
852template <typename BV>
853size_t HierarchyTree<BV>::createNode(size_t parent, void* data) {
854 size_t node = allocateNode();
855 nodes[node].parent = parent;
856 nodes[node].data = data;
857 return node;
858}
859
860//==============================================================================
861template <typename BV>
862void HierarchyTree<BV>::deleteNode(size_t node) {
863 nodes[node].next = freelist;
864 freelist = node;
865 --n_nodes;
866}
867
868//==============================================================================
869template <typename BV>
870void HierarchyTree<BV>::recurseRefit(size_t node) {
871 if (!nodes[node].isLeaf()) {
872 recurseRefit(nodes[node].children[0]);
873 recurseRefit(nodes[node].children[1]);
874 nodes[node].bv =
875 nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
876 } else
877 return;
878}
879
880//==============================================================================
881template <typename BV>
882void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) {
883 if ((!nodes[root].isLeaf()) && depth) {
884 fetchLeaves(nodes[root].children[0], leaves, depth - 1);
885 fetchLeaves(nodes[root].children[1], leaves, depth - 1);
886 deleteNode(root);
887 } else {
888 *leaves = nodes[root];
889 leaves++;
890 }
891}
892
893//==============================================================================
894template <typename BV>
896 : nodes(nodes_), d(d_) {
897 // Do nothing
898}
899
900//==============================================================================
901template <typename BV>
902bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
903 if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
904
905 return false;
906}
907
908//==============================================================================
909template <typename S, typename BV>
911 static bool run(size_t query, size_t node1, size_t node2,
912 NodeBase<BV>* nodes) {
917
918 return 0;
919 }
920
921 static bool run(const BV& query, size_t node1, size_t node2,
922 NodeBase<BV>* nodes) {
927
928 return 0;
929 }
930};
931
932//==============================================================================
933template <typename BV>
934size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
935 return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
936}
937
938//==============================================================================
939template <typename BV>
940size_t select(const BV& query, size_t node1, size_t node2,
941 NodeBase<BV>* nodes) {
942 return SelectImpl<FCL_REAL, BV>::run(query, node1, node2, nodes);
943}
944
945//==============================================================================
946template <typename S>
947struct SelectImpl<S, AABB> {
948 static bool run(size_t query, size_t node1, size_t node2,
949 NodeBase<AABB>* nodes) {
950 const AABB& bv = nodes[query].bv;
951 const AABB& bv1 = nodes[node1].bv;
952 const AABB& bv2 = nodes[node2].bv;
953 Vec3f v = bv.min_ + bv.max_;
954 Vec3f v1 = v - (bv1.min_ + bv1.max_);
955 Vec3f v2 = v - (bv2.min_ + bv2.max_);
956 FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
957 FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
958 return (d1 < d2) ? 0 : 1;
959 }
960
961 static bool run(const AABB& query, size_t node1, size_t node2,
962 NodeBase<AABB>* nodes) {
963 const AABB& bv = query;
964 const AABB& bv1 = nodes[node1].bv;
965 const AABB& bv2 = nodes[node2].bv;
966 Vec3f v = bv.min_ + bv.max_;
967 Vec3f v1 = v - (bv1.min_ + bv1.max_);
968 Vec3f v2 = v - (bv2.min_ + bv2.max_);
969 FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
970 FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
971 return (d1 < d2) ? 0 : 1;
972 }
973};
974
975} // namespace implementation_array
976} // namespace detail
977} // namespace fcl
978} // namespace hpp
979
980#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_array.h:59
size_t getRoot() const
get the root of the tree
Definition: hierarchy_tree_array-inl.h:428
void extractLeaves(size_t root, Node *&leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree_array-inl.h:410
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
Definition: hierarchy_tree_array-inl.h:404
Node * getNodes() const
get the pointer to the nodes array
Definition: hierarchy_tree_array-inl.h:434
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree_array-inl.h:322
void print(size_t root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree_array-inl.h:440
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree_array-inl.h:364
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_array-inl.h:55
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree_array-inl.h:330
~HierarchyTree()
Definition: hierarchy_tree_array-inl.h:72
void update(size_t leaf, int lookahead_level=-1)
update one leaf node
Definition: hierarchy_tree_array-inl.h:276
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree_array-inl.h:340
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree_array-inl.h:422
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree_array-inl.h:270
size_t insert(const BV &bv, void *data)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree_array-inl.h:237
void remove(size_t leaf)
Remove a leaf node.
Definition: hierarchy_tree_array-inl.h:246
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree_array-inl.h:386
void clear()
Clear the tree.
Definition: hierarchy_tree_array-inl.h:254
void init(Node *leaves, int n_leaves_, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree_array-inl.h:78
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:55
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
size_t select(size_t query, size_t node1, size_t node2, NodeBase< BV > *nodes)
select the node from node1 and node2 which is close to the query-th node in the nodes....
Definition: hierarchy_tree_array-inl.h:934
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
Definition: node_base_array.h:51
size_t children[2]
Definition: node_base_array.h:60
BV bv
Definition: node_base_array.h:52
bool isLeaf() const
Definition: node_base_array-inl.h:52
size_t next
Definition: node_base_array.h:56
static bool run(size_t query, size_t node1, size_t node2, NodeBase< AABB > *nodes)
Definition: hierarchy_tree_array-inl.h:948
static bool run(const AABB &query, size_t node1, size_t node2, NodeBase< AABB > *nodes)
Definition: hierarchy_tree_array-inl.h:961
Definition: hierarchy_tree_array-inl.h:910
static bool run(const BV &query, size_t node1, size_t node2, NodeBase< BV > *nodes)
Definition: hierarchy_tree_array-inl.h:921
static bool run(size_t query, size_t node1, size_t node2, NodeBase< BV > *nodes)
Definition: hierarchy_tree_array-inl.h:911
bool operator()(size_t i, size_t j) const
Definition: hierarchy_tree_array-inl.h:902
nodeBaseLess(const NodeBase< BV > *nodes_, size_t d_)
Definition: hierarchy_tree_array-inl.h:895