hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
50namespace hpp {
51namespace fcl {
52
53namespace detail {
54
55namespace implementation_array {
56
58template <typename BV>
60 public:
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{};
80 uint32_t split{};
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
267template <typename BV>
269
271template <typename BV>
273 nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
274
275 bool operator()(size_t i, size_t j) const;
276
277 private:
279 const NodeBase<BV>* nodes;
280
282 size_t d;
283};
284
287template <typename BV>
288size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
289
292template <typename BV>
293size_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
Class for hierarchy tree structure.
Definition: hierarchy_tree_array.h:59
size_t getRoot() const
get the root of the tree
Definition: hierarchy_tree_array-inl.h:428
void extractLeaves(size_t root, Node *&leaves) const
extract all the leaves of the tree
Definition: hierarchy_tree_array-inl.h:410
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
Node * getNodes() const
get the pointer to the nodes array
Definition: hierarchy_tree_array-inl.h:434
Node * nodes
Definition: hierarchy_tree_array.h:246
size_t getMaxHeight() const
get the max height of the tree
Definition: hierarchy_tree_array-inl.h:322
void print(size_t root, int depth)
print the tree in a recursive way
Definition: hierarchy_tree_array-inl.h:440
size_t root_node
Definition: hierarchy_tree_array.h:245
int max_lookahead_level
Definition: hierarchy_tree_array.h:254
void balanceTopdown()
balance the tree from top
Definition: hierarchy_tree_array-inl.h:364
size_t freelist
Definition: hierarchy_tree_array.h:251
size_t getMaxDepth() const
get the max depth of the tree
Definition: hierarchy_tree_array-inl.h:330
size_t n_leaves
Definition: hierarchy_tree_array.h:250
static const size_t NULL_NODE
Definition: hierarchy_tree_array.h:264
size_t n_nodes
Definition: hierarchy_tree_array.h:247
~HierarchyTree()
Definition: hierarchy_tree_array-inl.h:72
void update(size_t leaf, int lookahead_level=-1)
update one leaf node
Definition: hierarchy_tree_array-inl.h:276
int topdown_level
decide which topdown algorithm to use
Definition: hierarchy_tree_array.h:258
void balanceBottomup()
balance the tree from bottom
Definition: hierarchy_tree_array-inl.h:340
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition: hierarchy_tree_array.h:261
NodeBase< BV > Node
Definition: hierarchy_tree_array.h:61
size_t size() const
number of leaves in the tree
Definition: hierarchy_tree_array-inl.h:422
unsigned int opath
Definition: hierarchy_tree_array.h:252
bool empty() const
Whether the tree is empty.
Definition: hierarchy_tree_array-inl.h:270
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
size_t n_nodes_alloc
Definition: hierarchy_tree_array.h:248
void remove(size_t leaf)
Remove a leaf node.
Definition: hierarchy_tree_array-inl.h:246
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition: hierarchy_tree_array-inl.h:386
void clear()
Clear the tree.
Definition: hierarchy_tree_array-inl.h:254
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
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
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
Definition: node_base_array.h:51
Functor comparing two nodes.
Definition: hierarchy_tree_array.h:272
bool operator()(size_t i, size_t j) const
Definition: hierarchy_tree_array-inl.h:902