hpp-fcl 2.2.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
50namespace hpp {
51namespace fcl {
52
53namespace detail {
54
56template <typename BV>
58 public:
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:
253
254 size_t n_leaves;
255
256 unsigned int opath;
257
261
263
264 public:
267
270};
271
273template <typename BV>
274bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
275
278template <typename BV>
279size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
280 const NodeBase<BV>& node2);
281
284template <typename BV>
285size_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
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition: hierarchy_tree.h:269
~HierarchyTree()
Definition: hierarchy_tree-inl.h:62
int max_lookahead_level
Definition: hierarchy_tree.h:262
void clear()
Clear the tree.
Definition: hierarchy_tree-inl.h:107
void init(std::vector< Node * > &leaves, int level=0)
Initialize the tree by a set of leaves using algorithm with a given level.
Definition: hierarchy_tree-inl.h:68
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...
Definition: hierarchy_tree-inl.h:124
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree-inl.h:193
int topdown_level
decide which topdown algorithm to use
Definition: hierarchy_tree.h:266
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree-inl.h:268
void extractLeaves(const Node *root, std::vector< Node * > &leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree-inl.h:257
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree-inl.h:200
Node * insert(const BV &bv, void *data)
Insest a node.
Definition: hierarchy_tree-inl.h:89
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree-inl.h:233
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree-inl.h:210
void refit()
refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a botto...
Definition: hierarchy_tree-inl.h:251
size_t n_leaves
Definition: hierarchy_tree.h:254
Node * getRoot() const
get the root of the tree
Definition: hierarchy_tree-inl.h:274
void remove(Node *leaf)
Remove a leaf node.
Definition: hierarchy_tree-inl.h:99
Node * root_node
Definition: hierarchy_tree.h:252
NodeBase< BV > Node
Definition: hierarchy_tree.h:59
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree-inl.h:222
Node * free_node
Definition: hierarchy_tree.h:260
unsigned int opath
Definition: hierarchy_tree.h:256
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree-inl.h:118
void print(Node *root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree-inl.h:286
bool nodeBaseLess(NodeBase< BV > *a, NodeBase< BV > *b, int d)
Compare two nodes accoording to the d-th dimension of node center.
Definition: hierarchy_tree-inl.h:930
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
Definition: hierarchy_tree-inl.h:952
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
dynamic AABB tree node
Definition: node_base.h:50
uint32_t code
morton code for current BV
Definition: node_base.h:70