Directory: | ./ |
---|---|
File: | include/coal/broadphase/detail/hierarchy_tree-inl.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 301 | 466 | 64.6% |
Branches: | 155 | 408 | 38.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_INL_H | ||
39 | #define COAL_HIERARCHY_TREE_INL_H | ||
40 | |||
41 | #include "coal/broadphase/detail/hierarchy_tree.h" | ||
42 | |||
43 | namespace coal { | ||
44 | |||
45 | namespace detail { | ||
46 | |||
47 | //============================================================================== | ||
48 | template <typename BV> | ||
49 | 104 | HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) { | |
50 | 104 | root_node = nullptr; | |
51 | 104 | n_leaves = 0; | |
52 | 104 | free_node = nullptr; | |
53 | 104 | max_lookahead_level = -1; | |
54 | 104 | opath = 0; | |
55 | 104 | bu_threshold = bu_threshold_; | |
56 | 104 | topdown_level = topdown_level_; | |
57 | 104 | } | |
58 | |||
59 | //============================================================================== | ||
60 | template <typename BV> | ||
61 | 102 | HierarchyTree<BV>::~HierarchyTree() { | |
62 | 102 | clear(); | |
63 | 102 | } | |
64 | |||
65 | //============================================================================== | ||
66 | template <typename BV> | ||
67 | 86 | void HierarchyTree<BV>::init(std::vector<Node*>& leaves, int level) { | |
68 |
2/5✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 43 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
86 | switch (level) { |
69 | 43 | case 0: | |
70 | 43 | init_0(leaves); | |
71 | 43 | break; | |
72 | ✗ | case 1: | |
73 | ✗ | init_1(leaves); | |
74 | ✗ | break; | |
75 | 43 | case 2: | |
76 | 43 | init_2(leaves); | |
77 | 43 | break; | |
78 | ✗ | case 3: | |
79 | ✗ | init_3(leaves); | |
80 | ✗ | break; | |
81 | ✗ | default: | |
82 | ✗ | init_0(leaves); | |
83 | } | ||
84 | 86 | } | |
85 | |||
86 | //============================================================================== | ||
87 | template <typename BV> | ||
88 | 4 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::insert(const BV& bv, | |
89 | void* data) { | ||
90 | 4 | Node* leaf = createNode(nullptr, bv, data); | |
91 | 4 | insertLeaf(root_node, leaf); | |
92 | 4 | ++n_leaves; | |
93 | 4 | return leaf; | |
94 | } | ||
95 | |||
96 | //============================================================================== | ||
97 | template <typename BV> | ||
98 | ✗ | void HierarchyTree<BV>::remove(Node* leaf) { | |
99 | ✗ | removeLeaf(leaf); | |
100 | ✗ | deleteNode(leaf); | |
101 | ✗ | --n_leaves; | |
102 | } | ||
103 | |||
104 | //============================================================================== | ||
105 | template <typename BV> | ||
106 | 188 | void HierarchyTree<BV>::clear() { | |
107 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 102 times.
|
188 | if (root_node) recurseDeleteNode(root_node); |
108 | 188 | n_leaves = 0; | |
109 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 102 times.
|
188 | delete free_node; |
110 | 188 | free_node = nullptr; | |
111 | 188 | max_lookahead_level = -1; | |
112 | 188 | opath = 0; | |
113 | 188 | } | |
114 | |||
115 | //============================================================================== | ||
116 | template <typename BV> | ||
117 | ✗ | bool HierarchyTree<BV>::empty() const { | |
118 | ✗ | return (nullptr == root_node); | |
119 | } | ||
120 | |||
121 | //============================================================================== | ||
122 | template <typename BV> | ||
123 | 350 | void HierarchyTree<BV>::update(Node* leaf, int lookahead_level) { | |
124 | // TODO(DamrongGuoy): Since we update a leaf node by removing and | ||
125 | // inserting the same leaf node, it is likely to change the structure of | ||
126 | // the tree even if no object's pose has changed. In the future, | ||
127 | // find a way to preserve the structure of the tree to solve this issue: | ||
128 | // https://github.com/flexible-collision-library/fcl/issues/368 | ||
129 | |||
130 | // First we remove the leaf node pointed by `leaf` variable from the tree. | ||
131 | // The `sub_root` variable is the root of the subtree containing nodes | ||
132 | // whose BVs were adjusted due to node removal. | ||
133 | 350 | typename HierarchyTree<BV>::Node* sub_root = removeLeaf(leaf); | |
134 |
1/2✓ Branch 0 taken 350 times.
✗ Branch 1 not taken.
|
350 | if (sub_root) { |
135 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 350 times.
|
350 | if (lookahead_level > 0) { |
136 | // For positive `lookahead_level`, we move the `sub_root` variable up. | ||
137 | ✗ | for (int i = 0; (i < lookahead_level) && sub_root->parent; ++i) | |
138 | ✗ | sub_root = sub_root->parent; | |
139 | } else | ||
140 | // By default, lookahead_level = -1, and we reset the `sub_root` variable | ||
141 | // to the root of the entire tree. | ||
142 | 350 | sub_root = root_node; | |
143 | } | ||
144 | // Then we insert the node pointed by `leaf` variable back into the | ||
145 | // subtree rooted at `sub_root` variable. | ||
146 | 350 | insertLeaf(sub_root, leaf); | |
147 | 350 | } | |
148 | |||
149 | //============================================================================== | ||
150 | template <typename BV> | ||
151 | ✗ | bool HierarchyTree<BV>::update(Node* leaf, const BV& bv) { | |
152 | ✗ | if (leaf->bv.contain(bv)) return false; | |
153 | ✗ | update_(leaf, bv); | |
154 | ✗ | return true; | |
155 | } | ||
156 | |||
157 | //============================================================================== | ||
158 | template <typename S, typename BV> | ||
159 | struct UpdateImpl { | ||
160 | static bool run(const HierarchyTree<BV>& tree, | ||
161 | typename HierarchyTree<BV>::Node* leaf, const BV& bv, | ||
162 | const Vec3s& /*vel*/, Scalar /*margin*/) { | ||
163 | if (leaf->bv.contain(bv)) return false; | ||
164 | tree.update_(leaf, bv); | ||
165 | return true; | ||
166 | } | ||
167 | |||
168 | static bool run(const HierarchyTree<BV>& tree, | ||
169 | typename HierarchyTree<BV>::Node* leaf, const BV& bv, | ||
170 | const Vec3s& /*vel*/) { | ||
171 | if (leaf->bv.contain(bv)) return false; | ||
172 | tree.update_(leaf, bv); | ||
173 | return true; | ||
174 | } | ||
175 | }; | ||
176 | |||
177 | //============================================================================== | ||
178 | template <typename BV> | ||
179 | bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel, | ||
180 | Scalar margin) { | ||
181 | return UpdateImpl<Scalar, BV>::run(*this, leaf, bv, vel, margin); | ||
182 | } | ||
183 | |||
184 | //============================================================================== | ||
185 | template <typename BV> | ||
186 | bool HierarchyTree<BV>::update(Node* leaf, const BV& bv, const Vec3s& vel) { | ||
187 | return UpdateImpl<Scalar, BV>::run(*this, leaf, bv, vel); | ||
188 | } | ||
189 | |||
190 | //============================================================================== | ||
191 | template <typename BV> | ||
192 | 35 | size_t HierarchyTree<BV>::getMaxHeight() const { | |
193 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 35 times.
|
35 | if (!root_node) return 0; |
194 | 35 | return getMaxHeight(root_node); | |
195 | } | ||
196 | |||
197 | //============================================================================== | ||
198 | template <typename BV> | ||
199 | size_t HierarchyTree<BV>::getMaxDepth() const { | ||
200 | if (!root_node) return 0; | ||
201 | |||
202 | size_t max_depth; | ||
203 | getMaxDepth(root_node, 0, max_depth); | ||
204 | return max_depth; | ||
205 | } | ||
206 | |||
207 | //============================================================================== | ||
208 | template <typename BV> | ||
209 | void HierarchyTree<BV>::balanceBottomup() { | ||
210 | if (root_node) { | ||
211 | std::vector<Node*> leaves; | ||
212 | leaves.reserve(n_leaves); | ||
213 | fetchLeaves(root_node, leaves); | ||
214 | bottomup(leaves.begin(), leaves.end()); | ||
215 | root_node = leaves[0]; | ||
216 | } | ||
217 | } | ||
218 | |||
219 | //============================================================================== | ||
220 | template <typename BV> | ||
221 | ✗ | void HierarchyTree<BV>::balanceTopdown() { | |
222 | ✗ | if (root_node) { | |
223 | ✗ | std::vector<Node*> leaves; | |
224 | ✗ | leaves.reserve(n_leaves); | |
225 | ✗ | fetchLeaves(root_node, leaves); | |
226 | ✗ | root_node = topdown(leaves.begin(), leaves.end()); | |
227 | } | ||
228 | } | ||
229 | |||
230 | //============================================================================== | ||
231 | template <typename BV> | ||
232 | 35 | void HierarchyTree<BV>::balanceIncremental(int iterations) { | |
233 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 35 times.
|
35 | if (iterations < 0) iterations = (int)n_leaves; |
234 |
2/4✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
|
35 | if (root_node && (iterations > 0)) { |
235 |
2/2✓ Branch 0 taken 350 times.
✓ Branch 1 taken 35 times.
|
385 | for (int i = 0; i < iterations; ++i) { |
236 | 350 | Node* node = root_node; | |
237 | 350 | unsigned int bit = 0; | |
238 |
2/2✓ Branch 1 taken 1362 times.
✓ Branch 2 taken 350 times.
|
1712 | while (!node->isLeaf()) { |
239 | 1362 | node = sort(node, root_node)->children[(opath >> bit) & 1]; | |
240 | 1362 | bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1); | |
241 | } | ||
242 | 350 | update(node); | |
243 | 350 | ++opath; | |
244 | } | ||
245 | } | ||
246 | 35 | } | |
247 | |||
248 | //============================================================================== | ||
249 | template <typename BV> | ||
250 | 77 | void HierarchyTree<BV>::refit() { | |
251 |
1/2✓ Branch 0 taken 77 times.
✗ Branch 1 not taken.
|
77 | if (root_node) recurseRefit(root_node); |
252 | 77 | } | |
253 | |||
254 | //============================================================================== | ||
255 | template <typename BV> | ||
256 | void HierarchyTree<BV>::extractLeaves(const Node* root, | ||
257 | std::vector<Node*>& leaves) const { | ||
258 | if (!root->isLeaf()) { | ||
259 | extractLeaves(root->children[0], leaves); | ||
260 | extractLeaves(root->children[1], leaves); | ||
261 | } else | ||
262 | leaves.push_back(root); | ||
263 | } | ||
264 | |||
265 | //============================================================================== | ||
266 | template <typename BV> | ||
267 | 8037 | size_t HierarchyTree<BV>::size() const { | |
268 | 8037 | return n_leaves; | |
269 | } | ||
270 | |||
271 | //============================================================================== | ||
272 | template <typename BV> | ||
273 | 7763 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getRoot() const { | |
274 | 7763 | return root_node; | |
275 | } | ||
276 | |||
277 | //============================================================================== | ||
278 | template <typename BV> | ||
279 | ✗ | typename HierarchyTree<BV>::Node*& HierarchyTree<BV>::getRoot() { | |
280 | ✗ | return root_node; | |
281 | } | ||
282 | |||
283 | //============================================================================== | ||
284 | template <typename BV> | ||
285 | void HierarchyTree<BV>::print(Node* root, int depth) { | ||
286 | for (int i = 0; i < depth; ++i) std::cout << " "; | ||
287 | std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " | ||
288 | << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " | ||
289 | << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; | ||
290 | if (root->isLeaf()) { | ||
291 | } else { | ||
292 | print(root->children[0], depth + 1); | ||
293 | print(root->children[1], depth + 1); | ||
294 | } | ||
295 | } | ||
296 | |||
297 | //============================================================================== | ||
298 | template <typename BV> | ||
299 | 4837 | void HierarchyTree<BV>::bottomup(const NodeVecIterator lbeg, | |
300 | const NodeVecIterator lend) { | ||
301 | 4837 | NodeVecIterator lcur_end = lend; | |
302 |
2/2✓ Branch 2 taken 4837 times.
✓ Branch 3 taken 4837 times.
|
9674 | while (lbeg < lcur_end - 1) { |
303 | 4837 | NodeVecIterator min_it1 = lbeg; | |
304 | 4837 | NodeVecIterator min_it2 = lbeg + 1; | |
305 | 4837 | Scalar min_size = (std::numeric_limits<Scalar>::max)(); | |
306 |
2/2✓ Branch 2 taken 9674 times.
✓ Branch 3 taken 4837 times.
|
14511 | for (NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { |
307 |
2/2✓ Branch 3 taken 4837 times.
✓ Branch 4 taken 9674 times.
|
14511 | for (NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { |
308 |
2/4✓ Branch 3 taken 4837 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4837 times.
✗ Branch 7 not taken.
|
4837 | Scalar cur_size = ((*it1)->bv + (*it2)->bv).size(); |
309 |
1/2✓ Branch 0 taken 4837 times.
✗ Branch 1 not taken.
|
4837 | if (cur_size < min_size) { |
310 | 4837 | min_size = cur_size; | |
311 | 4837 | min_it1 = it1; | |
312 | 4837 | min_it2 = it2; | |
313 | } | ||
314 | } | ||
315 | } | ||
316 | |||
317 | 4837 | Node* n[2] = {*min_it1, *min_it2}; | |
318 |
1/2✓ Branch 1 taken 4837 times.
✗ Branch 2 not taken.
|
4837 | Node* p = createNode(nullptr, n[0]->bv, n[1]->bv, nullptr); |
319 | 4837 | p->children[0] = n[0]; | |
320 | 4837 | p->children[1] = n[1]; | |
321 | 4837 | n[0]->parent = p; | |
322 | 4837 | n[1]->parent = p; | |
323 | 4837 | *min_it1 = p; | |
324 | 4837 | Node* tmp = *min_it2; | |
325 | 4837 | lcur_end--; | |
326 | 4837 | *min_it2 = *lcur_end; | |
327 | 4837 | *lcur_end = tmp; | |
328 | } | ||
329 | 4837 | } | |
330 | |||
331 | //============================================================================== | ||
332 | template <typename BV> | ||
333 | 43 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown( | |
334 | const NodeVecIterator lbeg, const NodeVecIterator lend) { | ||
335 |
1/3✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
43 | switch (topdown_level) { |
336 | 43 | case 0: | |
337 | 43 | return topdown_0(lbeg, lend); | |
338 | break; | ||
339 | ✗ | case 1: | |
340 | ✗ | return topdown_1(lbeg, lend); | |
341 | break; | ||
342 | ✗ | default: | |
343 | ✗ | return topdown_0(lbeg, lend); | |
344 | } | ||
345 | } | ||
346 | |||
347 | //============================================================================== | ||
348 | template <typename BV> | ||
349 | 5377 | size_t HierarchyTree<BV>::getMaxHeight(Node* node) const { | |
350 |
2/2✓ Branch 1 taken 2671 times.
✓ Branch 2 taken 2706 times.
|
5377 | if (!node->isLeaf()) { |
351 |
1/2✓ Branch 1 taken 2671 times.
✗ Branch 2 not taken.
|
2671 | size_t height1 = getMaxHeight(node->children[0]); |
352 |
1/2✓ Branch 1 taken 2671 times.
✗ Branch 2 not taken.
|
2671 | size_t height2 = getMaxHeight(node->children[1]); |
353 | 2671 | return std::max(height1, height2) + 1; | |
354 | } else | ||
355 | 2706 | return 0; | |
356 | } | ||
357 | |||
358 | //============================================================================== | ||
359 | template <typename BV> | ||
360 | void HierarchyTree<BV>::getMaxDepth(Node* node, size_t depth, | ||
361 | size_t& max_depth) const { | ||
362 | if (!node->isLeaf()) { | ||
363 | getMaxDepth(node->children[0], depth + 1, max_depth); | ||
364 | getMaxDepth(node->children[1], depth + 1, max_depth); | ||
365 | } else | ||
366 | max_depth = std::max(max_depth, depth); | ||
367 | } | ||
368 | |||
369 | //============================================================================== | ||
370 | template <typename BV> | ||
371 | 15625 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_0( | |
372 | const NodeVecIterator lbeg, const NodeVecIterator lend) { | ||
373 | 15625 | long num_leaves = lend - lbeg; | |
374 |
2/2✓ Branch 0 taken 12628 times.
✓ Branch 1 taken 2997 times.
|
15625 | if (num_leaves > 1) { |
375 |
2/2✓ Branch 0 taken 7791 times.
✓ Branch 1 taken 4837 times.
|
12628 | if (num_leaves > bu_threshold) { |
376 |
1/2✓ Branch 2 taken 7791 times.
✗ Branch 3 not taken.
|
7791 | BV vol = (*lbeg)->bv; |
377 |
3/4✓ Branch 3 taken 110608 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 110608 times.
✓ Branch 8 taken 7791 times.
|
118399 | for (NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; |
378 | |||
379 | 7791 | int best_axis = 0; | |
380 |
3/6✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7791 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7791 times.
✗ Branch 8 not taken.
|
7791 | Scalar extent[3] = {vol.width(), vol.height(), vol.depth()}; |
381 |
2/2✓ Branch 0 taken 3946 times.
✓ Branch 1 taken 3845 times.
|
7791 | if (extent[1] > extent[0]) best_axis = 1; |
382 |
2/2✓ Branch 0 taken 3760 times.
✓ Branch 1 taken 4031 times.
|
7791 | if (extent[2] > extent[best_axis]) best_axis = 2; |
383 | |||
384 | // compute median | ||
385 | 7791 | NodeVecIterator lcenter = lbeg + num_leaves / 2; | |
386 |
2/4✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7791 times.
✗ Branch 5 not taken.
|
7791 | std::nth_element(lbeg, lcenter, lend, |
387 | std::bind(&nodeBaseLess<BV>, std::placeholders::_1, | ||
388 | 7791 | std::placeholders::_2, std::ref(best_axis))); | |
389 | |||
390 |
1/2✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
|
7791 | Node* node = createNode(nullptr, vol, nullptr); |
391 |
1/2✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
|
7791 | node->children[0] = topdown_0(lbeg, lcenter); |
392 |
1/2✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
|
7791 | node->children[1] = topdown_0(lcenter, lend); |
393 | 7791 | node->children[0]->parent = node; | |
394 | 7791 | node->children[1]->parent = node; | |
395 | 7791 | return node; | |
396 | } else { | ||
397 | 4837 | bottomup(lbeg, lend); | |
398 | 4837 | return *lbeg; | |
399 | } | ||
400 | } | ||
401 | 2997 | return *lbeg; | |
402 | } | ||
403 | |||
404 | //============================================================================== | ||
405 | template <typename BV> | ||
406 | ✗ | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::topdown_1( | |
407 | const NodeVecIterator lbeg, const NodeVecIterator lend) { | ||
408 | ✗ | long num_leaves = lend - lbeg; | |
409 | ✗ | if (num_leaves > 1) { | |
410 | ✗ | if (num_leaves > bu_threshold) { | |
411 | ✗ | Vec3s split_p = (*lbeg)->bv.center(); | |
412 | ✗ | BV vol = (*lbeg)->bv; | |
413 | ✗ | NodeVecIterator it; | |
414 | ✗ | for (it = lbeg + 1; it < lend; ++it) { | |
415 | ✗ | split_p += (*it)->bv.center(); | |
416 | ✗ | vol += (*it)->bv; | |
417 | } | ||
418 | ✗ | split_p /= static_cast<Scalar>(num_leaves); | |
419 | ✗ | int best_axis = -1; | |
420 | ✗ | long bestmidp = num_leaves; | |
421 | ✗ | int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}}; | |
422 | ✗ | for (it = lbeg; it < lend; ++it) { | |
423 | ✗ | Vec3s x = (*it)->bv.center() - split_p; | |
424 | ✗ | for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; | |
425 | } | ||
426 | |||
427 | ✗ | for (int i = 0; i < 3; ++i) { | |
428 | ✗ | if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { | |
429 | ✗ | long midp = std::abs(splitcount[i][0] - splitcount[i][1]); | |
430 | ✗ | if (midp < bestmidp) { | |
431 | ✗ | best_axis = i; | |
432 | ✗ | bestmidp = midp; | |
433 | } | ||
434 | } | ||
435 | } | ||
436 | |||
437 | ✗ | if (best_axis < 0) best_axis = 0; | |
438 | |||
439 | ✗ | Scalar split_value = split_p[best_axis]; | |
440 | ✗ | NodeVecIterator lcenter = lbeg; | |
441 | ✗ | for (it = lbeg; it < lend; ++it) { | |
442 | ✗ | if ((*it)->bv.center()[best_axis] < split_value) { | |
443 | ✗ | Node* temp = *it; | |
444 | ✗ | *it = *lcenter; | |
445 | ✗ | *lcenter = temp; | |
446 | ✗ | ++lcenter; | |
447 | } | ||
448 | } | ||
449 | |||
450 | ✗ | Node* node = createNode(nullptr, vol, nullptr); | |
451 | ✗ | node->children[0] = topdown_1(lbeg, lcenter); | |
452 | ✗ | node->children[1] = topdown_1(lcenter, lend); | |
453 | ✗ | node->children[0]->parent = node; | |
454 | ✗ | node->children[1]->parent = node; | |
455 | ✗ | return node; | |
456 | } else { | ||
457 | ✗ | bottomup(lbeg, lend); | |
458 | ✗ | return *lbeg; | |
459 | } | ||
460 | } | ||
461 | ✗ | return *lbeg; | |
462 | } | ||
463 | |||
464 | //============================================================================== | ||
465 | template <typename BV> | ||
466 | 43 | void HierarchyTree<BV>::init_0(std::vector<Node*>& leaves) { | |
467 | 43 | clear(); | |
468 | 43 | root_node = topdown(leaves.begin(), leaves.end()); | |
469 | 43 | n_leaves = leaves.size(); | |
470 | 43 | max_lookahead_level = -1; | |
471 | 43 | opath = 0; | |
472 | 43 | } | |
473 | |||
474 | //============================================================================== | ||
475 | template <typename BV> | ||
476 | ✗ | void HierarchyTree<BV>::init_1(std::vector<Node*>& leaves) { | |
477 | ✗ | clear(); | |
478 | |||
479 | ✗ | BV bound_bv; | |
480 | ✗ | if (leaves.size() > 0) bound_bv = leaves[0]->bv; | |
481 | ✗ | for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; | |
482 | |||
483 | ✗ | morton_functor<Scalar, uint32_t> coder(bound_bv); | |
484 | ✗ | for (size_t i = 0; i < leaves.size(); ++i) | |
485 | ✗ | leaves[i]->code = coder(leaves[i]->bv.center()); | |
486 | |||
487 | ✗ | std::sort(leaves.begin(), leaves.end(), SortByMorton()); | |
488 | |||
489 | ✗ | root_node = mortonRecurse_0(leaves.begin(), leaves.end(), | |
490 | ✗ | (1 << (coder.bits() - 1)), coder.bits() - 1); | |
491 | |||
492 | ✗ | refit(); | |
493 | ✗ | n_leaves = leaves.size(); | |
494 | ✗ | max_lookahead_level = -1; | |
495 | ✗ | opath = 0; | |
496 | } | ||
497 | |||
498 | //============================================================================== | ||
499 | template <typename BV> | ||
500 | 43 | void HierarchyTree<BV>::init_2(std::vector<Node*>& leaves) { | |
501 |
1/2✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
|
43 | clear(); |
502 | |||
503 |
1/2✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
|
43 | BV bound_bv; |
504 |
2/4✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 43 times.
✗ Branch 6 not taken.
|
43 | if (leaves.size() > 0) bound_bv = leaves[0]->bv; |
505 |
3/4✓ Branch 2 taken 12628 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12628 times.
✓ Branch 6 taken 43 times.
|
12671 | for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; |
506 | |||
507 |
1/2✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
|
43 | morton_functor<Scalar, uint32_t> coder(bound_bv); |
508 |
2/2✓ Branch 1 taken 12671 times.
✓ Branch 2 taken 43 times.
|
12714 | for (size_t i = 0; i < leaves.size(); ++i) |
509 |
2/4✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12671 times.
✗ Branch 6 not taken.
|
12671 | leaves[i]->code = coder(leaves[i]->bv.center()); |
510 | |||
511 |
1/2✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
|
43 | std::sort(leaves.begin(), leaves.end(), SortByMorton()); |
512 | |||
513 |
1/2✓ Branch 2 taken 43 times.
✗ Branch 3 not taken.
|
43 | root_node = mortonRecurse_1(leaves.begin(), leaves.end(), |
514 | 43 | (1 << (coder.bits() - 1)), coder.bits() - 1); | |
515 | |||
516 |
1/2✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
|
43 | refit(); |
517 | 43 | n_leaves = leaves.size(); | |
518 | 43 | max_lookahead_level = -1; | |
519 | 43 | opath = 0; | |
520 | 43 | } | |
521 | |||
522 | //============================================================================== | ||
523 | template <typename BV> | ||
524 | ✗ | void HierarchyTree<BV>::init_3(std::vector<Node*>& leaves) { | |
525 | ✗ | clear(); | |
526 | |||
527 | ✗ | BV bound_bv; | |
528 | ✗ | if (leaves.size() > 0) bound_bv = leaves[0]->bv; | |
529 | ✗ | for (size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; | |
530 | |||
531 | ✗ | morton_functor<Scalar, uint32_t> coder(bound_bv); | |
532 | ✗ | for (size_t i = 0; i < leaves.size(); ++i) | |
533 | ✗ | leaves[i]->code = coder(leaves[i]->bv.center()); | |
534 | |||
535 | ✗ | std::sort(leaves.begin(), leaves.end(), SortByMorton()); | |
536 | |||
537 | ✗ | root_node = mortonRecurse_2(leaves.begin(), leaves.end()); | |
538 | |||
539 | ✗ | refit(); | |
540 | ✗ | n_leaves = leaves.size(); | |
541 | ✗ | max_lookahead_level = -1; | |
542 | ✗ | opath = 0; | |
543 | } | ||
544 | |||
545 | //============================================================================== | ||
546 | template <typename BV> | ||
547 | ✗ | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_0( | |
548 | const NodeVecIterator lbeg, const NodeVecIterator lend, | ||
549 | const uint32_t& split, int bits) { | ||
550 | ✗ | long num_leaves = lend - lbeg; | |
551 | ✗ | if (num_leaves > 1) { | |
552 | ✗ | if (bits > 0) { | |
553 | ✗ | Node dummy; | |
554 | ✗ | dummy.code = split; | |
555 | NodeVecIterator lcenter = | ||
556 | ✗ | std::lower_bound(lbeg, lend, &dummy, SortByMorton()); | |
557 | |||
558 | ✗ | if (lcenter == lbeg) { | |
559 | ✗ | uint32_t split2 = split | (1 << (bits - 1)); | |
560 | ✗ | return mortonRecurse_0(lbeg, lend, split2, bits - 1); | |
561 | ✗ | } else if (lcenter == lend) { | |
562 | ✗ | uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); | |
563 | ✗ | return mortonRecurse_0(lbeg, lend, split1, bits - 1); | |
564 | } else { | ||
565 | ✗ | uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); | |
566 | ✗ | uint32_t split2 = split | (1 << (bits - 1)); | |
567 | |||
568 | ✗ | Node* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); | |
569 | ✗ | Node* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); | |
570 | ✗ | Node* node = createNode(nullptr, nullptr); | |
571 | ✗ | node->children[0] = child1; | |
572 | ✗ | node->children[1] = child2; | |
573 | ✗ | child1->parent = node; | |
574 | ✗ | child2->parent = node; | |
575 | ✗ | return node; | |
576 | } | ||
577 | } else { | ||
578 | ✗ | Node* node = topdown(lbeg, lend); | |
579 | ✗ | return node; | |
580 | } | ||
581 | } else | ||
582 | ✗ | return *lbeg; | |
583 | } | ||
584 | |||
585 | //============================================================================== | ||
586 | template <typename BV> | ||
587 | 37559 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_1( | |
588 | const NodeVecIterator lbeg, const NodeVecIterator lend, | ||
589 | const uint32_t& split, int bits) { | ||
590 | 37559 | long num_leaves = lend - lbeg; | |
591 |
2/2✓ Branch 0 taken 24888 times.
✓ Branch 1 taken 12671 times.
|
37559 | if (num_leaves > 1) { |
592 |
1/2✓ Branch 0 taken 24888 times.
✗ Branch 1 not taken.
|
24888 | if (bits > 0) { |
593 |
1/2✓ Branch 1 taken 24888 times.
✗ Branch 2 not taken.
|
24888 | Node dummy; |
594 | 24888 | dummy.code = split; | |
595 | NodeVecIterator lcenter = | ||
596 |
1/2✓ Branch 1 taken 24888 times.
✗ Branch 2 not taken.
|
24888 | std::lower_bound(lbeg, lend, &dummy, SortByMorton()); |
597 | |||
598 |
2/2✓ Branch 1 taken 5995 times.
✓ Branch 2 taken 18893 times.
|
24888 | if (lcenter == lbeg) { |
599 | 5995 | uint32_t split2 = split | (1 << (bits - 1)); | |
600 |
1/2✓ Branch 1 taken 5995 times.
✗ Branch 2 not taken.
|
5995 | return mortonRecurse_1(lbeg, lend, split2, bits - 1); |
601 |
2/2✓ Branch 1 taken 6265 times.
✓ Branch 2 taken 12628 times.
|
18893 | } else if (lcenter == lend) { |
602 | 6265 | uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); | |
603 |
1/2✓ Branch 1 taken 6265 times.
✗ Branch 2 not taken.
|
6265 | return mortonRecurse_1(lbeg, lend, split1, bits - 1); |
604 | } else { | ||
605 | 12628 | uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); | |
606 | 12628 | uint32_t split2 = split | (1 << (bits - 1)); | |
607 | |||
608 |
1/2✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
|
12628 | Node* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); |
609 |
1/2✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
|
12628 | Node* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); |
610 |
1/2✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
|
12628 | Node* node = createNode(nullptr, nullptr); |
611 | 12628 | node->children[0] = child1; | |
612 | 12628 | node->children[1] = child2; | |
613 | 12628 | child1->parent = node; | |
614 | 12628 | child2->parent = node; | |
615 | 12628 | return node; | |
616 | } | ||
617 | } else { | ||
618 | ✗ | Node* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); | |
619 | ✗ | Node* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); | |
620 | ✗ | Node* node = createNode(nullptr, nullptr); | |
621 | ✗ | node->children[0] = child1; | |
622 | ✗ | node->children[1] = child2; | |
623 | ✗ | child1->parent = node; | |
624 | ✗ | child2->parent = node; | |
625 | ✗ | return node; | |
626 | } | ||
627 | } else | ||
628 | 12671 | return *lbeg; | |
629 | } | ||
630 | |||
631 | //============================================================================== | ||
632 | template <typename BV> | ||
633 | ✗ | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::mortonRecurse_2( | |
634 | const NodeVecIterator lbeg, const NodeVecIterator lend) { | ||
635 | ✗ | long num_leaves = lend - lbeg; | |
636 | ✗ | if (num_leaves > 1) { | |
637 | ✗ | Node* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); | |
638 | ✗ | Node* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); | |
639 | ✗ | Node* node = createNode(nullptr, nullptr); | |
640 | ✗ | node->children[0] = child1; | |
641 | ✗ | node->children[1] = child2; | |
642 | ✗ | child1->parent = node; | |
643 | ✗ | child2->parent = node; | |
644 | ✗ | return node; | |
645 | } else | ||
646 | ✗ | return *lbeg; | |
647 | } | ||
648 | |||
649 | //============================================================================== | ||
650 | template <typename BV> | ||
651 | ✗ | void HierarchyTree<BV>::update_(Node* leaf, const BV& bv) { | |
652 | ✗ | Node* root = removeLeaf(leaf); | |
653 | ✗ | if (root) { | |
654 | ✗ | if (max_lookahead_level >= 0) { | |
655 | ✗ | for (int i = 0; (i < max_lookahead_level) && root->parent; ++i) | |
656 | ✗ | root = root->parent; | |
657 | } else | ||
658 | ✗ | root = root_node; | |
659 | } | ||
660 | |||
661 | ✗ | leaf->bv = bv; | |
662 | ✗ | insertLeaf(root, leaf); | |
663 | } | ||
664 | |||
665 | //============================================================================== | ||
666 | template <typename BV> | ||
667 | 1362 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::sort(Node* n, Node*& r) { | |
668 | 1362 | Node* p = n->parent; | |
669 |
2/2✓ Branch 0 taken 399 times.
✓ Branch 1 taken 963 times.
|
1362 | if (p > n) { |
670 | 399 | size_t i = indexOf(n); | |
671 | 399 | size_t j = 1 - i; | |
672 | 399 | Node* s = p->children[j]; | |
673 | 399 | Node* q = p->parent; | |
674 |
2/2✓ Branch 0 taken 356 times.
✓ Branch 1 taken 43 times.
|
399 | if (q) |
675 | 356 | q->children[indexOf(p)] = n; | |
676 | else | ||
677 | 43 | r = n; | |
678 | 399 | s->parent = n; | |
679 | 399 | p->parent = n; | |
680 | 399 | n->parent = q; | |
681 | 399 | p->children[0] = n->children[0]; | |
682 | 399 | p->children[1] = n->children[1]; | |
683 | 399 | n->children[0]->parent = p; | |
684 | 399 | n->children[1]->parent = p; | |
685 | 399 | n->children[i] = p; | |
686 | 399 | n->children[j] = s; | |
687 | 399 | std::swap(p->bv, n->bv); | |
688 | 399 | return p; | |
689 | } | ||
690 | 963 | return n; | |
691 | } | ||
692 | |||
693 | //============================================================================== | ||
694 | template <typename BV> | ||
695 | 354 | void HierarchyTree<BV>::insertLeaf(Node* const sub_root, Node* const leaf) | |
696 | // Attempts to insert `leaf` into a subtree rooted at `sub_root`. | ||
697 | // 1. If the whole tree is empty, then `leaf` simply becomes the tree. | ||
698 | // 2. Otherwise, a leaf node called `sibling` of the subtree rooted at | ||
699 | // `sub_root` is selected (the selection criteria is a black box for this | ||
700 | // algorithm), and the `sibling` leaf is replaced by an internal node with | ||
701 | // two children, `sibling` and `leaf`. The bounding volumes are updated as | ||
702 | // necessary. | ||
703 | { | ||
704 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 352 times.
|
354 | if (!root_node) { |
705 | // If the entire tree is empty, the node pointed by `leaf` variable will | ||
706 | // become the root of the tree. | ||
707 | 2 | root_node = leaf; | |
708 | 2 | leaf->parent = nullptr; | |
709 | 2 | return; | |
710 | } | ||
711 | // Traverse the tree from the given `sub_root` down to an existing leaf | ||
712 | // node that we call `sibling`. The `sibling` node will eventually become | ||
713 | // the sibling of the given `leaf` node. | ||
714 | 352 | Node* sibling = sub_root; | |
715 |
2/2✓ Branch 1 taken 1160 times.
✓ Branch 2 taken 352 times.
|
1512 | while (!sibling->isLeaf()) { |
716 | 1160 | sibling = sibling->children[select(*leaf, *(sibling->children[0]), | |
717 | 1160 | *(sibling->children[1]))]; | |
718 | } | ||
719 | 352 | Node* prev = sibling->parent; | |
720 | // Create a new `node` that later will become the new parent of both the | ||
721 | // `sibling` and the given `leaf`. | ||
722 | 352 | Node* node = createNode(prev, leaf->bv, sibling->bv, nullptr); | |
723 |
2/2✓ Branch 0 taken 260 times.
✓ Branch 1 taken 92 times.
|
352 | if (prev) |
724 | // If the parent `prev` of the `sibling` is an interior node, we will | ||
725 | // replace the `sibling` with the subtree {node {`sibling`, leaf}} like | ||
726 | // this: | ||
727 | // Before After | ||
728 | // | ||
729 | // ╱ ╱ | ||
730 | // prev prev | ||
731 | // ╱ ╲ ─> ╱ ╲ | ||
732 | // sibling ... node ... | ||
733 | // ╱ ╲ | ||
734 | // sibling leaf | ||
735 | { | ||
736 | 260 | prev->children[indexOf(sibling)] = node; | |
737 | 260 | node->children[0] = sibling; | |
738 | 260 | sibling->parent = node; | |
739 | 260 | node->children[1] = leaf; | |
740 | 260 | leaf->parent = node; | |
741 | // Now that we've inserted `leaf` some of the existing bounding | ||
742 | // volumes might not fully enclose their children. Walk up the tree | ||
743 | // looking for parents that don't already enclose their children | ||
744 | // and create a new tight-fitting bounding volume for those. | ||
745 | do { | ||
746 |
2/2✓ Branch 1 taken 625 times.
✓ Branch 2 taken 170 times.
|
795 | if (!prev->bv.contain(node->bv)) |
747 |
1/2✓ Branch 2 taken 625 times.
✗ Branch 3 not taken.
|
625 | prev->bv = prev->children[0]->bv + prev->children[1]->bv; |
748 | else | ||
749 | 170 | break; | |
750 | 625 | node = prev; | |
751 |
2/2✓ Branch 0 taken 535 times.
✓ Branch 1 taken 90 times.
|
625 | } while (nullptr != (prev = node->parent)); |
752 | } else | ||
753 | // If the `sibling` has no parent, i.e., the tree is a singleton, | ||
754 | // we will replace it with the 3-node tree {node {`sibling`, `leaf`}} like | ||
755 | // this: | ||
756 | // | ||
757 | // node | ||
758 | // ╱ ╲ | ||
759 | // sibling leaf | ||
760 | { | ||
761 | 92 | node->children[0] = sibling; | |
762 | 92 | sibling->parent = node; | |
763 | 92 | node->children[1] = leaf; | |
764 | 92 | leaf->parent = node; | |
765 | 92 | root_node = node; | |
766 | } | ||
767 | |||
768 | // Note that the above algorithm always adds the new `leaf` node as the right | ||
769 | // child, i.e., children[1]. Calling removeLeaf(l) followed by calling | ||
770 | // this function insertLeaf(l) where l is a left child will result in | ||
771 | // switching l and its sibling even if no object's pose has changed. | ||
772 | } | ||
773 | |||
774 | //============================================================================== | ||
775 | template <typename BV> | ||
776 | 350 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::removeLeaf( | |
777 | Node* const leaf) { | ||
778 | // Deletes `leaf` by replacing the subtree consisting of `leaf`, its sibling, | ||
779 | // and its parent with just its sibling. It then "tightens" the ancestor | ||
780 | // bounding volumes. Returns a pointer to the parent of the highest node whose | ||
781 | // BV changed due to the removal. | ||
782 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 350 times.
|
350 | if (leaf == root_node) { |
783 | // If the `leaf` node is the only node in the tree, the tree becomes empty. | ||
784 | ✗ | root_node = nullptr; | |
785 | ✗ | return nullptr; | |
786 | } | ||
787 | 350 | Node* parent = leaf->parent; | |
788 | 350 | Node* prev = parent->parent; | |
789 | 350 | Node* sibling = parent->children[1 - indexOf(leaf)]; | |
790 |
2/2✓ Branch 0 taken 260 times.
✓ Branch 1 taken 90 times.
|
350 | if (prev) { |
791 | // If the parent node is not the root node, the sibling node will | ||
792 | // replace the parent node like this: | ||
793 | // | ||
794 | // Before After | ||
795 | // ... ... | ||
796 | // ╱ ╱ | ||
797 | // prev prev | ||
798 | // ╱ ╲ ╱ ╲ | ||
799 | // parent ... ─> sibling ... | ||
800 | // ╱ ╲ ╱╲ | ||
801 | // leaf sibling ... | ||
802 | // ╱╲ | ||
803 | // ... | ||
804 | // | ||
805 | // Step 1: replace the subtree {parent {leaf, sibling {...}}} with | ||
806 | // {sibling {...}}. | ||
807 | 260 | prev->children[indexOf(parent)] = sibling; | |
808 | 260 | sibling->parent = prev; | |
809 | 260 | deleteNode(parent); | |
810 | // Step 2: tighten up the BVs of the ancestor nodes. | ||
811 |
2/2✓ Branch 0 taken 640 times.
✓ Branch 1 taken 90 times.
|
730 | while (prev) { |
812 |
1/2✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
|
640 | BV new_bv = prev->children[0]->bv + prev->children[1]->bv; |
813 |
3/4✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 470 times.
✓ Branch 4 taken 170 times.
|
640 | if (!(new_bv == prev->bv)) { |
814 |
1/2✓ Branch 1 taken 470 times.
✗ Branch 2 not taken.
|
470 | prev->bv = new_bv; |
815 | 470 | prev = prev->parent; | |
816 | } else | ||
817 | 170 | break; | |
818 | } | ||
819 | |||
820 |
2/2✓ Branch 0 taken 90 times.
✓ Branch 1 taken 170 times.
|
260 | return prev ? prev : root_node; |
821 | } else { | ||
822 | // If the parent node is the root node, the sibling node will become the | ||
823 | // root of the whole tree like this: | ||
824 | // | ||
825 | // Before After | ||
826 | // | ||
827 | // parent | ||
828 | // ╱ ╲ | ||
829 | // leaf sibling ─> sibling | ||
830 | // ╱╲ ╱╲ | ||
831 | // ... ... | ||
832 | 90 | root_node = sibling; | |
833 | 90 | sibling->parent = nullptr; | |
834 | 90 | deleteNode(parent); | |
835 | 90 | return root_node; | |
836 | } | ||
837 | } | ||
838 | |||
839 | //============================================================================== | ||
840 | template <typename BV> | ||
841 | ✗ | void HierarchyTree<BV>::fetchLeaves(Node* root, std::vector<Node*>& leaves, | |
842 | int depth) { | ||
843 | ✗ | if ((!root->isLeaf()) && depth) { | |
844 | ✗ | fetchLeaves(root->children[0], leaves, depth - 1); | |
845 | ✗ | fetchLeaves(root->children[1], leaves, depth - 1); | |
846 | ✗ | deleteNode(root); | |
847 | } else { | ||
848 | ✗ | leaves.push_back(root); | |
849 | } | ||
850 | } | ||
851 | |||
852 | //============================================================================== | ||
853 | template <typename BV> | ||
854 | 1625 | size_t HierarchyTree<BV>::indexOf(Node* node) { | |
855 | // node cannot be nullptr | ||
856 | 1625 | return (node->parent->children[1] == node); | |
857 | } | ||
858 | |||
859 | //============================================================================== | ||
860 | template <typename BV> | ||
861 | 7795 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, | |
862 | const BV& bv, | ||
863 | void* data) { | ||
864 | 7795 | Node* node = createNode(parent, data); | |
865 | 7795 | node->bv = bv; | |
866 | 7795 | return node; | |
867 | } | ||
868 | |||
869 | //============================================================================== | ||
870 | template <typename BV> | ||
871 | 5189 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, | |
872 | const BV& bv1, | ||
873 | const BV& bv2, | ||
874 | void* data) { | ||
875 | 5189 | Node* node = createNode(parent, data); | |
876 |
1/2✓ Branch 2 taken 5189 times.
✗ Branch 3 not taken.
|
5189 | node->bv = bv1 + bv2; |
877 | 5189 | return node; | |
878 | } | ||
879 | |||
880 | //============================================================================== | ||
881 | template <typename BV> | ||
882 | 25612 | typename HierarchyTree<BV>::Node* HierarchyTree<BV>::createNode(Node* parent, | |
883 | void* data) { | ||
884 | 25612 | Node* node = nullptr; | |
885 |
2/2✓ Branch 0 taken 350 times.
✓ Branch 1 taken 25262 times.
|
25612 | if (free_node) { |
886 | 350 | node = free_node; | |
887 | 350 | free_node = nullptr; | |
888 | } else | ||
889 |
1/2✓ Branch 2 taken 25262 times.
✗ Branch 3 not taken.
|
25262 | node = new Node(); |
890 | 25612 | node->parent = parent; | |
891 | 25612 | node->data = data; | |
892 | 25612 | node->children[1] = 0; | |
893 | 25612 | return node; | |
894 | } | ||
895 | |||
896 | //============================================================================== | ||
897 | template <typename BV> | ||
898 | 49756 | void HierarchyTree<BV>::deleteNode(Node* node) { | |
899 |
1/2✓ Branch 0 taken 49756 times.
✗ Branch 1 not taken.
|
49756 | if (free_node != node) { |
900 |
2/2✓ Branch 0 taken 49320 times.
✓ Branch 1 taken 436 times.
|
49756 | delete free_node; |
901 | 49756 | free_node = node; | |
902 | } | ||
903 | 49756 | } | |
904 | |||
905 | //============================================================================== | ||
906 | template <typename BV> | ||
907 | 49406 | void HierarchyTree<BV>::recurseDeleteNode(Node* node) { | |
908 |
2/2✓ Branch 1 taken 24660 times.
✓ Branch 2 taken 24746 times.
|
49406 | if (!node->isLeaf()) { |
909 | 24660 | recurseDeleteNode(node->children[0]); | |
910 | 24660 | recurseDeleteNode(node->children[1]); | |
911 | } | ||
912 | |||
913 |
2/2✓ Branch 0 taken 86 times.
✓ Branch 1 taken 49320 times.
|
49406 | if (node == root_node) root_node = nullptr; |
914 | 49406 | deleteNode(node); | |
915 | 49406 | } | |
916 | |||
917 | //============================================================================== | ||
918 | template <typename BV> | ||
919 | 30673 | void HierarchyTree<BV>::recurseRefit(Node* node) { | |
920 |
2/2✓ Branch 1 taken 15298 times.
✓ Branch 2 taken 15375 times.
|
30673 | if (!node->isLeaf()) { |
921 | 15298 | recurseRefit(node->children[0]); | |
922 | 15298 | recurseRefit(node->children[1]); | |
923 |
1/2✓ Branch 2 taken 15298 times.
✗ Branch 3 not taken.
|
15298 | node->bv = node->children[0]->bv + node->children[1]->bv; |
924 | } else | ||
925 | 15375 | return; | |
926 | } | ||
927 | |||
928 | //============================================================================== | ||
929 | template <typename BV> | ||
930 | 325711 | bool nodeBaseLess(NodeBase<BV>* a, NodeBase<BV>* b, int d) { | |
931 |
5/8✓ Branch 2 taken 325711 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 325711 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 325711 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 183775 times.
✓ Branch 11 taken 141936 times.
|
325711 | if (a->bv.center()[d] < b->bv.center()[d]) return true; |
932 | 141936 | return false; | |
933 | } | ||
934 | |||
935 | //============================================================================== | ||
936 | template <typename S, typename BV> | ||
937 | struct SelectImpl { | ||
938 | static std::size_t run(const NodeBase<BV>& /*query*/, | ||
939 | const NodeBase<BV>& /*node1*/, | ||
940 | const NodeBase<BV>& /*node2*/) { | ||
941 | return 0; | ||
942 | } | ||
943 | |||
944 | static std::size_t run(const BV& /*query*/, const NodeBase<BV>& /*node1*/, | ||
945 | const NodeBase<BV>& /*node2*/) { | ||
946 | return 0; | ||
947 | } | ||
948 | }; | ||
949 | |||
950 | //============================================================================== | ||
951 | template <typename BV> | ||
952 | 1160 | size_t select(const NodeBase<BV>& query, const NodeBase<BV>& node1, | |
953 | const NodeBase<BV>& node2) { | ||
954 | 1160 | return SelectImpl<Scalar, BV>::run(query, node1, node2); | |
955 | } | ||
956 | |||
957 | //============================================================================== | ||
958 | template <typename BV> | ||
959 | 31555 | size_t select(const BV& query, const NodeBase<BV>& node1, | |
960 | const NodeBase<BV>& node2) { | ||
961 | 31555 | return SelectImpl<Scalar, BV>::run(query, node1, node2); | |
962 | } | ||
963 | |||
964 | //============================================================================== | ||
965 | template <typename S> | ||
966 | struct SelectImpl<S, AABB> { | ||
967 | 1160 | static std::size_t run(const NodeBase<AABB>& node, | |
968 | const NodeBase<AABB>& node1, | ||
969 | const NodeBase<AABB>& node2) { | ||
970 | 1160 | const AABB& bv = node.bv; | |
971 | 1160 | const AABB& bv1 = node1.bv; | |
972 | 1160 | const AABB& bv2 = node2.bv; | |
973 |
2/4✓ Branch 1 taken 1160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1160 times.
✗ Branch 5 not taken.
|
1160 | Vec3s v = bv.min_ + bv.max_; |
974 |
3/6✓ Branch 1 taken 1160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1160 times.
✗ Branch 8 not taken.
|
1160 | Vec3s v1 = v - (bv1.min_ + bv1.max_); |
975 |
3/6✓ Branch 1 taken 1160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1160 times.
✗ Branch 8 not taken.
|
1160 | Vec3s v2 = v - (bv2.min_ + bv2.max_); |
976 |
3/6✓ Branch 1 taken 1160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1160 times.
✗ Branch 8 not taken.
|
1160 | Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
977 |
3/6✓ Branch 1 taken 1160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1160 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1160 times.
✗ Branch 8 not taken.
|
1160 | Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
978 | 1160 | return (d1 < d2) ? 0 : 1; | |
979 | } | ||
980 | |||
981 | 31555 | static std::size_t run(const AABB& query, const NodeBase<AABB>& node1, | |
982 | const NodeBase<AABB>& node2) { | ||
983 | 31555 | const AABB& bv = query; | |
984 | 31555 | const AABB& bv1 = node1.bv; | |
985 | 31555 | const AABB& bv2 = node2.bv; | |
986 |
2/4✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31555 times.
✗ Branch 5 not taken.
|
31555 | Vec3s v = bv.min_ + bv.max_; |
987 |
3/6✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31555 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31555 times.
✗ Branch 8 not taken.
|
31555 | Vec3s v1 = v - (bv1.min_ + bv1.max_); |
988 |
3/6✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31555 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31555 times.
✗ Branch 8 not taken.
|
31555 | Vec3s v2 = v - (bv2.min_ + bv2.max_); |
989 |
3/6✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31555 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31555 times.
✗ Branch 8 not taken.
|
31555 | Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); |
990 |
3/6✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31555 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31555 times.
✗ Branch 8 not taken.
|
31555 | Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); |
991 | 31555 | return (d1 < d2) ? 0 : 1; | |
992 | } | ||
993 | }; | ||
994 | |||
995 | } // namespace detail | ||
996 | } // namespace coal | ||
997 | |||
998 | #endif | ||
999 |