Directory: | ./ |
---|---|
File: | include/coal/broadphase/detail/hierarchy_tree_array.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 9 | 11 | 81.8% |
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 |