hpp-fcl  2.4.1
HPP fork of FCL -- The Flexible Collision Library
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 
41 #include "hpp/fcl/broadphase/detail/hierarchy_tree_array.h"
42 
43 #include <algorithm>
44 #include <iostream>
45 
46 namespace hpp {
47 namespace fcl {
48 
49 namespace detail {
50 
51 namespace implementation_array {
52 
53 //==============================================================================
54 template <typename BV>
55 HierarchyTree<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 //==============================================================================
71 template <typename BV>
72 HierarchyTree<BV>::~HierarchyTree() {
73  delete[] nodes;
74 }
75 
76 //==============================================================================
77 template <typename BV>
78 void 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  }
95 }
96 
97 //==============================================================================
98 template <typename BV>
99 void 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;
111 
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 }
121 
122 //==============================================================================
123 template <typename BV>
124 void 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;
136 
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;
140 
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();
155 
156  opath = 0;
157  max_lookahead_level = -1;
158 }
159 
160 //==============================================================================
161 template <typename BV>
162 void 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 //==============================================================================
199 template <typename BV>
200 void 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 //==============================================================================
236 template <typename BV>
237 size_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 //==============================================================================
245 template <typename BV>
246 void HierarchyTree<BV>::remove(size_t leaf) {
247  removeLeaf(leaf);
248  deleteNode(leaf);
249  --n_leaves;
250 }
251 
252 //==============================================================================
253 template <typename BV>
254 void HierarchyTree<BV>::clear() {
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 //==============================================================================
269 template <typename BV>
270 bool HierarchyTree<BV>::empty() const {
271  return (n_nodes == 0);
272 }
273 
274 //==============================================================================
275 template <typename BV>
276 void 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 //==============================================================================
290 template <typename BV>
291 bool 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 //==============================================================================
298 template <typename BV>
299 bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel,
300  FCL_REAL margin) {
301  HPP_FCL_UNUSED_VARIABLE(bv);
302  HPP_FCL_UNUSED_VARIABLE(vel);
303  HPP_FCL_UNUSED_VARIABLE(margin);
304 
305  if (nodes[leaf].bv.contain(bv)) return false;
306  update_(leaf, bv);
307  return true;
308 }
309 
310 //==============================================================================
311 template <typename BV>
312 bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3f& vel) {
313  HPP_FCL_UNUSED_VARIABLE(vel);
314 
315  if (nodes[leaf].bv.contain(bv)) return false;
316  update_(leaf, bv);
317  return true;
318 }
319 
320 //==============================================================================
321 template <typename BV>
322 size_t HierarchyTree<BV>::getMaxHeight() const {
323  if (root_node == NULL_NODE) return 0;
324 
325  return getMaxHeight(root_node);
326 }
327 
328 //==============================================================================
329 template <typename BV>
330 size_t HierarchyTree<BV>::getMaxDepth() const {
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 //==============================================================================
339 template <typename BV>
340 void HierarchyTree<BV>::balanceBottomup() {
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 //==============================================================================
363 template <typename BV>
364 void HierarchyTree<BV>::balanceTopdown() {
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 //==============================================================================
385 template <typename BV>
386 void HierarchyTree<BV>::balanceIncremental(int iterations) {
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 //==============================================================================
403 template <typename BV>
404 void HierarchyTree<BV>::refit() {
405  if (root_node != NULL_NODE) recurseRefit(root_node);
406 }
407 
408 //==============================================================================
409 template <typename BV>
410 void 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 //==============================================================================
421 template <typename BV>
422 size_t HierarchyTree<BV>::size() const {
423  return n_leaves;
424 }
425 
426 //==============================================================================
427 template <typename BV>
428 size_t HierarchyTree<BV>::getRoot() const {
429  return root_node;
430 }
431 
432 //==============================================================================
433 template <typename BV>
434 typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const {
435  return nodes;
436 }
437 
438 //==============================================================================
439 template <typename BV>
440 void 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 //==============================================================================
454 template <typename BV>
455 size_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 //==============================================================================
465 template <typename BV>
466 void 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 //==============================================================================
476 template <typename BV>
477 void 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 //==============================================================================
508 template <typename BV>
509 size_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 //==============================================================================
523 template <typename BV>
524 size_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 //==============================================================================
555 template <typename BV>
556 size_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 //==============================================================================
613 template <typename BV>
614 size_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 //==============================================================================
650 template <typename BV>
651 size_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 //==============================================================================
693 template <typename BV>
694 size_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 //==============================================================================
710 template <typename BV>
711 void 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 //==============================================================================
750 template <typename BV>
751 size_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 //==============================================================================
785 template <typename BV>
786 void 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 //==============================================================================
801 template <typename BV>
802 size_t HierarchyTree<BV>::indexOf(size_t node) {
803  return (nodes[nodes[node].parent].children[1] == node);
804 }
805 
806 //==============================================================================
807 template <typename BV>
808 size_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 //==============================================================================
831 template <typename BV>
832 size_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 //==============================================================================
842 template <typename BV>
843 size_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 //==============================================================================
852 template <typename BV>
853 size_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 //==============================================================================
861 template <typename BV>
862 void HierarchyTree<BV>::deleteNode(size_t node) {
863  nodes[node].next = freelist;
864  freelist = node;
865  --n_nodes;
866 }
867 
868 //==============================================================================
869 template <typename BV>
870 void 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 //==============================================================================
881 template <typename BV>
882 void 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 //==============================================================================
894 template <typename BV>
895 nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
896  : nodes(nodes_), d(d_) {
897  // Do nothing
898 }
899 
900 //==============================================================================
901 template <typename BV>
902 bool 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 //==============================================================================
909 template <typename S, typename BV>
910 struct SelectImpl {
911  static bool run(size_t query, size_t node1, size_t node2,
912  NodeBase<BV>* nodes) {
913  HPP_FCL_UNUSED_VARIABLE(query);
914  HPP_FCL_UNUSED_VARIABLE(node1);
915  HPP_FCL_UNUSED_VARIABLE(node2);
916  HPP_FCL_UNUSED_VARIABLE(nodes);
917 
918  return 0;
919  }
920 
921  static bool run(const BV& query, size_t node1, size_t node2,
922  NodeBase<BV>* nodes) {
923  HPP_FCL_UNUSED_VARIABLE(query);
924  HPP_FCL_UNUSED_VARIABLE(node1);
925  HPP_FCL_UNUSED_VARIABLE(node2);
926  HPP_FCL_UNUSED_VARIABLE(nodes);
927 
928  return 0;
929  }
930 };
931 
932 //==============================================================================
933 template <typename BV>
934 size_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 //==============================================================================
939 template <typename BV>
940 size_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 //==============================================================================
946 template <typename S>
947 struct 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
hpp::fcl::detail::implementation_array::HierarchyTree::getMaxHeight
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree_array-inl.h:322
hpp::fcl::detail::implementation_array::NodeBase
Definition: node_base_array.h:51
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
HPP_FCL_UNUSED_VARIABLE
#define HPP_FCL_UNUSED_VARIABLE(var)
Definition: fwd.hh:55
hpp::fcl::detail::implementation_array::HierarchyTree::print
void print(size_t root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree_array-inl.h:440
hpp::fcl::detail::implementation_array::HierarchyTree::remove
void remove(size_t leaf)
Remove a leaf node.
Definition: hierarchy_tree_array-inl.h:246
hpp::fcl::detail::NodeBase::isLeaf
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
hpp::fcl::detail::implementation_array::SelectImpl::run
static bool run(const BV &query, size_t node1, size_t node2, NodeBase< BV > *nodes)
Definition: hierarchy_tree_array-inl.h:921
hpp::fcl::detail::implementation_array::HierarchyTree::clear
void clear()
Clear the tree.
Definition: hierarchy_tree_array-inl.h:254
hpp::fcl::detail::implementation_array::HierarchyTree::size
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree_array-inl.h:422
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
hpp::fcl::detail::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
hpp::fcl::detail::implementation_array::HierarchyTree::getRoot
size_t getRoot() const
get the root of the tree
Definition: hierarchy_tree_array-inl.h:428
hpp::fcl::detail::implementation_array::NodeBase::bv
BV bv
Definition: node_base_array.h:52
hpp::fcl::detail::implementation_array::HierarchyTree::init
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
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::implementation_array::select
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
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hierarchy_tree_array.h
hpp::fcl::detail::implementation_array::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree_array.h:59
hpp::fcl::detail::implementation_array::SelectImpl
Definition: hierarchy_tree_array-inl.h:910
hpp::fcl::detail::implementation_array::HierarchyTree::update
void update(size_t leaf, int lookahead_level=-1)
update one leaf node
Definition: hierarchy_tree_array-inl.h:276
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
hpp::fcl::detail::implementation_array::HierarchyTree::~HierarchyTree
~HierarchyTree()
Definition: hierarchy_tree_array-inl.h:72
hpp::fcl::detail::implementation_array::HierarchyTree::getMaxDepth
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree_array-inl.h:330
hpp::fcl::detail::implementation_array::HierarchyTree::balanceIncremental
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree_array-inl.h:386
hpp::fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:50
hpp::fcl::detail::implementation_array::HierarchyTree::empty
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree_array-inl.h:270
hpp::fcl::detail::implementation_array::HierarchyTree::balanceBottomup
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree_array-inl.h:340
hpp::fcl::detail::implementation_array::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_array-inl.h:55
hpp::fcl::detail::NodeBase::children
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
hpp::fcl::detail::implementation_array::HierarchyTree::insert
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
hpp::fcl::detail::implementation_array::HierarchyTree::balanceTopdown
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree_array-inl.h:364
hpp::fcl::detail::implementation_array::nodeBaseLess::operator()
bool operator()(size_t i, size_t j) const
Definition: hierarchy_tree_array-inl.h:902
hpp::fcl::detail::implementation_array::nodeBaseLess::nodeBaseLess
nodeBaseLess(const NodeBase< BV > *nodes_, size_t d_)
Definition: hierarchy_tree_array-inl.h:895
hpp::fcl::detail::implementation_array::SelectImpl< S, AABB >::run
static bool run(size_t query, size_t node1, size_t node2, NodeBase< AABB > *nodes)
Definition: hierarchy_tree_array-inl.h:948
hpp::fcl::detail::implementation_array::SelectImpl< S, AABB >::run
static bool run(const AABB &query, size_t node1, size_t node2, NodeBase< AABB > *nodes)
Definition: hierarchy_tree_array-inl.h:961
hpp::fcl::detail::implementation_array::SelectImpl::run
static bool run(size_t query, size_t node1, size_t node2, NodeBase< BV > *nodes)
Definition: hierarchy_tree_array-inl.h:911
hpp::fcl::detail::implementation_array::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_array-inl.h:404
hpp::fcl::detail::implementation_array::HierarchyTree::extractLeaves
void extractLeaves(size_t root, Node *&leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree_array-inl.h:410
hpp::fcl::detail::implementation_array::HierarchyTree::getNodes
Node * getNodes() const
get the pointer to the nodes array
Definition: hierarchy_tree_array-inl.h:434