GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/detail/hierarchy_tree-inl.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 301 473 63.6%
Functions: 33 45 73.3%
Branches: 159 418 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 411 times.
✓ Branch 1 taken 951 times.
 1362 if (p > n) {
670 411 size_t i = indexOf(n);
671 411 size_t j = 1 - i;
672 411 Node* s = p->children[j];
673 411 Node* q = p->parent;
674
2/2
✓ Branch 0 taken 360 times.
✓ Branch 1 taken 51 times.
 411 if (q)
675 360 q->children[indexOf(p)] = n;
676 else
677 51 r = n;
678 411 s->parent = n;
679 411 p->parent = n;
680 411 n->parent = q;
681 411 p->children[0] = n->children[0];
682 411 p->children[1] = n->children[1];
683 411 n->children[0]->parent = p;
684 411 n->children[1]->parent = p;
685 411 n->children[i] = p;
686 411 n->children[j] = s;
687 411 std::swap(p->bv, n->bv);
688 411 return p;
689 }
690 951 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
2/4
✓ Branch 1 taken 625 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 625 times.
✗ Branch 5 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 1641 size_t HierarchyTree<BV>::indexOf(Node* node) {
855 // node cannot be nullptr
856 1641 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
2/4
✓ Branch 1 taken 5189 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5189 times.
✗ Branch 5 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/4
✓ Branch 2 taken 25262 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 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
2/4
✓ Branch 1 taken 15298 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15298 times.
✗ Branch 5 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
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 (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