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 HPP_FCL_HIERARCHY_TREE_ARRAY_H
39 #define HPP_FCL_HIERARCHY_TREE_ARRAY_H
40 
41 #include <vector>
42 #include <map>
43 #include <functional>
44 
45 #include "hpp/fcl/fwd.hh"
46 #include "hpp/fcl/BV/AABB.h"
49 
50 namespace hpp {
51 namespace fcl {
52 
53 namespace detail {
54 
55 namespace implementation_array {
56 
58 template <typename BV>
60  public:
61  typedef NodeBase<BV> Node;
62 
63  private:
64  struct SortByMorton {
65  SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
66  SortByMorton(Node* nodes_in, uint32_t split_in)
67  : nodes(nodes_in), split(split_in) {}
68  bool operator()(size_t a, size_t b) const {
69  if ((a != NULL_NODE) && (b != NULL_NODE))
70  return nodes[a].code < nodes[b].code;
71  else if (a == NULL_NODE)
72  return split < nodes[b].code;
73  else if (b == NULL_NODE)
74  return nodes[a].code < split;
75 
76  return false;
77  }
78 
79  Node* nodes{};
81  };
82 
83  public:
89  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
90 
92 
95  void init(Node* leaves, int n_leaves_, int level = 0);
96 
99  size_t insert(const BV& bv, void* data);
100 
102  void remove(size_t leaf);
103 
105  void clear();
106 
108  bool empty() const;
109 
111  void update(size_t leaf, int lookahead_level = -1);
112 
115  bool update(size_t leaf, const BV& bv);
116 
118  bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
119 
121  bool update(size_t leaf, const BV& bv, const Vec3f& 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 
140  void refit();
141 
143  void extractLeaves(size_t root, Node*& leaves) const;
144 
146  size_t size() const;
147 
149  size_t getRoot() const;
150 
152  Node* getNodes() const;
153 
155  void print(size_t root, int depth);
156 
157  private:
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 
175  size_t topdown_0(size_t* lbeg, size_t* lend);
176 
183  size_t topdown_1(size_t* lbeg, size_t* lend);
184 
187  void init_0(Node* leaves, int n_leaves_);
188 
193  void init_1(Node* leaves, int n_leaves_);
194 
199  void init_2(Node* leaves, int n_leaves_);
200 
204  void init_3(Node* leaves, int n_leaves_);
205 
206  size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
207  int bits);
208 
209  size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
210  int bits);
211 
212  size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
213 
215  void update_(size_t leaf, const BV& bv);
216 
218  void insertLeaf(size_t root, size_t leaf);
219 
223  size_t removeLeaf(size_t leaf);
224 
227  void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
228 
229  size_t indexOf(size_t node);
230 
231  size_t allocateNode();
232 
234  size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
235 
236  size_t createNode(size_t parent, const BV& bv, void* data);
237 
238  size_t createNode(size_t parent, void* data);
239 
240  void deleteNode(size_t node);
241 
242  void recurseRefit(size_t node);
243 
244  protected:
245  size_t root_node;
247  size_t n_nodes;
249 
250  size_t n_leaves;
251  size_t freelist;
252  unsigned int opath;
253 
255 
256  public:
259 
262 
263  public:
264  static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
265 };
266 
267 template <typename BV>
268 const size_t HierarchyTree<BV>::NULL_NODE;
269 
271 template <typename BV>
272 struct nodeBaseLess {
273  nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
274 
275  bool operator()(size_t i, size_t j) const;
276 
277  private:
280 
282  size_t d;
283 };
284 
287 template <typename BV>
288 size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
289 
292 template <typename BV>
293 size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
294 
295 } // namespace implementation_array
296 } // namespace detail
297 } // namespace fcl
298 } // namespace hpp
299 
301 
302 #endif
hpp::fcl::detail::implementation_array::HierarchyTree::getMaxHeight
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree_array-inl.h:322
hpp::fcl::detail::implementation_array::NodeBase
Definition: node_base_array.h:51
hpp::fcl::detail::implementation_array::HierarchyTree::nodes
Node * nodes
Definition: hierarchy_tree_array.h:246
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton::operator()
bool operator()(size_t a, size_t b) const
Definition: hierarchy_tree_array.h:68
hpp::fcl::detail::implementation_array::nodeBaseLess::nodes
const NodeBase< BV > * nodes
the nodes array
Definition: hierarchy_tree_array.h:279
hpp::fcl::detail::implementation_array::HierarchyTree::print
void print(size_t root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree_array-inl.h:440
hpp::fcl::detail::implementation_array::HierarchyTree::max_lookahead_level
int max_lookahead_level
Definition: hierarchy_tree_array.h:254
hpp::fcl::detail::implementation_array::HierarchyTree::remove
void remove(size_t leaf)
Remove a leaf node.
Definition: hierarchy_tree_array-inl.h:246
hpp::fcl::detail::implementation_array::HierarchyTree::indexOf
size_t indexOf(size_t node)
Definition: hierarchy_tree_array-inl.h:802
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton::SortByMorton
SortByMorton(Node *nodes_in)
Definition: hierarchy_tree_array.h:65
hpp::fcl::detail::implementation_array::HierarchyTree::freelist
size_t freelist
Definition: hierarchy_tree_array.h:251
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton
Definition: hierarchy_tree_array.h:64
hpp::fcl::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: hierarchy_tree_array-inl.h:99
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton::SortByMorton
SortByMorton(Node *nodes_in, uint32_t split_in)
Definition: hierarchy_tree_array.h:66
hpp::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:524
hpp::fcl::detail::implementation_array::HierarchyTree::allocateNode
size_t allocateNode()
Definition: hierarchy_tree_array-inl.h:808
hpp::fcl::detail::implementation_array::HierarchyTree::n_nodes
size_t n_nodes
Definition: hierarchy_tree_array.h:247
uint32_t
uint32_t
hpp::fcl::detail::implementation_array::nodeBaseLess
Functor comparing two nodes.
Definition: hierarchy_tree_array.h:272
hpp::fcl::detail::implementation_array::HierarchyTree::clear
void clear()
Clear the tree.
Definition: hierarchy_tree_array-inl.h:254
hpp::fcl::detail::implementation_array::HierarchyTree::size
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree_array-inl.h:422
hpp::fcl::detail::implementation_array::NodeBase::code
uint32_t code
Definition: node_base_array.h:64
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton::split
uint32_t split
Definition: hierarchy_tree_array.h:80
hpp::fcl::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: hierarchy_tree_array-inl.h:882
hpp::fcl::detail::implementation_array::HierarchyTree::getRoot
size_t getRoot() const
get the root of the tree
Definition: hierarchy_tree_array-inl.h:428
hpp::fcl::detail::implementation_array::HierarchyTree::recurseRefit
void recurseRefit(size_t node)
Definition: hierarchy_tree_array-inl.h:870
hpp::fcl::detail::implementation_array::HierarchyTree::root_node
size_t root_node
Definition: hierarchy_tree_array.h:245
a
list a
hpp::fcl::detail::implementation_array::HierarchyTree::init
void init(Node *leaves, int n_leaves_, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree_array-inl.h:78
hpp::fcl::detail::implementation_array::HierarchyTree::NULL_NODE
static const size_t NULL_NODE
Definition: hierarchy_tree_array.h:264
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hierarchy_tree_array-inl.h
hpp::fcl::detail::implementation_array::HierarchyTree::topdown_level
int topdown_level
decide which topdown algorithm to use
Definition: hierarchy_tree_array.h:258
hpp::fcl::detail::implementation_array::HierarchyTree::SortByMorton::nodes
Node * nodes
Definition: hierarchy_tree_array.h:79
hpp::fcl::detail::implementation_array::select
size_t select(size_t query, size_t node1, size_t node2, NodeBase< BV > *nodes)
select the node from node1 and node2 which is close to the query-th node in the nodes....
Definition: hierarchy_tree_array-inl.h:934
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::detail::implementation_array::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree_array.h:59
hpp::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:509
hpp::fcl::detail::implementation_array::HierarchyTree::update
void update(size_t leaf, int lookahead_level=-1)
update one leaf node
Definition: hierarchy_tree_array-inl.h:276
hpp::fcl::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: hierarchy_tree_array-inl.h:162
hpp::fcl::detail::implementation_array::HierarchyTree::~HierarchyTree
~HierarchyTree()
Definition: hierarchy_tree_array-inl.h:72
hpp::fcl::detail::implementation_array::HierarchyTree::mortonRecurse_0
size_t mortonRecurse_0(size_t *lbeg, size_t *lend, const uint32_t &split, int bits)
Definition: hierarchy_tree_array-inl.h:614
hpp::fcl::detail::implementation_array::HierarchyTree::getMaxDepth
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree_array-inl.h:330
hpp::fcl::detail::implementation_array::HierarchyTree::balanceIncremental
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree_array-inl.h:386
hpp::fcl::detail::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:751
hpp::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:832
hpp::fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:50
morton.h
hpp::fcl::detail::implementation_array::HierarchyTree::empty
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree_array-inl.h:270
hpp::fcl::detail::implementation_array::HierarchyTree::update_
void update_(size_t leaf, const BV &bv)
update one leaf node's bounding volume
Definition: hierarchy_tree_array-inl.h:786
hpp::fcl::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: hierarchy_tree_array-inl.h:200
generate_distance_plot.b
float b
Definition: generate_distance_plot.py:7
hpp::fcl::detail::implementation_array::HierarchyTree::balanceBottomup
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree_array-inl.h:340
hpp::fcl::detail::implementation_array::HierarchyTree::mortonRecurse_2
size_t mortonRecurse_2(size_t *lbeg, size_t *lend)
Definition: hierarchy_tree_array-inl.h:694
hpp::fcl::detail::implementation_array::HierarchyTree::HierarchyTree
HierarchyTree(int bu_threshold_=16, int topdown_level_=0)
Create hierarchy tree with suitable setting. bu_threshold decides the height of tree node to start bo...
Definition: hierarchy_tree_array-inl.h:55
hpp::fcl::detail::implementation_array::HierarchyTree::bu_threshold
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition: hierarchy_tree_array.h:261
hpp::fcl::detail::implementation_array::nodeBaseLess::d
size_t d
the dimension to compare
Definition: hierarchy_tree_array.h:282
hpp::fcl::detail::implementation_array::HierarchyTree::insert
size_t insert(const BV &bv, void *data)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree_array-inl.h:237
hpp::fcl::detail::implementation_array::HierarchyTree::balanceTopdown
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree_array-inl.h:364
hpp::fcl::detail::implementation_array::nodeBaseLess::operator()
bool operator()(size_t i, size_t j) const
Definition: hierarchy_tree_array-inl.h:902
hpp::fcl::detail::implementation_array::HierarchyTree::Node
NodeBase< BV > Node
Definition: hierarchy_tree_array.h:61
hpp::fcl::detail::implementation_array::HierarchyTree::deleteNode
void deleteNode(size_t node)
Definition: hierarchy_tree_array-inl.h:862
AABB.h
fwd.hh
hpp::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:477
hpp::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:556
hpp::fcl::detail::implementation_array::HierarchyTree::n_nodes_alloc
size_t n_nodes_alloc
Definition: hierarchy_tree_array.h:248
hpp::fcl::detail::implementation_array::nodeBaseLess::nodeBaseLess
nodeBaseLess(const NodeBase< BV > *nodes_, size_t d_)
Definition: hierarchy_tree_array-inl.h:895
hpp::fcl::detail::implementation_array::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:711
hpp::fcl::detail::implementation_array::HierarchyTree::n_leaves
size_t n_leaves
Definition: hierarchy_tree_array.h:250
hpp::fcl::detail::implementation_array::HierarchyTree::refit
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
Definition: hierarchy_tree_array-inl.h:404
hpp::fcl::detail::implementation_array::HierarchyTree::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: hierarchy_tree_array-inl.h:124
hpp::fcl::detail::implementation_array::HierarchyTree::extractLeaves
void extractLeaves(size_t root, Node *&leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree_array-inl.h:410
node_base_array.h
hpp::fcl::detail::implementation_array::HierarchyTree::opath
unsigned int opath
Definition: hierarchy_tree_array.h:252
hpp::fcl::detail::implementation_array::HierarchyTree::mortonRecurse_1
size_t mortonRecurse_1(size_t *lbeg, size_t *lend, const uint32_t &split, int bits)
Definition: hierarchy_tree_array-inl.h:651
hpp::fcl::detail::implementation_array::HierarchyTree::getNodes
Node * getNodes() const
get the pointer to the nodes array
Definition: hierarchy_tree_array-inl.h:434


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:14