hierarchy_tree_array.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_H
39 #define FCL_HIERARCHY_TREE_ARRAY_H
40 
41 #include <vector>
42 #include <map>
43 #include <functional>
44 #include <iostream>
45 #include "fcl/common/warning.h"
46 #include "fcl/math/bv/AABB.h"
49 
50 namespace fcl
51 {
52 
53 namespace detail
54 {
55 
56 namespace implementation_array
57 {
58 
60 template<typename BV>
61 class FCL_EXPORT HierarchyTree
62 {
63  using S = typename BV::S;
65 
66  struct SortByMorton
67  {
68  SortByMorton(NodeType* nodes_in) : nodes(nodes_in) {}
69  SortByMorton(NodeType* nodes_in, uint32 split_in)
70  : nodes(nodes_in), split(split_in) {}
71  bool operator() (size_t a, size_t b) const
72  {
73  if((a != NULL_NODE) && (b != NULL_NODE))
74  return nodes[a].code < nodes[b].code;
75  else if(a == NULL_NODE)
76  return split < nodes[b].code;
77  else if(b == NULL_NODE)
78  return nodes[a].code < split;
79 
80  return false;
81  }
82 
83  NodeType* nodes{};
84  uint32 split{};
85  };
86 
87 public:
92  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
93 
94  ~HierarchyTree();
95 
97  void init(NodeType* leaves, int n_leaves_, int level = 0);
98 
100  size_t insert(const BV& bv, void* data);
101 
103  void remove(size_t leaf);
104 
106  void clear();
107 
109  bool empty() const;
110 
112  void update(size_t leaf, int lookahead_level = -1);
113 
115  bool update(size_t leaf, const BV& bv);
116 
118  bool update(size_t leaf, const BV& bv, const Vector3<S>& vel, S margin);
119 
121  bool update(size_t leaf, const BV& bv, const Vector3<S>& vel);
122 
124  size_t getMaxHeight() const;
125 
127  size_t getMaxDepth() const;
128 
130  void balanceBottomup();
131 
133  void balanceTopdown();
134 
136  void balanceIncremental(int iterations);
137 
139  void refit();
140 
142  void extractLeaves(size_t root, NodeType*& leaves) const;
143 
145  size_t size() const;
146 
148  size_t getRoot() const;
149 
151  NodeType* getNodes() const;
152 
154  void print(size_t root, int depth);
155 
156 private:
157 
159  void bottomup(size_t* lbeg, size_t* lend);
160 
162  size_t topdown(size_t* lbeg, size_t* lend);
163 
165  size_t getMaxHeight(size_t node) const;
166 
168  void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
169 
173  size_t topdown_0(size_t* lbeg, size_t* lend);
174 
179  size_t topdown_1(size_t* lbeg, size_t* lend);
180 
182  void init_0(NodeType* leaves, int n_leaves_);
183 
186  void init_1(NodeType* leaves, int n_leaves_);
187 
190  void init_2(NodeType* leaves, int n_leaves_);
191 
193  void init_3(NodeType* leaves, int n_leaves_);
194 
195  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32& split, int bits);
196 
197  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32& split, int bits);
198 
199  size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
200 
202  void update_(size_t leaf, const BV& bv);
203 
205  void insertLeaf(size_t root, size_t leaf);
206 
209  size_t removeLeaf(size_t leaf);
210 
212  void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1);
213 
214  size_t indexOf(size_t node);
215 
216  size_t allocateNode();
217 
219  size_t createNode(size_t parent,
220  const BV& bv1,
221  const BV& bv2,
222  void* data);
223 
224  size_t createNode(size_t parent,
225  const BV& bv,
226  void* data);
227 
228  size_t createNode(size_t parent,
229  void* data);
230 
231  void deleteNode(size_t node);
232 
233  void recurseRefit(size_t node);
234 
235 protected:
236  size_t root_node;
238  size_t n_nodes;
240 
241  size_t n_leaves;
242  size_t freelist;
243  unsigned int opath;
244 
246 
247 public:
250 
253 
254 public:
255  static const size_t NULL_NODE = -1;
256 };
257 
258 template<typename BV>
259 const size_t HierarchyTree<BV>::NULL_NODE;
260 
261 
263 template<typename BV>
265 {
266  nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
267 
268  bool operator() (size_t i, size_t j) const;
269 
270 private:
271 
274 
276  size_t d;
277 };
278 
281 template<typename BV>
282 size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
283 
286 template<typename BV>
287 size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
288 
289 } // namespace implementation_array
290 } // namespace detail
291 } // namespace fcl
292 
294 
295 #endif
fcl::detail::implementation_array::HierarchyTree::bu_threshold
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition: hierarchy_tree_array.h:252
fcl::detail::implementation_array::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree_array.h:61
fcl::uint32
std::uint32_t uint32
Definition: types.h:62
fcl::detail::implementation_array::nodeBaseLess::d
size_t d
the dimension to compare
Definition: hierarchy_tree_array.h:276
fcl::detail::implementation_array::HierarchyTree::NodeType
NodeBase< BV > NodeType
Definition: hierarchy_tree_array.h:64
fcl::detail::HierarchyTree< fcl::AABB< S > >::S
typename fcl::AABB< S > ::S S
Definition: hierarchy_tree.h:62
fcl::detail::implementation_array::nodeBaseLess::nodeBaseLess
nodeBaseLess(const NodeBase< BV > *nodes_, size_t d_)
Definition: hierarchy_tree_array-inl.h:1049
fcl::detail::implementation_array::HierarchyTree::SortByMorton
Definition: hierarchy_tree_array.h:66
fcl::detail::implementation_array::HierarchyTree::n_nodes_alloc
size_t n_nodes_alloc
Definition: hierarchy_tree_array.h:239
fcl::detail::implementation_array::HierarchyTree::opath
unsigned int opath
Definition: hierarchy_tree_array.h:243
fcl::detail::implementation_array::nodeBaseLess::nodes
const NodeBase< BV > * nodes
the nodes array
Definition: hierarchy_tree_array.h:273
warning.h
fcl::detail::implementation_array::nodeBaseLess
Functor comparing two nodes.
Definition: hierarchy_tree_array.h:264
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::implementation_array::HierarchyTree::n_nodes
size_t n_nodes
Definition: hierarchy_tree_array.h:238
fcl::detail::implementation_array::HierarchyTree::freelist
size_t freelist
Definition: hierarchy_tree_array.h:242
hierarchy_tree_array-inl.h
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::NodeBase
dynamic AABB tree node
Definition: node_base.h:51
fcl::detail::implementation_array::HierarchyTree::max_lookahead_level
int max_lookahead_level
Definition: hierarchy_tree_array.h:245
morton.h
fcl::detail::implementation_array::HierarchyTree::SortByMorton::SortByMorton
SortByMorton(NodeType *nodes_in)
Definition: hierarchy_tree_array.h:68
fcl::detail::implementation_array::HierarchyTree::nodes
NodeType * nodes
Definition: hierarchy_tree_array.h:237
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::n_leaves
size_t n_leaves
Definition: hierarchy_tree_array.h:241
AABB.h
fcl::detail::implementation_array::HierarchyTree::root_node
size_t root_node
Definition: hierarchy_tree_array.h:236
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
node_base_array.h
fcl::detail::implementation_array::HierarchyTree::topdown_level
int topdown_level
decide which topdown algorithm to use
Definition: hierarchy_tree_array.h:249
fcl::detail::implementation_array::HierarchyTree::SortByMorton::SortByMorton
SortByMorton(NodeType *nodes_in, uint32 split_in)
Definition: hierarchy_tree_array.h:69


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:51