GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/detail/hierarchy_tree_array.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 9 11 81.8%
Functions: 3 3 100.0%
Branches: 5 8 62.5%
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_ARRAY_H
39 #define COAL_HIERARCHY_TREE_ARRAY_H
40
41 #include <vector>
42 #include <map>
43 #include <functional>
44
45 #include "coal/fwd.hh"
46 #include "coal/BV/AABB.h"
47 #include "coal/broadphase/detail/morton.h"
48 #include "coal/broadphase/detail/node_base_array.h"
49
50 namespace coal {
51
52 namespace detail {
53
54 namespace implementation_array {
55
56 /// @brief Class for hierarchy tree structure
57 template <typename BV>
58 class HierarchyTree {
59 public:
60 typedef NodeBase<BV> Node;
61
62 private:
63 struct SortByMorton {
64 43 SortByMorton(Node* nodes_in) : nodes(nodes_in) {}
65 24888 SortByMorton(Node* nodes_in, uint32_t split_in)
66 24888 : nodes(nodes_in), split(split_in) {}
67 208655 bool operator()(size_t a, size_t b) const {
68
3/4
✓ Branch 0 taken 208655 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 152863 times.
✓ Branch 3 taken 55792 times.
 208655 if ((a != NULL_NODE) && (b != NULL_NODE))
69 152863 return nodes[a].code < nodes[b].code;
70
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 55792 times.
 55792 else if (a == NULL_NODE)
71 ✗ return split < nodes[b].code;
72
1/2
✓ Branch 0 taken 55792 times.
✗ Branch 1 not taken.
 55792 else if (b == NULL_NODE)
73 55792 return nodes[a].code < split;
74
75 ✗ return false;
76 }
77
78 Node* nodes{};
79 uint32_t split{};
80 };
81
82 public:
83 /// @brief Create hierarchy tree with suitable setting.
84 /// bu_threshold decides the height of tree node to start bottom-up
85 /// construction / optimization; topdown_level decides different methods to
86 /// construct tree in topdown manner. lower level method constructs tree with
87 /// better quality but is slower.
88 HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0);
89
90 ~HierarchyTree();
91
92 /// @brief Initialize the tree by a set of leaves using algorithm with a given
93 /// level.
94 void init(Node* leaves, int n_leaves_, int level = 0);
95
96 /// @brief Initialize the tree by a set of leaves using algorithm with a given
97 /// level.
98 size_t insert(const BV& bv, void* data);
99
100 /// @brief Remove a leaf node
101 void remove(size_t leaf);
102
103 /// @brief Clear the tree
104 void clear();
105
106 /// @brief Whether the tree is empty
107 bool empty() const;
108
109 /// @brief update one leaf node
110 void update(size_t leaf, int lookahead_level = -1);
111
112 /// @brief update the tree when the bounding volume of a given leaf has
113 /// changed
114 bool update(size_t leaf, const BV& bv);
115
116 /// @brief update one leaf's bounding volume, with prediction
117 bool update(size_t leaf, const BV& bv, const Vec3s& vel, Scalar margin);
118
119 /// @brief update one leaf's bounding volume, with prediction
120 bool update(size_t leaf, const BV& bv, const Vec3s& vel);
121
122 /// @brief get the max height of the tree
123 size_t getMaxHeight() const;
124
125 /// @brief get the max depth of the tree
126 size_t getMaxDepth() const;
127
128 /// @brief balance the tree from bottom
129 void balanceBottomup();
130
131 /// @brief balance the tree from top
132 void balanceTopdown();
133
134 /// @brief balance the tree in an incremental way
135 void balanceIncremental(int iterations);
136
137 /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change,
138 /// update the entire tree in a bottom-up manner
139 void refit();
140
141 /// @brief extract all the leaves of the tree
142 void extractLeaves(size_t root, Node*& leaves) const;
143
144 /// @brief number of leaves in the tree
145 size_t size() const;
146
147 /// @brief get the root of the tree
148 size_t getRoot() const;
149
150 /// @brief get the pointer to the nodes array
151 Node* getNodes() const;
152
153 /// @brief print the tree in a recursive way
154 void print(size_t root, int depth);
155
156 private:
157 /// @brief construct a tree for a set of leaves from bottom -- very heavy way
158 void bottomup(size_t* lbeg, size_t* lend);
159
160 /// @brief construct a tree for a set of leaves from top
161 size_t topdown(size_t* lbeg, size_t* lend);
162
163 /// @brief compute the maximum height of a subtree rooted from a given node
164 size_t getMaxHeight(size_t node) const;
165
166 /// @brief compute the maximum depth of a subtree rooted from a given node
167 void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const;
168
169 /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
170 /// topdown manner. During construction, first compute the best split axis as
171 /// the axis along with the longest AABB edge. Then compute the median of all
172 /// nodes' center projection onto the axis and using it as the split
173 /// threshold.
174 size_t topdown_0(size_t* lbeg, size_t* lend);
175
176 /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a
177 /// topdown manner. During construction, first compute the best split
178 /// thresholds for different axes as the average of all nodes' center. Then
179 /// choose the split axis as the axis whose threshold can divide the nodes
180 /// into two parts with almost similar size. This construction is more
181 /// expensive then topdown_0, but also can provide tree with better quality.
182 size_t topdown_1(size_t* lbeg, size_t* lend);
183
184 /// @brief init tree from leaves in the topdown manner (topdown_0 or
185 /// topdown_1)
186 void init_0(Node* leaves, int n_leaves_);
187
188 /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
189 /// for nodes which is of depth more than the maximum bits of the morton code,
190 /// we use bottomup method to construct the subtree, which is slow but can
191 /// construct tree with high quality.
192 void init_1(Node* leaves, int n_leaves_);
193
194 /// @brief init tree from leaves using morton code. It uses morton_0, i.e.,
195 /// for nodes which is of depth more than the maximum bits of the morton code,
196 /// we split the leaves into two parts with the same size simply using the
197 /// node index.
198 void init_2(Node* leaves, int n_leaves_);
199
200 /// @brief init tree from leaves using morton code. It uses morton_2, i.e.,
201 /// for all nodes, we simply divide the leaves into parts with the same size
202 /// simply using the node index.
203 void init_3(Node* leaves, int n_leaves_);
204
205 size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const uint32_t& split,
206 int bits);
207
208 size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const uint32_t& split,
209 int bits);
210
211 size_t mortonRecurse_2(size_t* lbeg, size_t* lend);
212
213 /// @brief update one leaf node's bounding volume
214 void update_(size_t leaf, const BV& bv);
215
216 /// @brief Insert a leaf node and also update its ancestors
217 void insertLeaf(size_t root, size_t leaf);
218
219 /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the
220 /// unnecessary internal nodes are deleted. return the node with the smallest
221 /// depth and is influenced by the remove operation
222 size_t removeLeaf(size_t leaf);
223
224 /// @brief Delete all internal nodes and return all leaves nodes with given
225 /// depth from root
226 void fetchLeaves(size_t root, Node*& leaves, int depth = -1);
227
228 size_t indexOf(size_t node);
229
230 size_t allocateNode();
231
232 /// @brief create one node (leaf or internal)
233 size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data);
234
235 size_t createNode(size_t parent, const BV& bv, void* data);
236
237 size_t createNode(size_t parent, void* data);
238
239 void deleteNode(size_t node);
240
241 void recurseRefit(size_t node);
242
243 protected:
244 size_t root_node;
245 Node* nodes;
246 size_t n_nodes;
247 size_t n_nodes_alloc;
248
249 size_t n_leaves;
250 size_t freelist;
251 unsigned int opath;
252
253 int max_lookahead_level;
254
255 public:
256 /// @brief decide which topdown algorithm to use
257 int topdown_level;
258
259 /// @brief decide the depth to use expensive bottom-up algorithm
260 int bu_threshold;
261
262 public:
263 static const size_t NULL_NODE = std::numeric_limits<size_t>::max();
264 };
265
266 template <typename BV>
267 const size_t HierarchyTree<BV>::NULL_NODE;
268
269 /// @brief Functor comparing two nodes
270 template <typename BV>
271 struct nodeBaseLess {
272 nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_);
273
274 bool operator()(size_t i, size_t j) const;
275
276 private:
277 /// @brief the nodes array
278 const NodeBase<BV>* nodes;
279
280 /// @brief the dimension to compare
281 size_t d;
282 };
283
284 /// @brief select the node from node1 and node2 which is close to the query-th
285 /// node in the nodes. 0 for node1 and 1 for node2.
286 template <typename BV>
287 size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes);
288
289 /// @brief select the node from node1 and node2 which is close to the query
290 /// AABB. 0 for node1 and 1 for node2.
291 template <typename BV>
292 size_t select(const BV& query, size_t node1, size_t node2, NodeBase<BV>* nodes);
293
294 } // namespace implementation_array
295 } // namespace detail
296 } // namespace coal
297
298 #include "coal/broadphase/detail/hierarchy_tree_array-inl.h"
299
300 #endif
301