38 #ifndef HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H
39 #define HPP_FCL_HIERARCHY_TREE_ARRAY_INL_H
51 namespace implementation_array {
54 template <
typename BV>
56 root_node = NULL_NODE;
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;
65 max_lookahead_level = -1;
66 bu_threshold = bu_threshold_;
67 topdown_level = topdown_level_;
71 template <
typename BV>
77 template <
typename BV>
81 init_0(leaves, n_leaves_);
84 init_1(leaves, n_leaves_);
87 init_2(leaves, n_leaves_);
90 init_3(leaves, n_leaves_);
93 init_0(leaves, n_leaves_);
98 template <
typename BV>
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);
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;
115 root_node = topdown(ids, ids + n_leaves);
119 max_lookahead_level = -1;
123 template <
typename BV>
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);
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;
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());
145 size_t* ids =
new size_t[n_leaves];
146 for (
size_t i = 0; i < n_leaves; ++i) ids[i] = i;
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)),
157 max_lookahead_level = -1;
161 template <
typename BV>
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);
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;
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;
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());
183 size_t* ids =
new size_t[n_leaves];
184 for (
size_t i = 0; i < n_leaves; ++i) ids[i] = i;
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)),
195 max_lookahead_level = -1;
199 template <
typename BV>
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);
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;
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;
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());
221 size_t* ids =
new size_t[n_leaves];
222 for (
size_t i = 0; i < n_leaves; ++i) ids[i] = i;
224 const SortByMorton comp{nodes};
225 std::sort(ids, ids + n_leaves, comp);
226 root_node = mortonRecurse_2(ids, ids + n_leaves);
232 max_lookahead_level = -1;
236 template <
typename BV>
238 size_t node = createNode(NULL_NODE, bv,
data);
239 insertLeaf(root_node, node);
245 template <
typename BV>
253 template <
typename BV>
256 root_node = NULL_NODE;
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;
265 max_lookahead_level = -1;
269 template <
typename BV>
271 return (n_nodes == 0);
275 template <
typename BV>
277 size_t root = removeLeaf(leaf);
278 if (root != NULL_NODE) {
279 if (lookahead_level > 0) {
281 (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
282 root = nodes[root].parent;
286 insertLeaf(root, leaf);
290 template <
typename BV>
292 if (nodes[leaf].bv.contain(bv))
return false;
298 template <
typename BV>
305 if (nodes[leaf].bv.contain(bv))
return false;
311 template <
typename BV>
315 if (nodes[leaf].bv.contain(bv))
return false;
321 template <
typename BV>
323 if (root_node == NULL_NODE)
return 0;
325 return getMaxHeight(root_node);
329 template <
typename BV>
331 if (root_node == NULL_NODE)
return 0;
334 getMaxDepth(root_node, 0, max_depth);
339 template <
typename BV>
341 if (root_node != NULL_NODE) {
343 Node* leaves_ = leaves;
344 extractLeaves(root_node, leaves_);
345 root_node = NULL_NODE;
346 std::copy(leaves, leaves + n_leaves, nodes);
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;
352 size_t* ids =
new size_t[n_leaves];
353 for (
size_t i = 0; i < n_leaves; ++i) ids[i] = i;
355 bottomup(ids, ids + n_leaves);
363 template <
typename BV>
365 if (root_node != NULL_NODE) {
367 Node* leaves_ = leaves;
368 extractLeaves(root_node, leaves_);
369 root_node = NULL_NODE;
370 std::copy(leaves, leaves + n_leaves, nodes);
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;
376 size_t* ids =
new size_t[n_leaves];
377 for (
size_t i = 0; i < n_leaves; ++i) ids[i] = i;
379 root_node = topdown(ids, ids + n_leaves);
385 template <
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);
403 template <
typename BV>
405 if (root_node != NULL_NODE) recurseRefit(root_node);
409 template <
typename BV>
411 if (!nodes[root].isLeaf()) {
412 extractLeaves(nodes[root].children[0], leaves);
413 extractLeaves(nodes[root].children[1], leaves);
415 *leaves = nodes[root];
421 template <
typename BV>
427 template <
typename BV>
433 template <
typename BV>
439 template <
typename BV>
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;
454 template <
typename BV>
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;
465 template <
typename BV>
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);
472 max_depth = std::max(max_depth, depth);
476 template <
typename BV>
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) {
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;
500 size_t tmp = *min_it2;
502 *min_it2 = *lcur_end;
508 template <
typename BV>
510 switch (topdown_level) {
512 return topdown_0(lbeg, lend);
515 return topdown_1(lbeg, lend);
518 return topdown_0(lbeg, lend);
523 template <
typename BV>
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;
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;
536 nodeBaseLess<BV> comp(nodes, best_axis);
537 size_t* lcenter = lbeg + num_leaves / 2;
538 std::nth_element(lbeg, lcenter, lend, comp);
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;
547 bottomup(lbeg, lend);
555 template <
typename BV>
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();
566 split_p /=
static_cast<FCL_REAL>(num_leaves);
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];
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) {
585 if (best_axis < 0) best_axis = 0;
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) {
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;
605 bottomup(lbeg, lend);
613 template <
typename BV>
615 const uint32_t& split,
int bits) {
616 long num_leaves = lend - lbeg;
617 if (num_leaves > 1) {
619 const SortByMorton comp{nodes, split};
620 size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
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);
629 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
630 uint32_t split2 = split | (1 << (bits - 1));
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;
642 size_t node = topdown(lbeg, lend);
650 template <
typename BV>
652 const uint32_t& split,
int bits) {
653 long num_leaves = lend - lbeg;
654 if (num_leaves > 1) {
656 const SortByMorton comp{nodes, split};
657 size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
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);
666 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
667 uint32_t split2 = split | (1 << (bits - 1));
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;
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;
693 template <
typename BV>
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;
710 template <
typename BV>
712 if (root_node == NULL_NODE) {
714 nodes[leaf].parent = NULL_NODE;
716 if (!nodes[root].isLeaf()) {
718 root = nodes[root].children[
select(leaf, nodes[root].children[0],
719 nodes[root].children[1], nodes)];
720 }
while (!nodes[root].isLeaf());
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;
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;
738 }
while (NULL_NODE != (prev = nodes[node].parent));
740 nodes[node].children[0] = root;
741 nodes[root].parent = node;
742 nodes[node].children[1] = leaf;
743 nodes[leaf].parent = node;
750 template <
typename BV>
752 if (leaf == root_node) {
753 root_node = NULL_NODE;
756 size_t parent = nodes[leaf].parent;
757 size_t prev = nodes[parent].parent;
758 size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
760 if (prev != NULL_NODE) {
761 nodes[prev].children[indexOf(parent)] = sibling;
762 nodes[sibling].parent = prev;
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;
774 return (prev != NULL_NODE) ? prev : root_node;
777 nodes[sibling].parent = NULL_NODE;
785 template <
typename BV>
787 size_t root = removeLeaf(leaf);
788 if (root != NULL_NODE) {
789 if (max_lookahead_level >= 0) {
791 (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
792 root = nodes[root].parent;
796 insertLeaf(root, leaf);
801 template <
typename BV>
803 return (nodes[nodes[node].parent].children[1] == node);
807 template <
typename BV>
809 if (freelist == NULL_NODE) {
810 Node* old_nodes = nodes;
812 nodes =
new Node[n_nodes_alloc];
813 std::copy(old_nodes, old_nodes + n_nodes, nodes);
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;
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;
831 template <
typename BV>
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;
842 template <
typename BV>
844 size_t node = allocateNode();
845 nodes[node].parent = parent;
846 nodes[node].data =
data;
852 template <
typename BV>
854 size_t node = allocateNode();
855 nodes[node].parent = parent;
856 nodes[node].data =
data;
861 template <
typename BV>
863 nodes[node].next = freelist;
869 template <
typename BV>
871 if (!nodes[node].isLeaf()) {
872 recurseRefit(nodes[node].children[0]);
873 recurseRefit(nodes[node].children[1]);
875 nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
881 template <
typename BV>
883 if ((!nodes[root].isLeaf()) && depth) {
884 fetchLeaves(nodes[root].children[0], leaves, depth - 1);
885 fetchLeaves(nodes[root].children[1], leaves, depth - 1);
888 *leaves = nodes[root];
894 template <
typename BV>
896 : nodes(nodes_), d(d_) {
901 template <
typename BV>
903 if (nodes[i].bv.center()[(
int)d] < nodes[j].bv.center()[(
int)d])
return true;
909 template <
typename S,
typename BV>
911 static bool run(
size_t query,
size_t node1,
size_t node2,
921 static bool run(
const BV& query,
size_t node1,
size_t node2,
933 template <
typename BV>
939 template <
typename BV>
940 size_t select(
const BV& query,
size_t node1,
size_t node2,
946 template <
typename S>
948 static bool run(
size_t query,
size_t node1,
size_t node2,
950 const AABB& bv = nodes[query].
bv;
951 const AABB& bv1 = nodes[node1].
bv;
952 const AABB& bv2 = nodes[node2].
bv;
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;
961 static bool run(
const AABB& query,
size_t node1,
size_t node2,
963 const AABB& bv = query;
964 const AABB& bv1 = nodes[node1].
bv;
965 const AABB& bv2 = nodes[node2].
bv;
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;