GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/detail/hierarchy_tree.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 2 2 100.0%
Branches: 0 0 -%
Line Branch Exec Source
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
36 /** @author Jia Pan */
37
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"
47 #include "coal/broadphase/detail/morton.h"
48 #include "coal/broadphase/detail/node_base.h"
49
50 namespace coal {
51
52 namespace detail {
53
54 /// @brief Class for hierarchy tree structure
55 template <typename BV>
56 class HierarchyTree {
57 public:
58 typedef NodeBase<BV> Node;
59
60 /// @brief Create hierarchy tree with suitable setting.
61 /// bu_threshold decides the height of tree node to start bottom-up
62 /// construction / optimization; topdown_level decides different methods to
63 /// construct tree in topdown manner. lower level method constructs tree with
64 /// better quality but is slower.
65 HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
66
67 ~HierarchyTree();
68
69 /// @brief Initialize the tree by a set of leaves using algorithm with a given
70 /// level.
71 void init(std::vector<Node*>& leaves, int level = 0);
72
73 /// @brief Insest a node
74 Node* insert(const BV& bv, void* data);
75
76 /// @brief Remove a leaf node
77 void remove(Node* leaf);
78
79 /// @brief Clear the tree
80 void clear();
81
82 /// @brief Whether the tree is empty
83 bool empty() const;
84
85 /// @brief Updates a `leaf` node. A use case is when the bounding volume
86 /// of an object changes. Ensure every parent node has its bounding volume
87 /// expand or shrink to fit the bounding volumes of its children.
88 /// @note Strangely the structure of the tree may change even if the
89 /// bounding volume of the `leaf` node does not change. It is just
90 /// another valid tree of the same set of objects. This is because
91 /// update() works by first removing `leaf` and then inserting `leaf`
92 /// back. The structural change could be as simple as switching the
93 /// order of two leaves if the sibling of the `leaf` is also a leaf.
94 /// Or it could be more complicated if the sibling is an internal
95 /// node.
96 void update(Node* leaf, int lookahead_level = -1);
97
98 /// @brief update the tree when the bounding volume of a given leaf has
99 /// changed
100 bool update(Node* leaf, const BV& bv);
101
102 /// @brief update one leaf's bounding volume, with prediction
103 bool update(Node* leaf, const BV& bv, const Vec3s& vel, Scalar margin);
104
105 /// @brief update one leaf's bounding volume, with prediction
106 bool update(Node* leaf, const BV& bv, const Vec3s& vel);
107
108 /// @brief get the max height of the tree
109 size_t getMaxHeight() const;
110
111 /// @brief get the max depth of the tree
112 size_t getMaxDepth() const;
113
114 /// @brief balance the tree from bottom
115 void balanceBottomup();
116
117 /// @brief balance the tree from top
118 void balanceTopdown();
119
120 /// @brief balance the tree in an incremental way
121 void balanceIncremental(int iterations);
122
123 /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
124 /// update the entire tree in a bottom-up manner
125 void refit();
126
127 /// @brief extract all the leaves of the tree
128 void extractLeaves(const Node* root, std::vector<Node*>& leaves) const;
129
130 /// @brief number of leaves in the tree
131 size_t size() const;
132
133 /// @brief get the root of the tree
134 Node* getRoot() const;
135
136 Node*& getRoot();
137
138 /// @brief print the tree in a recursive way
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 208655 bool operator()(const Node* a, const Node* b) const {
148 208655 return a->code < b->code;
149 }
150 };
151
152 /// @brief construct a tree for a set of leaves from bottom -- very heavy way
153 void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend);
154
155 /// @brief construct a tree for a set of leaves from top
156 Node* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend);
157
158 /// @brief compute the maximum height of a subtree rooted from a given node
159 size_t getMaxHeight(Node* node) const;
160
161 /// @brief compute the maximum depth of a subtree rooted from a given node
162 void getMaxDepth(Node* node, size_t depth, size_t& max_depth) const;
163
164 /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
165 /// topdown manner. During construction, first compute the best split axis as
166 /// the axis along with the longest AABB edge. Then compute the median of all
167 /// nodes' center projection onto the axis and using it as the split
168 /// threshold.
169 Node* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend);
170
171 /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
172 /// topdown manner. During construction, first compute the best split
173 /// thresholds for different axes as the average of all nodes' center. Then
174 /// choose the split axis as the axis whose threshold can divide the nodes
175 /// into two parts with almost similar size. This construction is more
176 /// expensive then topdown_0, but also can provide tree with better quality.
177 Node* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend);
178
179 /// @brief init tree from leaves in the topdown manner (topdown_0 or
180 /// topdown_1)
181 void init_0(std::vector<Node*>& leaves);
182
183 /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
184 /// for nodes which is of depth more than the maximum bits of the morton code,
185 /// we use bottomup method to construct the subtree, which is slow but can
186 /// construct tree with high quality.
187 void init_1(std::vector<Node*>& leaves);
188
189 /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
190 /// for nodes which is of depth more than the maximum bits of the morton code,
191 /// we split the leaves into two parts with the same size simply using the
192 /// node index.
193 void init_2(std::vector<Node*>& leaves);
194
195 /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
196 /// for all nodes, we simply divide the leaves into parts with the same size
197 /// simply using the node index.
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
208 /// @brief update one leaf node's bounding volume
209 void update_(Node* leaf, const BV& bv);
210
211 /// @brief sort node n and its parent according to their memory position
212 Node* sort(Node* n, Node*& r);
213
214 /// @brief Insert a leaf node and also update its ancestors. Maintain the
215 /// tree as a full binary tree (every interior node has exactly two children).
216 /// Furthermore, adjust the BV of interior nodes so that each parent's BV
217 /// tightly fits its children's BVs.
218 /// @param sub_root The root of the subtree into which we will insert the
219 // leaf node.
220 void insertLeaf(Node* const sub_root, Node* const leaf);
221
222 /// @brief Remove a leaf. Maintain the tree as a full binary tree (every
223 /// interior node has exactly two children). Furthermore, adjust the BV of
224 /// interior nodes so that each parent's BV tightly fits its children's BVs.
225 /// @note The leaf node itself is not deleted yet, but all the unnecessary
226 /// internal nodes are deleted.
227 /// @returns the root of the subtree containing the nodes whose BVs were
228 // adjusted.
229 Node* removeLeaf(Node* const leaf);
230
231 /// @brief Delete all internal nodes and return all leaves nodes with given
232 /// depth from root
233 void fetchLeaves(Node* root, std::vector<Node*>& leaves, int depth = -1);
234
235 static size_t indexOf(Node* node);
236
237 /// @brief create one node (leaf or internal)
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:
251 Node* root_node;
252
253 size_t n_leaves;
254
255 unsigned int opath;
256
257 /// This is a one Node cache, the reason is that we need to remove a node and
258 /// then add it again frequently.
259 Node* free_node;
260
261 int max_lookahead_level;
262
263 public:
264 /// @brief decide which topdown algorithm to use
265 int topdown_level;
266
267 /// @brief decide the depth to use expensive bottom-up algorithm
268 int bu_threshold;
269 };
270
271 /// @brief Compare two nodes accoording to the d-th dimension of node center
272 template <typename BV>
273 bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d);
274
275 /// @brief select from node1 and node2 which is close to a given query. 0 for
276 /// node1 and 1 for node2
277 template <typename BV>
278 size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1,
279 const NodeBase<BV>& node2);
280
281 /// @brief select from node1 and node2 which is close to a given query bounding
282 /// volume. 0 for node1 and 1 for node2
283 template <typename BV>
284 size_t select(const BV& query, const NodeBase<BV>& node1,
285 const NodeBase<BV>& node2);
286
287 } // namespace detail
288 } // namespace coal
289
290 #include "coal/broadphase/detail/hierarchy_tree-inl.h"
291
292 #endif
293