coal 3.0.1
Coal, The Collision Detection Library. Previously known as HPP-FCL, 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 COAL_HIERARCHY_TREE_H
39#define COAL_HIERARCHY_TREE_H
40
41#include <vector>
42#include <map>
43#include <functional>
44#include <iostream>
45#include "coal/warning.hh"
46#include "coal/BV/AABB.h"
49
50namespace coal {
51
52namespace detail {
53
55template <typename BV>
57 public:
59
65 HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
66
68
71 void init(std::vector<Node*>& leaves, int level = 0);
72
74 Node* insert(const BV& bv, void* data);
75
77 void remove(Node* leaf);
78
80 void clear();
81
83 bool empty() const;
84
96 void update(Node* leaf, int lookahead_level = -1);
97
100 bool update(Node* leaf, const BV& bv);
101
103 bool update(Node* leaf, const BV& bv, const Vec3s& vel, Scalar margin);
104
106 bool update(Node* leaf, const BV& bv, const Vec3s& vel);
107
109 size_t getMaxHeight() const;
110
112 size_t getMaxDepth() const;
113
115 void balanceBottomup();
116
118 void balanceTopdown();
119
121 void balanceIncremental(int iterations);
122
125 void refit();
126
128 void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
129
131 size_t size() const;
132
134 Node* getRoot() const;
135
136 Node*& getRoot();
137
139 void print(Node* root, int depth);
140
141 private:
142 typedef typename std::vector<NodeBase<BV>*>::iterator NodeVecIterator;
143 typedef
144 typename std::vector<NodeBase<BV>*>::const_iterator NodeVecConstIterator;
145
146 struct SortByMorton {
147 bool operator()(const Node* a, const Node* b) const {
148 return a->code < b->code;
149 }
150 };
151
153 void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
154
156 Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
157
159 size_t getMaxHeight(Node* node) const;
160
162 void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
163
169 Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
170
177 Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
178
181 void init_0(std::vector<Node*>& leaves);
182
187 void init_1(std::vector<Node*>& leaves);
188
193 void init_2(std::vector<Node*>& leaves);
194
198 void init_3(std::vector<Node*>& leaves);
199
200 Node* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend,
201 const uint32_t& split, int bits);
202
203 Node* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend,
204 const uint32_t& split, int bits);
205
206 Node* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend);
207
209 void update_(Node* leaf, const BV& bv);
210
212 Node* sort(Node* n, Node*& r);
213
219 // leaf node.
220 void insertLeaf(Node* const sub_root, Node* const leaf);
221
228 // adjusted.
229 Node* removeLeaf(Node* const leaf);
230
233 void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
234
235 static size_t indexOf(Node* node);
236
238 Node* createNode(Node* parent, const BV& bv, void* data);
239
240 Node* createNode(Node* parent, const BV& bv1, const BV& bv2, void* data);
241
242 Node* createNode(Node* parent, void* data);
243
244 void deleteNode(Node* node);
245
246 void recurseDeleteNode(Node* node);
247
248 void recurseRefit(Node* node);
249
250 protected:
252
253 size_t n_leaves;
254
255 unsigned int opath;
256
260
262
263 public:
266
269};
270
272template <typename BV>
273bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
274
277template <typename BV>
278size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
279 const NodeBase<BV>& node2);
280
283template <typename BV>
284size_t select(const BV& query, const NodeBase<BV>& node1,
285 const NodeBase<BV>& node2);
286
287} // namespace detail
288} // namespace coal
289
291
292#endif
Class for hierarchy tree structure.
Definition hierarchy_tree.h:56
~HierarchyTree()
Definition hierarchy_tree-inl.h:61
Node * getRoot() const
get the root of the tree
Definition hierarchy_tree-inl.h:273
void balanceBottomup()
balance the tree from bottom
Definition hierarchy_tree-inl.h:209
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:123
Node * insert(const BV &bv, void *data)
Insest a node.
Definition hierarchy_tree-inl.h:88
int topdown_level
decide which topdown algorithm to use
Definition hierarchy_tree.h:265
void clear()
Clear the tree.
Definition hierarchy_tree-inl.h:106
void remove(Node *leaf)
Remove a leaf node.
Definition hierarchy_tree-inl.h:98
void balanceTopdown()
balance the tree from top
Definition hierarchy_tree-inl.h:221
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:67
int bu_threshold
decide the depth to use expensive bottom-up algorithm
Definition hierarchy_tree.h:268
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:250
bool empty() const
Whether the tree is empty.
Definition hierarchy_tree-inl.h:117
size_t getMaxDepth() const
get the max depth of the tree
Definition hierarchy_tree-inl.h:199
size_t n_leaves
Definition hierarchy_tree.h:253
size_t size() const
number of leaves in the tree
Definition hierarchy_tree-inl.h:267
void balanceIncremental(int iterations)
balance the tree in an incremental way
Definition hierarchy_tree-inl.h:232
NodeBase< BV > Node
Definition hierarchy_tree.h:58
void print(Node *root, int depth)
print the tree in a recursive way
Definition hierarchy_tree-inl.h:285
Node * free_node
Definition hierarchy_tree.h:259
int max_lookahead_level
Definition hierarchy_tree.h:261
size_t getMaxHeight() const
get the max height of the tree
Definition hierarchy_tree-inl.h:192
unsigned int opath
Definition hierarchy_tree.h:255
Node * root_node
Definition hierarchy_tree.h:251
void extractLeaves(const Node *root, std::vector< Node * > &leaves) const
extract all the leaves of the tree
Definition hierarchy_tree-inl.h:256
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
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
Main namespace.
Definition broadphase_bruteforce.h:44
Eigen::Matrix< Scalar, 3, 1 > Vec3s
Definition data_types.h:70
double Scalar
Definition data_types.h:68
dynamic AABB tree node
Definition node_base.h:49
uint32_t code
morton code for current BV
Definition node_base.h:69