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


fcl
Author(s):
autogenerated on Sat Sep 11 2021 02:37:41