hierarchy_tree.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_H
39 #define HPP_FCL_HIERARCHY_TREE_H
40 
41 #include <vector>
42 #include <map>
43 #include <functional>
44 #include <iostream>
45 #include "hpp/fcl/warning.hh"
46 #include "hpp/fcl/BV/AABB.h"
49 
50 namespace hpp {
51 namespace fcl {
52 
53 namespace detail {
54 
56 template <typename BV>
58  public:
59  typedef NodeBase<BV> Node;
60 
66  HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
67 
69 
72  void init(std::vector<Node*>& leaves, int level = 0);
73 
75  Node* insert(const BV& bv, void* data);
76 
78  void remove(Node* leaf);
79 
81  void clear();
82 
84  bool empty() const;
85 
97  void update(Node* leaf, int lookahead_level = -1);
98 
101  bool update(Node* leaf, const BV& bv);
102 
104  bool update(Node* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin);
105 
107  bool update(Node* leaf, const BV& bv, const Vec3f& vel);
108 
110  size_t getMaxHeight() const;
111 
113  size_t getMaxDepth() const;
114 
116  void balanceBottomup();
117 
119  void balanceTopdown();
120 
122  void balanceIncremental(int iterations);
123 
126  void refit();
127 
129  void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
130 
132  size_t size() const;
133 
135  Node* getRoot() const;
136 
137  Node*& getRoot();
138 
140  void print(Node* root, int depth);
141 
142  private:
143  typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
144  typedef
145  typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
146 
147  struct SortByMorton {
148  bool operator()(const Node* a, const Node* b) const {
149  return a->code < b->code;
150  }
151  };
152 
154  void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
155 
157  Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
158 
160  size_t getMaxHeight(Node* node) const;
161 
163  void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
164 
170  Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
171 
178  Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
179 
182  void init_0(std::vector<Node*>& leaves);
183 
188  void init_1(std::vector<Node*>& leaves);
189 
194  void init_2(std::vector<Node*>& leaves);
195 
199  void init_3(std::vector<Node*>& leaves);
200 
201  Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
202  const uint32_t& split, int bits);
203 
204  Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
205  const uint32_t& split, int bits);
206 
207  Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
208 
210  void update_(Node* leaf, const BV& bv);
211 
213  Node* sort(Node* n, Node*& r);
214 
220  // leaf node.
221  void insertLeaf(Node* const sub_root, Node* const leaf);
222 
229  // adjusted.
230  Node* removeLeaf(Node* const leaf);
231 
234  void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
235 
236  static size_t indexOf(Node* node);
237 
239  Node* createNode(Node* parent, const BV& bv, void* data);
240 
241  Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
242 
243  Node* createNode(Node* parent, void* data);
244 
245  void deleteNode(Node* node);
246 
247  void recurseDeleteNode(Node* node);
248 
249  void recurseRefit(Node* node);
250 
251  protected:
252  Node* root_node;
253 
254  size_t n_leaves;
255 
256  unsigned int opath;
257 
260  Node* free_node;
261 
263 
264  public:
267 
270 };
271 
273 template <typename BV>
274 bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
275 
278 template <typename BV>
279 size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
280  const NodeBase<BV>& node2);
281 
284 template <typename BV>
285 size_t select(const BV& query, const NodeBase<BV>& node1,
286  const NodeBase<BV>& node2);
287 
288 } // namespace detail
289 } // namespace fcl
290 } // namespace hpp
291 
293 
294 #endif
Node * createNode(Node *parent, const BV &bv, void *data)
create one node (leaf or internal)
void init_2(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more tha...
std::vector< NodeBase< BV > * >::iterator NodeVecIterator
int topdown_level
decide which topdown algorithm to use
Main namespace.
uint32_t code
morton code for current BV
Definition: node_base.h:70
void balanceTopdown()
balance the tree from top
Node * mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend)
Node * topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. During construction...
static size_t indexOf(Node *node)
data
Node * getRoot() const
get the root of the tree
void fetchLeaves(Node *root, std::vector< Node *> &leaves, int depth=-1)
Delete all internal nodes and return all leaves nodes with given depth from root. ...
Node * mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t &split, int bits)
int bu_threshold
decide the depth to use expensive bottom-up algorithm
void balanceIncremental(int iterations)
balance the tree in an incremental way
void refit()
refit the tree, i.e., when the leaf nodes&#39; bounding volumes change, update the entire tree in a botto...
size_t size() const
number of leaves in the tree
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...
Class for hierarchy tree structure.
double FCL_REAL
Definition: data_types.h:65
size_t getMaxHeight() const
get the max height of the tree
bool operator()(const Node *a, const Node *b) const
void update(Node *leaf, int lookahead_level=-1)
Updates a leaf node. A use case is when the bounding volume of an object changes. Ensure every parent...
Node * topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. During construction...
std::vector< NodeBase< BV > * >::const_iterator NodeVecConstIterator
void balanceBottomup()
balance the tree from bottom
Node * mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32_t &split, int bits)
void extractLeaves(const Node *root, std::vector< Node *> &leaves) const
extract all the leaves of the tree
Node * sort(Node *n, Node *&r)
sort node n and its parent according to their memory position
void insertLeaf(Node *const sub_root, Node *const leaf)
Insert a leaf node and also update its ancestors. Maintain the tree as a full binary tree (every inte...
size_t getMaxDepth() const
get the max depth of the tree
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 ...
Node * topdown(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from top
void print(Node *root, int depth)
print the tree in a recursive way
void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend)
construct a tree for a set of leaves from bottom – very heavy way
dynamic AABB tree node
Definition: node_base.h:50
void init_3(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the ...
Node * insert(const BV &bv, void *data)
Insest a node.
void init(std::vector< Node *> &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Node * removeLeaf(Node *const leaf)
Remove a leaf. Maintain the tree as a full binary tree (every interior node has exactly two children)...
void init_1(std::vector< Node *> &leaves)
init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more tha...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
void update_(Node *leaf, const BV &bv)
update one leaf node&#39;s bounding volume
bool empty() const
Whether the tree is empty.
bool nodeBaseLess(NodeBase< BV > *a, NodeBase< BV > *b, int d)
Compare two nodes accoording to the d-th dimension of node center.
void init_0(std::vector< Node *> &leaves)
init tree from leaves in the topdown manner (topdown_0 or topdown_1)


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:01