GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/detail/hierarchy_tree_array-inl.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 302 527 57.3%
Functions: 33 45 73.3%
Branches: 174 526 33.1%
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_INL_H
39 #define COAL_HIERARCHY_TREE_ARRAY_INL_H
40
41 #include "coal/broadphase/detail/hierarchy_tree_array.h"
42
43 #include <algorithm>
44 #include <iostream>
45
46 namespace coal {
47
48 namespace detail {
49
50 namespace implementation_array {
51
52 //==============================================================================
53 template <typename BV>
54 102 HierarchyTree<BV>::HierarchyTree(int bu_threshold_, int topdown_level_) {
55 102 root_node = NULL_NODE;
56 102 n_nodes = 0;
57 102 n_nodes_alloc = 16;
58
4/8
✓ Branch 0 taken 102 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 1632 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1632 times.
✓ Branch 8 taken 102 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 1734 nodes = new Node[n_nodes_alloc];
59
2/2
✓ Branch 0 taken 1530 times.
✓ Branch 1 taken 102 times.
 1632 for (size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
60 102 nodes[n_nodes_alloc - 1].next = NULL_NODE;
61 102 n_leaves = 0;
62 102 freelist = 0;
63 102 opath = 0;
64 102 max_lookahead_level = -1;
65 102 bu_threshold = bu_threshold_;
66 102 topdown_level = topdown_level_;
67 102 }
68
69 //==============================================================================
70 template <typename BV>
71 100 HierarchyTree<BV>::~HierarchyTree() {
72
1/2
✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
 100 delete[] nodes;
73 100 }
74
75 //==============================================================================
76 template <typename BV>
77 86 void HierarchyTree<BV>::init(Node* leaves, int n_leaves_, int level) {
78
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) {
79 43 case 0:
80 43 init_0(leaves, n_leaves_);
81 43 break;
82 ✗ case 1:
83 ✗ init_1(leaves, n_leaves_);
84 ✗ break;
85 43 case 2:
86 43 init_2(leaves, n_leaves_);
87 43 break;
88 ✗ case 3:
89 ✗ init_3(leaves, n_leaves_);
90 ✗ break;
91 ✗ default:
92 ✗ init_0(leaves, n_leaves_);
93 }
94 86 }
95
96 //==============================================================================
97 template <typename BV>
98 43 void HierarchyTree<BV>::init_0(Node* leaves, int n_leaves_) {
99 43 clear();
100
101 43 n_leaves = (size_t)n_leaves_;
102 43 root_node = NULL_NODE;
103
4/8
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 25342 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 25342 times.
✓ Branch 8 taken 43 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 25385 nodes = new Node[n_leaves * 2];
104 43 std::copy(leaves, leaves + n_leaves, nodes);
105 43 freelist = n_leaves;
106 43 n_nodes = n_leaves;
107 43 n_nodes_alloc = 2 * n_leaves;
108
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 43 times.
 12714 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
109 43 nodes[n_nodes_alloc - 1].next = NULL_NODE;
110
111
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 size_t* ids = new size_t[n_leaves];
112
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 43 times.
 12714 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
113
114 43 root_node = topdown(ids, ids + n_leaves);
115
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 delete[] ids;
116
117 43 opath = 0;
118 43 max_lookahead_level = -1;
119 43 }
120
121 //==============================================================================
122 template <typename BV>
123 ✗ void HierarchyTree<BV>::init_1(Node* leaves, int n_leaves_) {
124 ✗ clear();
125
126 ✗ n_leaves = (size_t)n_leaves_;
127 ✗ root_node = NULL_NODE;
128 ✗ nodes = new Node[n_leaves * 2];
129 ✗ std::copy(leaves, leaves + n_leaves, nodes);
130 ✗ freelist = n_leaves;
131 ✗ n_nodes = n_leaves;
132 ✗ n_nodes_alloc = 2 * n_leaves;
133 ✗ for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
134 ✗ nodes[n_nodes_alloc - 1].next = NULL_NODE;
135
136 ✗ BV bound_bv;
137 ✗ if (n_leaves > 0) bound_bv = nodes[0].bv;
138 ✗ for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
139
140 ✗ morton_functor<Scalar, uint32_t> coder(bound_bv);
141 ✗ for (size_t i = 0; i < n_leaves; ++i)
142 ✗ nodes[i].code = coder(nodes[i].bv.center());
143
144 ✗ size_t* ids = new size_t[n_leaves];
145 ✗ for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
146
147 ✗ const SortByMorton comp{nodes};
148 ✗ std::sort(ids, ids + n_leaves, comp);
149 ✗ root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
150 ✗ coder.bits() - 1);
151 ✗ delete[] ids;
152
153 ✗ refit();
154
155 ✗ opath = 0;
156 ✗ max_lookahead_level = -1;
157 ✗ }
158
159 //==============================================================================
160 template <typename BV>
161 43 void HierarchyTree<BV>::init_2(Node* leaves, int n_leaves_) {
162
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 clear();
163
164 43 n_leaves = (size_t)n_leaves_;
165 43 root_node = NULL_NODE;
166
5/10
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25342 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 25342 times.
✓ Branch 10 taken 43 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 25385 nodes = new Node[n_leaves * 2];
167
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 std::copy(leaves, leaves + n_leaves, nodes);
168 43 freelist = n_leaves;
169 43 n_nodes = n_leaves;
170 43 n_nodes_alloc = 2 * n_leaves;
171
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 43 times.
 12714 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
172 43 nodes[n_nodes_alloc - 1].next = NULL_NODE;
173
174
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 BV bound_bv;
175
2/4
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
 43 if (n_leaves > 0) bound_bv = nodes[0].bv;
176
3/4
✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12628 times.
✓ Branch 4 taken 43 times.
 12671 for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
177
178
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 morton_functor<Scalar, uint32_t> coder(bound_bv);
179
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 43 times.
 12714 for (size_t i = 0; i < n_leaves; ++i)
180
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
 12671 nodes[i].code = coder(nodes[i].bv.center());
181
182
2/4
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
 43 size_t* ids = new size_t[n_leaves];
183
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 43 times.
 12714 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
184
185 43 const SortByMorton comp{nodes};
186
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 std::sort(ids, ids + n_leaves, comp);
187
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits() - 1)),
188 43 coder.bits() - 1);
189
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 delete[] ids;
190
191
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 refit();
192
193 43 opath = 0;
194 43 max_lookahead_level = -1;
195 43 }
196
197 //==============================================================================
198 template <typename BV>
199 ✗ void HierarchyTree<BV>::init_3(Node* leaves, int n_leaves_) {
200 ✗ clear();
201
202 ✗ n_leaves = (size_t)n_leaves_;
203 ✗ root_node = NULL_NODE;
204 ✗ nodes = new Node[n_leaves * 2];
205 ✗ std::copy(leaves, leaves + n_leaves, nodes);
206 ✗ freelist = n_leaves;
207 ✗ n_nodes = n_leaves;
208 ✗ n_nodes_alloc = 2 * n_leaves;
209 ✗ for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
210 ✗ nodes[n_nodes_alloc - 1].next = NULL_NODE;
211
212 ✗ BV bound_bv;
213 ✗ if (n_leaves > 0) bound_bv = nodes[0].bv;
214 ✗ for (size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv;
215
216 ✗ morton_functor<Scalar, uint32_t> coder(bound_bv);
217 ✗ for (size_t i = 0; i < n_leaves; ++i)
218 ✗ nodes[i].code = coder(nodes[i].bv.center());
219
220 ✗ size_t* ids = new size_t[n_leaves];
221 ✗ for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
222
223 ✗ const SortByMorton comp{nodes};
224 ✗ std::sort(ids, ids + n_leaves, comp);
225 ✗ root_node = mortonRecurse_2(ids, ids + n_leaves);
226 ✗ delete[] ids;
227
228 ✗ refit();
229
230 ✗ opath = 0;
231 ✗ max_lookahead_level = -1;
232 ✗ }
233
234 //==============================================================================
235 template <typename BV>
236 ✗ size_t HierarchyTree<BV>::insert(const BV& bv, void* data) {
237 ✗ size_t node = createNode(NULL_NODE, bv, data);
238 ✗ insertLeaf(root_node, node);
239 ✗ ++n_leaves;
240 ✗ return node;
241 }
242
243 //==============================================================================
244 template <typename BV>
245 ✗ void HierarchyTree<BV>::remove(size_t leaf) {
246 ✗ removeLeaf(leaf);
247 ✗ deleteNode(leaf);
248 ✗ --n_leaves;
249 ✗ }
250
251 //==============================================================================
252 template <typename BV>
253 86 void HierarchyTree<BV>::clear() {
254
1/2
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
 86 delete[] nodes;
255 86 root_node = NULL_NODE;
256 86 n_nodes = 0;
257 86 n_nodes_alloc = 16;
258
4/8
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 1376 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1376 times.
✓ Branch 8 taken 86 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 1462 nodes = new Node[n_nodes_alloc];
259
2/2
✓ Branch 0 taken 1376 times.
✓ Branch 1 taken 86 times.
 1462 for (size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
260 86 nodes[n_nodes_alloc - 1].next = NULL_NODE;
261 86 n_leaves = 0;
262 86 freelist = 0;
263 86 opath = 0;
264 86 max_lookahead_level = -1;
265 86 }
266
267 //==============================================================================
268 template <typename BV>
269 ✗ bool HierarchyTree<BV>::empty() const {
270 ✗ return (n_nodes == 0);
271 }
272
273 //==============================================================================
274 template <typename BV>
275 260 void HierarchyTree<BV>::update(size_t leaf, int lookahead_level) {
276 260 size_t root = removeLeaf(leaf);
277
1/2
✓ Branch 0 taken 260 times.
✗ Branch 1 not taken.
 260 if (root != NULL_NODE) {
278
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 260 times.
 260 if (lookahead_level > 0) {
279 ✗ for (int i = 0;
280 ✗ (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
281 ✗ root = nodes[root].parent;
282 } else
283 260 root = root_node;
284 }
285 260 insertLeaf(root, leaf);
286 260 }
287
288 //==============================================================================
289 template <typename BV>
290 ✗ bool HierarchyTree<BV>::update(size_t leaf, const BV& bv) {
291 ✗ if (nodes[leaf].bv.contain(bv)) return false;
292 ✗ update_(leaf, bv);
293 ✗ return true;
294 }
295
296 //==============================================================================
297 template <typename BV>
298 bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel,
299 Scalar margin) {
300 COAL_UNUSED_VARIABLE(bv);
301 COAL_UNUSED_VARIABLE(vel);
302 COAL_UNUSED_VARIABLE(margin);
303
304 if (nodes[leaf].bv.contain(bv)) return false;
305 update_(leaf, bv);
306 return true;
307 }
308
309 //==============================================================================
310 template <typename BV>
311 bool HierarchyTree<BV>::update(size_t leaf, const BV& bv, const Vec3s& vel) {
312 COAL_UNUSED_VARIABLE(vel);
313
314 if (nodes[leaf].bv.contain(bv)) return false;
315 update_(leaf, bv);
316 return true;
317 }
318
319 //==============================================================================
320 template <typename BV>
321 26 size_t HierarchyTree<BV>::getMaxHeight() const {
322
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
 26 if (root_node == NULL_NODE) return 0;
323
324 26 return getMaxHeight(root_node);
325 }
326
327 //==============================================================================
328 template <typename BV>
329 size_t HierarchyTree<BV>::getMaxDepth() const {
330 if (root_node == NULL_NODE) return 0;
331
332 size_t max_depth;
333 getMaxDepth(root_node, 0, max_depth);
334 return max_depth;
335 }
336
337 //==============================================================================
338 template <typename BV>
339 void HierarchyTree<BV>::balanceBottomup() {
340 if (root_node != NULL_NODE) {
341 Node* leaves = new Node[n_leaves];
342 Node* leaves_ = leaves;
343 extractLeaves(root_node, leaves_);
344 root_node = NULL_NODE;
345 std::copy(leaves, leaves + n_leaves, nodes);
346 freelist = n_leaves;
347 n_nodes = n_leaves;
348 for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
349 nodes[n_nodes_alloc - 1].next = NULL_NODE;
350
351 size_t* ids = new size_t[n_leaves];
352 for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
353
354 bottomup(ids, ids + n_leaves);
355 root_node = *ids;
356
357 delete[] ids;
358 }
359 }
360
361 //==============================================================================
362 template <typename BV>
363 ✗ void HierarchyTree<BV>::balanceTopdown() {
364 ✗ if (root_node != NULL_NODE) {
365 ✗ Node* leaves = new Node[n_leaves];
366 ✗ Node* leaves_ = leaves;
367 ✗ extractLeaves(root_node, leaves_);
368 ✗ root_node = NULL_NODE;
369 ✗ std::copy(leaves, leaves + n_leaves, nodes);
370 ✗ freelist = n_leaves;
371 ✗ n_nodes = n_leaves;
372 ✗ for (size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1;
373 ✗ nodes[n_nodes_alloc - 1].next = NULL_NODE;
374
375 ✗ size_t* ids = new size_t[n_leaves];
376 ✗ for (size_t i = 0; i < n_leaves; ++i) ids[i] = i;
377
378 ✗ root_node = topdown(ids, ids + n_leaves);
379 ✗ delete[] ids;
380 }
381 ✗ }
382
383 //==============================================================================
384 template <typename BV>
385 26 void HierarchyTree<BV>::balanceIncremental(int iterations) {
386
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
 26 if (iterations < 0) iterations = (int)n_leaves;
387
2/4
✓ Branch 0 taken 26 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 26 if ((root_node != NULL_NODE) && (iterations > 0)) {
388
2/2
✓ Branch 0 taken 260 times.
✓ Branch 1 taken 26 times.
 286 for (int i = 0; i < iterations; ++i) {
389 260 size_t node = root_node;
390 260 unsigned int bit = 0;
391
2/2
✓ Branch 1 taken 1272 times.
✓ Branch 2 taken 260 times.
 1532 while (!nodes[node].isLeaf()) {
392 1272 node = nodes[node].children[(opath >> bit) & 1];
393 1272 bit = (bit + 1) & (sizeof(unsigned int) * 8 - 1);
394 }
395 260 update(node);
396 260 ++opath;
397 }
398 }
399 26 }
400
401 //==============================================================================
402 template <typename BV>
403 69 void HierarchyTree<BV>::refit() {
404
1/2
✓ Branch 0 taken 69 times.
✗ Branch 1 not taken.
 69 if (root_node != NULL_NODE) recurseRefit(root_node);
405 69 }
406
407 //==============================================================================
408 template <typename BV>
409 ✗ void HierarchyTree<BV>::extractLeaves(size_t root, Node*& leaves) const {
410 ✗ if (!nodes[root].isLeaf()) {
411 ✗ extractLeaves(nodes[root].children[0], leaves);
412 ✗ extractLeaves(nodes[root].children[1], leaves);
413 } else {
414 ✗ *leaves = nodes[root];
415 ✗ leaves++;
416 }
417 ✗ }
418
419 //==============================================================================
420 template <typename BV>
421 8018 size_t HierarchyTree<BV>::size() const {
422 8018 return n_leaves;
423 }
424
425 //==============================================================================
426 template <typename BV>
427 7754 size_t HierarchyTree<BV>::getRoot() const {
428 7754 return root_node;
429 }
430
431 //==============================================================================
432 template <typename BV>
433 10442 typename HierarchyTree<BV>::Node* HierarchyTree<BV>::getNodes() const {
434 10442 return nodes;
435 }
436
437 //==============================================================================
438 template <typename BV>
439 void HierarchyTree<BV>::print(size_t root, int depth) {
440 for (int i = 0; i < depth; ++i) std::cout << " ";
441 Node* n = nodes + root;
442 std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", "
443 << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1]
444 << ", " << n->bv.max_[2] << ")" << std::endl;
445 if (n->isLeaf()) {
446 } else {
447 print(n->children[0], depth + 1);
448 print(n->children[1], depth + 1);
449 }
450 }
451
452 //==============================================================================
453 template <typename BV>
454 5350 size_t HierarchyTree<BV>::getMaxHeight(size_t node) const {
455
2/2
✓ Branch 1 taken 2662 times.
✓ Branch 2 taken 2688 times.
 5350 if (!nodes[node].isLeaf()) {
456
1/2
✓ Branch 1 taken 2662 times.
✗ Branch 2 not taken.
 2662 size_t height1 = getMaxHeight(nodes[node].children[0]);
457
1/2
✓ Branch 1 taken 2662 times.
✗ Branch 2 not taken.
 2662 size_t height2 = getMaxHeight(nodes[node].children[1]);
458 2662 return std::max(height1, height2) + 1;
459 } else
460 2688 return 0;
461 }
462
463 //==============================================================================
464 template <typename BV>
465 void HierarchyTree<BV>::getMaxDepth(size_t node, size_t depth,
466 size_t& max_depth) const {
467 if (!nodes[node].isLeaf()) {
468 getMaxDepth(nodes[node].children[0], depth + 1, max_depth);
469 getmaxDepth(nodes[node].children[1], depth + 1, max_depth);
470 } else
471 max_depth = std::max(max_depth, depth);
472 }
473
474 //==============================================================================
475 template <typename BV>
476 4837 void HierarchyTree<BV>::bottomup(size_t* lbeg, size_t* lend) {
477 4837 size_t* lcur_end = lend;
478
2/2
✓ Branch 0 taken 4837 times.
✓ Branch 1 taken 4837 times.
 9674 while (lbeg < lcur_end - 1) {
479 4837 size_t *min_it1 = nullptr, *min_it2 = nullptr;
480 4837 Scalar min_size = (std::numeric_limits<Scalar>::max)();
481
2/2
✓ Branch 0 taken 9674 times.
✓ Branch 1 taken 4837 times.
 14511 for (size_t* it1 = lbeg; it1 < lcur_end; ++it1) {
482
2/2
✓ Branch 0 taken 4837 times.
✓ Branch 1 taken 9674 times.
 14511 for (size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) {
483
2/4
✓ Branch 1 taken 4837 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4837 times.
✗ Branch 5 not taken.
 4837 Scalar cur_size = (nodes[*it1].bv + nodes[*it2].bv).size();
484
1/2
✓ Branch 0 taken 4837 times.
✗ Branch 1 not taken.
 4837 if (cur_size < min_size) {
485 4837 min_size = cur_size;
486 4837 min_it1 = it1;
487 4837 min_it2 = it2;
488 }
489 }
490 }
491
492 size_t p =
493 4837 createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, nullptr);
494 4837 nodes[p].children[0] = *min_it1;
495 4837 nodes[p].children[1] = *min_it2;
496 4837 nodes[*min_it1].parent = p;
497 4837 nodes[*min_it2].parent = p;
498 4837 *min_it1 = p;
499 4837 size_t tmp = *min_it2;
500 4837 lcur_end--;
501 4837 *min_it2 = *lcur_end;
502 4837 *lcur_end = tmp;
503 }
504 4837 }
505
506 //==============================================================================
507 template <typename BV>
508 43 size_t HierarchyTree<BV>::topdown(size_t* lbeg, size_t* lend) {
509
1/3
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 43 switch (topdown_level) {
510 43 case 0:
511 43 return topdown_0(lbeg, lend);
512 break;
513 ✗ case 1:
514 ✗ return topdown_1(lbeg, lend);
515 break;
516 ✗ default:
517 ✗ return topdown_0(lbeg, lend);
518 }
519 }
520
521 //==============================================================================
522 template <typename BV>
523 15625 size_t HierarchyTree<BV>::topdown_0(size_t* lbeg, size_t* lend) {
524 15625 long num_leaves = lend - lbeg;
525
2/2
✓ Branch 0 taken 12628 times.
✓ Branch 1 taken 2997 times.
 15625 if (num_leaves > 1) {
526
2/2
✓ Branch 0 taken 7791 times.
✓ Branch 1 taken 4837 times.
 12628 if (num_leaves > bu_threshold) {
527
1/2
✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
 7791 BV vol = nodes[*lbeg].bv;
528
3/4
✓ Branch 1 taken 110608 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 110608 times.
✓ Branch 4 taken 7791 times.
 118399 for (size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv;
529
530 7791 size_t best_axis = 0;
531
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()};
532
2/2
✓ Branch 0 taken 3946 times.
✓ Branch 1 taken 3845 times.
 7791 if (extent[1] > extent[0]) best_axis = 1;
533
2/2
✓ Branch 0 taken 3760 times.
✓ Branch 1 taken 4031 times.
 7791 if (extent[2] > extent[best_axis]) best_axis = 2;
534
535 7791 nodeBaseLess<BV> comp(nodes, best_axis);
536 7791 size_t* lcenter = lbeg + num_leaves / 2;
537
1/2
✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
 7791 std::nth_element(lbeg, lcenter, lend, comp);
538
539
1/2
✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
 7791 size_t node = createNode(NULL_NODE, vol, nullptr);
540
1/2
✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
 7791 nodes[node].children[0] = topdown_0(lbeg, lcenter);
541
1/2
✓ Branch 1 taken 7791 times.
✗ Branch 2 not taken.
 7791 nodes[node].children[1] = topdown_0(lcenter, lend);
542 7791 nodes[nodes[node].children[0]].parent = node;
543 7791 nodes[nodes[node].children[1]].parent = node;
544 7791 return node;
545 } else {
546 4837 bottomup(lbeg, lend);
547 4837 return *lbeg;
548 }
549 }
550 2997 return *lbeg;
551 }
552
553 //==============================================================================
554 template <typename BV>
555 ✗ size_t HierarchyTree<BV>::topdown_1(size_t* lbeg, size_t* lend) {
556 ✗ long num_leaves = lend - lbeg;
557 ✗ if (num_leaves > 1) {
558 ✗ if (num_leaves > bu_threshold) {
559 ✗ Vec3s split_p = nodes[*lbeg].bv.center();
560 ✗ BV vol = nodes[*lbeg].bv;
561 ✗ for (size_t* i = lbeg + 1; i < lend; ++i) {
562 ✗ split_p += nodes[*i].bv.center();
563 ✗ vol += nodes[*i].bv;
564 }
565 ✗ split_p /= static_cast<Scalar>(num_leaves);
566 ✗ int best_axis = -1;
567 ✗ int bestmidp = (int)num_leaves;
568 ✗ int splitcount[3][2] = {{0, 0}, {0, 0}, {0, 0}};
569 ✗ for (size_t* i = lbeg; i < lend; ++i) {
570 ✗ Vec3s x = nodes[*i].bv.center() - split_p;
571 ✗ for (int j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0];
572 }
573
574 ✗ for (size_t i = 0; i < 3; ++i) {
575 ✗ if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
576 ✗ int midp = std::abs(splitcount[i][0] - splitcount[i][1]);
577 ✗ if (midp < bestmidp) {
578 ✗ best_axis = (int)i;
579 ✗ bestmidp = midp;
580 }
581 }
582 }
583
584 ✗ if (best_axis < 0) best_axis = 0;
585
586 ✗ Scalar split_value = split_p[best_axis];
587 ✗ size_t* lcenter = lbeg;
588 ✗ for (size_t* i = lbeg; i < lend; ++i) {
589 ✗ if (nodes[*i].bv.center()[best_axis] < split_value) {
590 ✗ size_t temp = *i;
591 ✗ *i = *lcenter;
592 ✗ *lcenter = temp;
593 ✗ ++lcenter;
594 }
595 }
596
597 ✗ size_t node = createNode(NULL_NODE, vol, nullptr);
598 ✗ nodes[node].children[0] = topdown_1(lbeg, lcenter);
599 ✗ nodes[node].children[1] = topdown_1(lcenter, lend);
600 ✗ nodes[nodes[node].children[0]].parent = node;
601 ✗ nodes[nodes[node].children[1]].parent = node;
602 ✗ return node;
603 } else {
604 ✗ bottomup(lbeg, lend);
605 ✗ return *lbeg;
606 }
607 }
608 ✗ return *lbeg;
609 }
610
611 //==============================================================================
612 template <typename BV>
613 ✗ size_t HierarchyTree<BV>::mortonRecurse_0(size_t* lbeg, size_t* lend,
614 const uint32_t& split, int bits) {
615 ✗ long num_leaves = lend - lbeg;
616 ✗ if (num_leaves > 1) {
617 ✗ if (bits > 0) {
618 ✗ const SortByMorton comp{nodes, split};
619 ✗ size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
620
621 ✗ if (lcenter == lbeg) {
622 ✗ uint32_t split2 = split | (1 << (bits - 1));
623 ✗ return mortonRecurse_0(lbeg, lend, split2, bits - 1);
624 ✗ } else if (lcenter == lend) {
625 ✗ uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
626 ✗ return mortonRecurse_0(lbeg, lend, split1, bits - 1);
627 } else {
628 ✗ uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
629 ✗ uint32_t split2 = split | (1 << (bits - 1));
630
631 ✗ size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1);
632 ✗ size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1);
633 ✗ size_t node = createNode(NULL_NODE, nullptr);
634 ✗ nodes[node].children[0] = child1;
635 ✗ nodes[node].children[1] = child2;
636 ✗ nodes[child1].parent = node;
637 ✗ nodes[child2].parent = node;
638 ✗ return node;
639 }
640 } else {
641 ✗ size_t node = topdown(lbeg, lend);
642 ✗ return node;
643 }
644 } else
645 ✗ return *lbeg;
646 }
647
648 //==============================================================================
649 template <typename BV>
650 37559 size_t HierarchyTree<BV>::mortonRecurse_1(size_t* lbeg, size_t* lend,
651 const uint32_t& split, int bits) {
652 37559 long num_leaves = lend - lbeg;
653
2/2
✓ Branch 0 taken 24888 times.
✓ Branch 1 taken 12671 times.
 37559 if (num_leaves > 1) {
654
1/2
✓ Branch 0 taken 24888 times.
✗ Branch 1 not taken.
 24888 if (bits > 0) {
655 24888 const SortByMorton comp{nodes, split};
656
1/2
✓ Branch 1 taken 24888 times.
✗ Branch 2 not taken.
 24888 size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp);
657
658
2/2
✓ Branch 0 taken 5995 times.
✓ Branch 1 taken 18893 times.
 24888 if (lcenter == lbeg) {
659 5995 uint32_t split2 = split | (1 << (bits - 1));
660
1/2
✓ Branch 1 taken 5995 times.
✗ Branch 2 not taken.
 5995 return mortonRecurse_1(lbeg, lend, split2, bits - 1);
661
2/2
✓ Branch 0 taken 6265 times.
✓ Branch 1 taken 12628 times.
 18893 } else if (lcenter == lend) {
662 6265 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
663
1/2
✓ Branch 1 taken 6265 times.
✗ Branch 2 not taken.
 6265 return mortonRecurse_1(lbeg, lend, split1, bits - 1);
664 } else {
665 12628 uint32_t split1 = (split & (~(1 << bits))) | (1 << (bits - 1));
666 12628 uint32_t split2 = split | (1 << (bits - 1));
667
668
1/2
✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
 12628 size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1);
669
1/2
✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
 12628 size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1);
670
1/2
✓ Branch 1 taken 12628 times.
✗ Branch 2 not taken.
 12628 size_t node = createNode(NULL_NODE, nullptr);
671 12628 nodes[node].children[0] = child1;
672 12628 nodes[node].children[1] = child2;
673 12628 nodes[child1].parent = node;
674 12628 nodes[child2].parent = node;
675 12628 return node;
676 }
677 } else {
678 ✗ size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1);
679 ✗ size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1);
680 ✗ size_t node = createNode(NULL_NODE, nullptr);
681 ✗ nodes[node].children[0] = child1;
682 ✗ nodes[node].children[1] = child2;
683 ✗ nodes[child1].parent = node;
684 ✗ nodes[child2].parent = node;
685 ✗ return node;
686 }
687 } else
688 12671 return *lbeg;
689 }
690
691 //==============================================================================
692 template <typename BV>
693 ✗ size_t HierarchyTree<BV>::mortonRecurse_2(size_t* lbeg, size_t* lend) {
694 ✗ long num_leaves = lend - lbeg;
695 ✗ if (num_leaves > 1) {
696 ✗ size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2);
697 ✗ size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend);
698 ✗ size_t node = createNode(NULL_NODE, nullptr);
699 ✗ nodes[node].children[0] = child1;
700 ✗ nodes[node].children[1] = child2;
701 ✗ nodes[child1].parent = node;
702 ✗ nodes[child2].parent = node;
703 ✗ return node;
704 } else
705 ✗ return *lbeg;
706 }
707
708 //==============================================================================
709 template <typename BV>
710 260 void HierarchyTree<BV>::insertLeaf(size_t root, size_t leaf) {
711
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 260 times.
 260 if (root_node == NULL_NODE) {
712 ✗ root_node = leaf;
713 ✗ nodes[leaf].parent = NULL_NODE;
714 } else {
715
1/2
✓ Branch 1 taken 260 times.
✗ Branch 2 not taken.
 260 if (!nodes[root].isLeaf()) {
716 do {
717 2320 root = nodes[root].children[select(leaf, nodes[root].children[0],
718 1160 nodes[root].children[1], nodes)];
719
2/2
✓ Branch 1 taken 900 times.
✓ Branch 2 taken 260 times.
 1160 } while (!nodes[root].isLeaf());
720 }
721
722 260 size_t prev = nodes[root].parent;
723 260 size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, nullptr);
724
1/2
✓ Branch 0 taken 260 times.
✗ Branch 1 not taken.
 260 if (prev != NULL_NODE) {
725 260 nodes[prev].children[indexOf(root)] = node;
726 260 nodes[node].children[0] = root;
727 260 nodes[root].parent = node;
728 260 nodes[node].children[1] = leaf;
729 260 nodes[leaf].parent = node;
730 do {
731
2/2
✓ Branch 1 taken 625 times.
✓ Branch 2 taken 170 times.
 795 if (!nodes[prev].bv.contain(nodes[node].bv))
732
1/2
✓ Branch 1 taken 625 times.
✗ Branch 2 not taken.
 625 nodes[prev].bv = nodes[nodes[prev].children[0]].bv +
733
1/2
✓ Branch 1 taken 625 times.
✗ Branch 2 not taken.
 625 nodes[nodes[prev].children[1]].bv;
734 else
735 170 break;
736 625 node = prev;
737
2/2
✓ Branch 0 taken 535 times.
✓ Branch 1 taken 90 times.
 625 } while (NULL_NODE != (prev = nodes[node].parent));
738 } else {
739 ✗ nodes[node].children[0] = root;
740 ✗ nodes[root].parent = node;
741 ✗ nodes[node].children[1] = leaf;
742 ✗ nodes[leaf].parent = node;
743 ✗ root_node = node;
744 }
745 }
746 260 }
747
748 //==============================================================================
749 template <typename BV>
750 260 size_t HierarchyTree<BV>::removeLeaf(size_t leaf) {
751
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 260 times.
 260 if (leaf == root_node) {
752 ✗ root_node = NULL_NODE;
753 ✗ return NULL_NODE;
754 } else {
755 260 size_t parent = nodes[leaf].parent;
756 260 size_t prev = nodes[parent].parent;
757 260 size_t sibling = nodes[parent].children[1 - indexOf(leaf)];
758
759
1/2
✓ Branch 0 taken 260 times.
✗ Branch 1 not taken.
 260 if (prev != NULL_NODE) {
760 260 nodes[prev].children[indexOf(parent)] = sibling;
761 260 nodes[sibling].parent = prev;
762 260 deleteNode(parent);
763
2/2
✓ Branch 0 taken 640 times.
✓ Branch 1 taken 90 times.
 730 while (prev != NULL_NODE) {
764 640 BV new_bv = nodes[nodes[prev].children[0]].bv +
765
1/2
✓ Branch 1 taken 640 times.
✗ Branch 2 not taken.
 640 nodes[nodes[prev].children[1]].bv;
766
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 == nodes[prev].bv)) {
767
1/2
✓ Branch 1 taken 470 times.
✗ Branch 2 not taken.
 470 nodes[prev].bv = new_bv;
768 470 prev = nodes[prev].parent;
769 } else
770 170 break;
771 }
772
773
2/2
✓ Branch 0 taken 90 times.
✓ Branch 1 taken 170 times.
 260 return (prev != NULL_NODE) ? prev : root_node;
774 } else {
775 ✗ root_node = sibling;
776 ✗ nodes[sibling].parent = NULL_NODE;
777 ✗ deleteNode(parent);
778 ✗ return root_node;
779 }
780 }
781 }
782
783 //==============================================================================
784 template <typename BV>
785 ✗ void HierarchyTree<BV>::update_(size_t leaf, const BV& bv) {
786 ✗ size_t root = removeLeaf(leaf);
787 ✗ if (root != NULL_NODE) {
788 ✗ if (max_lookahead_level >= 0) {
789 ✗ for (int i = 0;
790 ✗ (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i)
791 ✗ root = nodes[root].parent;
792 }
793
794 ✗ nodes[leaf].bv = bv;
795 ✗ insertLeaf(root, leaf);
796 }
797 ✗ }
798
799 //==============================================================================
800 template <typename BV>
801 780 size_t HierarchyTree<BV>::indexOf(size_t node) {
802 780 return (nodes[nodes[node].parent].children[1] == node);
803 }
804
805 //==============================================================================
806 template <typename BV>
807 25516 size_t HierarchyTree<BV>::allocateNode() {
808
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 25516 times.
 25516 if (freelist == NULL_NODE) {
809 ✗ Node* old_nodes = nodes;
810 ✗ n_nodes_alloc *= 2;
811 ✗ nodes = new Node[n_nodes_alloc];
812 ✗ std::copy(old_nodes, old_nodes + n_nodes, nodes);
813 ✗ delete[] old_nodes;
814
815 ✗ for (size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1;
816 ✗ nodes[n_nodes_alloc - 1].next = NULL_NODE;
817 ✗ freelist = n_nodes;
818 }
819
820 25516 size_t node_id = freelist;
821 25516 freelist = nodes[node_id].next;
822 25516 nodes[node_id].parent = NULL_NODE;
823 25516 nodes[node_id].children[0] = NULL_NODE;
824 25516 nodes[node_id].children[1] = NULL_NODE;
825 25516 ++n_nodes;
826 25516 return node_id;
827 }
828
829 //==============================================================================
830 template <typename BV>
831 5097 size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv1,
832 const BV& bv2, void* data) {
833 5097 size_t node = allocateNode();
834 5097 nodes[node].parent = parent;
835 5097 nodes[node].data = data;
836
2/4
✓ Branch 1 taken 5097 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5097 times.
✗ Branch 5 not taken.
 5097 nodes[node].bv = bv1 + bv2;
837 5097 return node;
838 }
839
840 //==============================================================================
841 template <typename BV>
842 7791 size_t HierarchyTree<BV>::createNode(size_t parent, const BV& bv, void* data) {
843 7791 size_t node = allocateNode();
844 7791 nodes[node].parent = parent;
845 7791 nodes[node].data = data;
846 7791 nodes[node].bv = bv;
847 7791 return node;
848 }
849
850 //==============================================================================
851 template <typename BV>
852 12628 size_t HierarchyTree<BV>::createNode(size_t parent, void* data) {
853 12628 size_t node = allocateNode();
854 12628 nodes[node].parent = parent;
855 12628 nodes[node].data = data;
856 12628 return node;
857 }
858
859 //==============================================================================
860 template <typename BV>
861 260 void HierarchyTree<BV>::deleteNode(size_t node) {
862 260 nodes[node].next = freelist;
863 260 freelist = node;
864 260 --n_nodes;
865 260 }
866
867 //==============================================================================
868 template <typename BV>
869 30649 void HierarchyTree<BV>::recurseRefit(size_t node) {
870
2/2
✓ Branch 1 taken 15290 times.
✓ Branch 2 taken 15359 times.
 30649 if (!nodes[node].isLeaf()) {
871 15290 recurseRefit(nodes[node].children[0]);
872 15290 recurseRefit(nodes[node].children[1]);
873
1/2
✓ Branch 1 taken 15290 times.
✗ Branch 2 not taken.
 15290 nodes[node].bv =
874
1/2
✓ Branch 1 taken 15290 times.
✗ Branch 2 not taken.
 15290 nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv;
875 } else
876 15359 return;
877 }
878
879 //==============================================================================
880 template <typename BV>
881 void HierarchyTree<BV>::fetchLeaves(size_t root, Node*& leaves, int depth) {
882 if ((!nodes[root].isLeaf()) && depth) {
883 fetchLeaves(nodes[root].children[0], leaves, depth - 1);
884 fetchLeaves(nodes[root].children[1], leaves, depth - 1);
885 deleteNode(root);
886 } else {
887 *leaves = nodes[root];
888 leaves++;
889 }
890 }
891
892 //==============================================================================
893 template <typename BV>
894 7791 nodeBaseLess<BV>::nodeBaseLess(const NodeBase<BV>* nodes_, size_t d_)
895 7791 : nodes(nodes_), d(d_) {
896 // Do nothing
897 7791 }
898
899 //==============================================================================
900 template <typename BV>
901 325711 bool nodeBaseLess<BV>::operator()(size_t i, size_t j) const {
902
6/10
✓ Branch 1 taken 325711 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 325711 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 325711 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 325711 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 183775 times.
✓ Branch 13 taken 141936 times.
 325711 if (nodes[i].bv.center()[(int)d] < nodes[j].bv.center()[(int)d]) return true;
903
904 141936 return false;
905 }
906
907 //==============================================================================
908 template <typename S, typename BV>
909 struct SelectImpl {
910 static bool run(size_t query, size_t node1, size_t node2,
911 NodeBase<BV>* nodes) {
912 COAL_UNUSED_VARIABLE(query);
913 COAL_UNUSED_VARIABLE(node1);
914 COAL_UNUSED_VARIABLE(node2);
915 COAL_UNUSED_VARIABLE(nodes);
916
917 return 0;
918 }
919
920 static bool run(const BV& query, size_t node1, size_t node2,
921 NodeBase<BV>* nodes) {
922 COAL_UNUSED_VARIABLE(query);
923 COAL_UNUSED_VARIABLE(node1);
924 COAL_UNUSED_VARIABLE(node2);
925 COAL_UNUSED_VARIABLE(nodes);
926
927 return 0;
928 }
929 };
930
931 //==============================================================================
932 template <typename BV>
933 1160 size_t select(size_t query, size_t node1, size_t node2, NodeBase<BV>* nodes) {
934 1160 return SelectImpl<Scalar, BV>::run(query, node1, node2, nodes);
935 }
936
937 //==============================================================================
938 template <typename BV>
939 31554 size_t select(const BV& query, size_t node1, size_t node2,
940 NodeBase<BV>* nodes) {
941 31554 return SelectImpl<Scalar, BV>::run(query, node1, node2, nodes);
942 }
943
944 //==============================================================================
945 template <typename S>
946 struct SelectImpl<S, AABB> {
947 1160 static bool run(size_t query, size_t node1, size_t node2,
948 NodeBase<AABB>* nodes) {
949 1160 const AABB& bv = nodes[query].bv;
950 1160 const AABB& bv1 = nodes[node1].bv;
951 1160 const AABB& bv2 = nodes[node2].bv;
952
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_;
953
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_);
954
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_);
955
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]);
956
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]);
957 1160 return (d1 < d2) ? 0 : 1;
958 }
959
960 31554 static bool run(const AABB& query, size_t node1, size_t node2,
961 NodeBase<AABB>* nodes) {
962 31554 const AABB& bv = query;
963 31554 const AABB& bv1 = nodes[node1].bv;
964 31554 const AABB& bv2 = nodes[node2].bv;
965
2/4
✓ Branch 1 taken 31554 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31554 times.
✗ Branch 5 not taken.
 31554 Vec3s v = bv.min_ + bv.max_;
966
3/6
✓ Branch 1 taken 31554 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31554 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31554 times.
✗ Branch 8 not taken.
 31554 Vec3s v1 = v - (bv1.min_ + bv1.max_);
967
3/6
✓ Branch 1 taken 31554 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31554 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31554 times.
✗ Branch 8 not taken.
 31554 Vec3s v2 = v - (bv2.min_ + bv2.max_);
968
3/6
✓ Branch 1 taken 31554 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31554 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31554 times.
✗ Branch 8 not taken.
 31554 Scalar d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]);
969
3/6
✓ Branch 1 taken 31554 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31554 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31554 times.
✗ Branch 8 not taken.
 31554 Scalar d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]);
970 31554 return (d1 < d2) ? 0 : 1;
971 }
972 };
973
974 } // namespace implementation_array
975 } // namespace detail
976 } // namespace coal
977
978 #endif
979