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

hpp-fcl
Author(s):
autogenerated on Fri Feb 14 2025 03:45:50