GCC Code Coverage Report

Directory: ./
File: src/traversal/traversal_recurse.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 168 168 100.0%
Branches: 148 228 64.9%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, 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 #include "coal/internal/traversal_recurse.h"
39
40 #include <vector>
41
42 namespace coal {
43 50104872 void collisionRecurse(CollisionTraversalNodeBase* node, unsigned int b1,
44 unsigned int b2, BVHFrontList* front_list,
45 Scalar& sqrDistLowerBound) {
46 50104872 Scalar sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
47
1/2
✓ Branch 1 taken 50104872 times.
✗ Branch 2 not taken.
 50104872 bool l1 = node->isFirstNodeLeaf(b1);
48
1/2
✓ Branch 1 taken 50104872 times.
✗ Branch 2 not taken.
 50104872 bool l2 = node->isSecondNodeLeaf(b2);
49
4/4
✓ Branch 0 taken 34961432 times.
✓ Branch 1 taken 15143440 times.
✓ Branch 2 taken 29212186 times.
✓ Branch 3 taken 5749246 times.
 50104872 if (l1 && l2) {
50
1/2
✓ Branch 1 taken 29212186 times.
✗ Branch 2 not taken.
 29212186 updateFrontList(front_list, b1, b2);
51
52 // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
53
1/2
✓ Branch 1 taken 29212186 times.
✗ Branch 2 not taken.
 29212186 node->leafCollides(b1, b2, sqrDistLowerBound);
54 29285764 return;
55 }
56
57
3/4
✓ Branch 1 taken 20892686 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 71118 times.
✓ Branch 4 taken 20821568 times.
 20892686 if (node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
58
1/2
✓ Branch 1 taken 71118 times.
✗ Branch 2 not taken.
 71118 updateFrontList(front_list, b1, b2);
59 71118 return;
60 }
61
3/4
✓ Branch 1 taken 20821568 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12788205 times.
✓ Branch 4 taken 8033363 times.
 20821568 if (node->firstOverSecond(b1, b2)) {
62
1/2
✓ Branch 1 taken 12788205 times.
✗ Branch 2 not taken.
 12788205 unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
63
1/2
✓ Branch 1 taken 12788205 times.
✗ Branch 2 not taken.
 12788205 unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
64
65
1/2
✓ Branch 1 taken 12788205 times.
✗ Branch 2 not taken.
 12788205 collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
66
67 // early stop is disabled is front_list is used
68
6/8
✓ Branch 1 taken 12788205 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1331 times.
✓ Branch 4 taken 12786874 times.
✓ Branch 5 taken 1331 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1331 times.
✓ Branch 8 taken 12786874 times.
 12788205 if (node->canStop() && !front_list) return;
69
70
1/2
✓ Branch 1 taken 12786874 times.
✗ Branch 2 not taken.
 12786874 collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
71 12786874 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
72 } else {
73
1/2
✓ Branch 1 taken 8033363 times.
✗ Branch 2 not taken.
 8033363 unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
74
1/2
✓ Branch 1 taken 8033363 times.
✗ Branch 2 not taken.
 8033363 unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
75
76
1/2
✓ Branch 1 taken 8033363 times.
✗ Branch 2 not taken.
 8033363 collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
77
78 // early stop is disabled is front_list is used
79
6/8
✓ Branch 1 taken 8033363 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1129 times.
✓ Branch 4 taken 8032234 times.
✓ Branch 5 taken 1129 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1129 times.
✓ Branch 8 taken 8032234 times.
 8033363 if (node->canStop() && !front_list) return;
80
81
1/2
✓ Branch 1 taken 8032234 times.
✗ Branch 2 not taken.
 8032234 collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
82 8032234 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
83 }
84 }
85
86 12 void collisionNonRecurse(CollisionTraversalNodeBase* node,
87 BVHFrontList* front_list, Scalar& sqrDistLowerBound) {
88 typedef std::pair<unsigned int, unsigned int> BVPair_t;
89 // typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
90 typedef std::vector<BVPair_t> Stack_t;
91
92 12 Stack_t pairs;
93
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 pairs.reserve(1000);
94 12 sqrDistLowerBound = std::numeric_limits<Scalar>::infinity();
95 12 Scalar sdlb = std::numeric_limits<Scalar>::infinity();
96
97
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 pairs.push_back(BVPair_t(0, 0));
98
99
2/2
✓ Branch 1 taken 4892 times.
✓ Branch 2 taken 12 times.
 4904 while (!pairs.empty()) {
100 4892 unsigned int a = pairs.back().first, b = pairs.back().second;
101 4892 pairs.pop_back();
102
103
1/2
✓ Branch 1 taken 4892 times.
✗ Branch 2 not taken.
 4892 bool la = node->isFirstNodeLeaf(a);
104
1/2
✓ Branch 1 taken 4892 times.
✗ Branch 2 not taken.
 4892 bool lb = node->isSecondNodeLeaf(b);
105
106 // Leaf / Leaf case
107
4/4
✓ Branch 0 taken 4382 times.
✓ Branch 1 taken 510 times.
✓ Branch 2 taken 1821 times.
✓ Branch 3 taken 2561 times.
 4892 if (la && lb) {
108
1/2
✓ Branch 1 taken 1821 times.
✗ Branch 2 not taken.
 1821 updateFrontList(front_list, a, b);
109
110 // TODO should we test the BVs ?
111 // if(node->BVDijsoints(a, b, sdlb)) {
112 // if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
113 // continue;
114 //}
115
1/2
✓ Branch 1 taken 1821 times.
✗ Branch 2 not taken.
 1821 node->leafCollides(a, b, sdlb);
116
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1821 times.
 1821 if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
117
3/8
✓ Branch 1 taken 1821 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1821 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1821 times.
 1821 if (node->canStop() && !front_list) return;
118 2452 continue;
119 }
120
121 // TODO shouldn't we test the leaf triangle against BV is la != lb
122 // if (la && !lb) { // leaf triangle 1 against BV 2
123 // } else if (!la && lb) { // BV 1 against leaf triangle 2
124 // }
125
126 // Check the BV
127
3/4
✓ Branch 1 taken 3071 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 631 times.
✓ Branch 4 taken 2440 times.
 3071 if (node->BVDisjoints(a, b, sdlb)) {
128
2/2
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 570 times.
 631 if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
129
1/2
✓ Branch 1 taken 631 times.
✗ Branch 2 not taken.
 631 updateFrontList(front_list, a, b);
130 631 continue;
131 }
132
133
3/4
✓ Branch 1 taken 2440 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 282 times.
✓ Branch 4 taken 2158 times.
 2440 if (node->firstOverSecond(a, b)) {
134
1/2
✓ Branch 1 taken 282 times.
✗ Branch 2 not taken.
 282 unsigned int c1 = (unsigned int)node->getFirstLeftChild(a);
135
1/2
✓ Branch 1 taken 282 times.
✗ Branch 2 not taken.
 282 unsigned int c2 = (unsigned int)node->getFirstRightChild(a);
136
1/2
✓ Branch 2 taken 282 times.
✗ Branch 3 not taken.
 282 pairs.push_back(BVPair_t(c2, b));
137
1/2
✓ Branch 2 taken 282 times.
✗ Branch 3 not taken.
 282 pairs.push_back(BVPair_t(c1, b));
138 } else {
139
1/2
✓ Branch 1 taken 2158 times.
✗ Branch 2 not taken.
 2158 unsigned int c1 = (unsigned int)node->getSecondLeftChild(b);
140
1/2
✓ Branch 1 taken 2158 times.
✗ Branch 2 not taken.
 2158 unsigned int c2 = (unsigned int)node->getSecondRightChild(b);
141
1/2
✓ Branch 2 taken 2158 times.
✗ Branch 3 not taken.
 2158 pairs.push_back(BVPair_t(a, c2));
142
1/2
✓ Branch 2 taken 2158 times.
✗ Branch 3 not taken.
 2158 pairs.push_back(BVPair_t(a, c1));
143 }
144 }
145
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 }
146
147 /** Recurse function for self collision
148 * Make sure node is set correctly so that the first and second tree are the
149 * same
150 */
151 1136145 void distanceRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
152 unsigned int b2, BVHFrontList* front_list) {
153 1136145 bool l1 = node->isFirstNodeLeaf(b1);
154 1136145 bool l2 = node->isSecondNodeLeaf(b2);
155
156
4/4
✓ Branch 0 taken 401431 times.
✓ Branch 1 taken 734714 times.
✓ Branch 2 taken 332460 times.
✓ Branch 3 taken 68971 times.
 1136145 if (l1 && l2) {
157 332460 updateFrontList(front_list, b1, b2);
158
159 332460 node->leafComputeDistance(b1, b2);
160 332460 return;
161 }
162
163 unsigned int a1, a2, c1, c2;
164
165
2/2
✓ Branch 1 taken 568145 times.
✓ Branch 2 taken 235540 times.
 803685 if (node->firstOverSecond(b1, b2)) {
166 568145 a1 = (unsigned int)node->getFirstLeftChild(b1);
167 568145 a2 = b2;
168 568145 c1 = (unsigned int)node->getFirstRightChild(b1);
169 568145 c2 = b2;
170 } else {
171 235540 a1 = b1;
172 235540 a2 = (unsigned int)node->getSecondLeftChild(b2);
173 235540 c1 = b1;
174 235540 c2 = (unsigned int)node->getSecondRightChild(b2);
175 }
176
177 803685 Scalar d1 = node->BVDistanceLowerBound(a1, a2);
178 803685 Scalar d2 = node->BVDistanceLowerBound(c1, c2);
179
180
2/2
✓ Branch 0 taken 398249 times.
✓ Branch 1 taken 405436 times.
 803685 if (d2 < d1) {
181
2/2
✓ Branch 1 taken 350828 times.
✓ Branch 2 taken 47421 times.
 398249 if (!node->canStop(d2))
182 350828 distanceRecurse(node, c1, c2, front_list);
183 else
184 47421 updateFrontList(front_list, c1, c2);
185
186
2/2
✓ Branch 1 taken 192652 times.
✓ Branch 2 taken 205597 times.
 398249 if (!node->canStop(d1))
187 192652 distanceRecurse(node, a1, a2, front_list);
188 else
189 205597 updateFrontList(front_list, a1, a2);
190 } else {
191
2/2
✓ Branch 1 taken 365049 times.
✓ Branch 2 taken 40387 times.
 405436 if (!node->canStop(d1))
192 365049 distanceRecurse(node, a1, a2, front_list);
193 else
194 40387 updateFrontList(front_list, a1, a2);
195
196
2/2
✓ Branch 1 taken 220348 times.
✓ Branch 2 taken 185088 times.
 405436 if (!node->canStop(d2))
197 220348 distanceRecurse(node, c1, c2, front_list);
198 else
199 185088 updateFrontList(front_list, c1, c2);
200 }
201 }
202
203 /** @brief Bounding volume test structure */
204 struct COAL_LOCAL BVT {
205 /** @brief distance between bvs */
206 Scalar d;
207
208 /** @brief bv indices for a pair of bvs in two models */
209 unsigned int b1, b2;
210 };
211
212 /** @brief Comparer between two BVT */
213 39290 struct COAL_LOCAL BVT_Comparer{bool operator()(const BVT& lhs, const BVT& rhs)
214 39290 const {return lhs.d > rhs.d;
215 } // namespace coal
216 }
217 ;
218
219 struct COAL_LOCAL BVTQ {
220 609 BVTQ() : qsize(2) {}
221
222 7936 bool empty() const { return pq.empty(); }
223
224 size_t size() const { return pq.size(); }
225
226 7859 const BVT& top() const { return pq.top(); }
227
228 10908 void push(const BVT& x) { pq.push(x); }
229
230 7859 void pop() { pq.pop(); }
231
232 6042 bool full() const { return (pq.size() + 1 >= qsize); }
233
234 std::priority_queue<BVT, std::vector<BVT>, BVT_Comparer> pq;
235
236 /** @brief Queue size */
237 unsigned int qsize;
238 };
239
240 609 void distanceQueueRecurse(DistanceTraversalNodeBase* node, unsigned int b1,
241 unsigned int b2, BVHFrontList* front_list,
242 unsigned int qsize) {
243
1/2
✓ Branch 1 taken 609 times.
✗ Branch 2 not taken.
 609 BVTQ bvtq;
244 609 bvtq.qsize = qsize;
245
246 BVT min_test;
247 609 min_test.b1 = b1;
248 609 min_test.b2 = b2;
249
250 while (1) {
251
1/2
✓ Branch 1 taken 7936 times.
✗ Branch 2 not taken.
 7936 bool l1 = node->isFirstNodeLeaf(min_test.b1);
252
1/2
✓ Branch 1 taken 7936 times.
✗ Branch 2 not taken.
 7936 bool l2 = node->isSecondNodeLeaf(min_test.b2);
253
254
4/4
✓ Branch 0 taken 6874 times.
✓ Branch 1 taken 1062 times.
✓ Branch 2 taken 1894 times.
✓ Branch 3 taken 4980 times.
 7936 if (l1 && l2) {
255
1/2
✓ Branch 1 taken 1894 times.
✗ Branch 2 not taken.
 1894 updateFrontList(front_list, min_test.b1, min_test.b2);
256
257
1/2
✓ Branch 1 taken 1894 times.
✗ Branch 2 not taken.
 1894 node->leafComputeDistance(min_test.b1, min_test.b2);
258
3/4
✓ Branch 1 taken 6042 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 588 times.
✓ Branch 4 taken 5454 times.
 6042 } else if (bvtq.full()) {
259 // queue should not get two more tests, recur
260
261
1/2
✓ Branch 1 taken 588 times.
✗ Branch 2 not taken.
 588 distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize);
262 } else {
263 // queue capacity is not full yet
264 BVT bvt1, bvt2;
265
266
3/4
✓ Branch 1 taken 5454 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 765 times.
✓ Branch 4 taken 4689 times.
 5454 if (node->firstOverSecond(min_test.b1, min_test.b2)) {
267
1/2
✓ Branch 1 taken 765 times.
✗ Branch 2 not taken.
 765 unsigned int c1 = (unsigned int)node->getFirstLeftChild(min_test.b1);
268
1/2
✓ Branch 1 taken 765 times.
✗ Branch 2 not taken.
 765 unsigned int c2 = (unsigned int)node->getFirstRightChild(min_test.b1);
269 765 bvt1.b1 = c1;
270 765 bvt1.b2 = min_test.b2;
271
1/2
✓ Branch 1 taken 765 times.
✗ Branch 2 not taken.
 765 bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
272
273 765 bvt2.b1 = c2;
274 765 bvt2.b2 = min_test.b2;
275
1/2
✓ Branch 1 taken 765 times.
✗ Branch 2 not taken.
 765 bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
276 } else {
277
1/2
✓ Branch 1 taken 4689 times.
✗ Branch 2 not taken.
 4689 unsigned int c1 = (unsigned int)node->getSecondLeftChild(min_test.b2);
278
1/2
✓ Branch 1 taken 4689 times.
✗ Branch 2 not taken.
 4689 unsigned int c2 = (unsigned int)node->getSecondRightChild(min_test.b2);
279 4689 bvt1.b1 = min_test.b1;
280 4689 bvt1.b2 = c1;
281
1/2
✓ Branch 1 taken 4689 times.
✗ Branch 2 not taken.
 4689 bvt1.d = node->BVDistanceLowerBound(bvt1.b1, bvt1.b2);
282
283 4689 bvt2.b1 = min_test.b1;
284 4689 bvt2.b2 = c2;
285
1/2
✓ Branch 1 taken 4689 times.
✗ Branch 2 not taken.
 4689 bvt2.d = node->BVDistanceLowerBound(bvt2.b1, bvt2.b2);
286 }
287
288
1/2
✓ Branch 1 taken 5454 times.
✗ Branch 2 not taken.
 5454 bvtq.push(bvt1);
289
1/2
✓ Branch 1 taken 5454 times.
✗ Branch 2 not taken.
 5454 bvtq.push(bvt2);
290 }
291
292
3/4
✓ Branch 1 taken 7936 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 77 times.
✓ Branch 4 taken 7859 times.
 7936 if (bvtq.empty())
293 77 break;
294 else {
295
1/2
✓ Branch 1 taken 7859 times.
✗ Branch 2 not taken.
 7859 min_test = bvtq.top();
296
1/2
✓ Branch 1 taken 7859 times.
✗ Branch 2 not taken.
 7859 bvtq.pop();
297
298
3/4
✓ Branch 1 taken 7859 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 532 times.
✓ Branch 4 taken 7327 times.
 7859 if (node->canStop(min_test.d)) {
299
1/2
✓ Branch 1 taken 532 times.
✗ Branch 2 not taken.
 532 updateFrontList(front_list, min_test.b1, min_test.b2);
300 532 break;
301 }
302 }
303 7327 }
304 609 }
305
306 46 void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node,
307 const CollisionRequest& /*request*/,
308 CollisionResult& result,
309 BVHFrontList* front_list) {
310 46 Scalar sqrDistLowerBound = -1, sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
311 46 BVHFrontList::iterator front_iter;
312 46 BVHFrontList append;
313
2/2
✓ Branch 3 taken 8460475 times.
✓ Branch 4 taken 46 times.
 8460521 for (front_iter = front_list->begin(); front_iter != front_list->end();
314 8460475 ++front_iter) {
315 8460475 unsigned int b1 = front_iter->left;
316 8460475 unsigned int b2 = front_iter->right;
317
1/2
✓ Branch 1 taken 8460475 times.
✗ Branch 2 not taken.
 8460475 bool l1 = node->isFirstNodeLeaf(b1);
318
1/2
✓ Branch 1 taken 8460475 times.
✗ Branch 2 not taken.
 8460475 bool l2 = node->isSecondNodeLeaf(b2);
319
320
2/2
✓ Branch 0 taken 8446667 times.
✓ Branch 1 taken 13808 times.
 8460475 if (l1 & l2) {
321 8446667 front_iter->valid = false; // the front node is no longer valid, in
322 // collideRecurse will add again.
323
1/2
✓ Branch 1 taken 8446667 times.
✗ Branch 2 not taken.
 8446667 collisionRecurse(node, b1, b2, &append, sqrDistLowerBound);
324 } else {
325
3/4
✓ Branch 1 taken 13808 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1731 times.
✓ Branch 4 taken 12077 times.
 13808 if (!node->BVDisjoints(b1, b2, sqrDistLowerBound)) {
326 1731 front_iter->valid = false;
327
3/4
✓ Branch 1 taken 1731 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1047 times.
✓ Branch 4 taken 684 times.
 1731 if (node->firstOverSecond(b1, b2)) {
328
1/2
✓ Branch 1 taken 1047 times.
✗ Branch 2 not taken.
 1047 unsigned int c1 = (unsigned int)node->getFirstLeftChild(b1);
329
1/2
✓ Branch 1 taken 1047 times.
✗ Branch 2 not taken.
 1047 unsigned int c2 = (unsigned int)node->getFirstRightChild(b1);
330
331
1/2
✓ Branch 1 taken 1047 times.
✗ Branch 2 not taken.
 1047 collisionRecurse(node, c1, b2, front_list, sqrDistLowerBound1);
332
1/2
✓ Branch 1 taken 1047 times.
✗ Branch 2 not taken.
 1047 collisionRecurse(node, c2, b2, front_list, sqrDistLowerBound2);
333 1047 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
334 } else {
335
1/2
✓ Branch 1 taken 684 times.
✗ Branch 2 not taken.
 684 unsigned int c1 = (unsigned int)node->getSecondLeftChild(b2);
336
1/2
✓ Branch 1 taken 684 times.
✗ Branch 2 not taken.
 684 unsigned int c2 = (unsigned int)node->getSecondRightChild(b2);
337
338
1/2
✓ Branch 1 taken 684 times.
✗ Branch 2 not taken.
 684 collisionRecurse(node, b1, c1, front_list, sqrDistLowerBound1);
339
1/2
✓ Branch 1 taken 684 times.
✗ Branch 2 not taken.
 684 collisionRecurse(node, b1, c2, front_list, sqrDistLowerBound2);
340 684 sqrDistLowerBound = std::min(sqrDistLowerBound1, sqrDistLowerBound2);
341 }
342 }
343 }
344 8460475 result.updateDistanceLowerBound(sqrt(sqrDistLowerBound));
345 }
346
347 // clean the old front list (remove invalid node)
348
2/2
✓ Branch 3 taken 8460475 times.
✓ Branch 4 taken 46 times.
 8460521 for (front_iter = front_list->begin(); front_iter != front_list->end();) {
349
2/2
✓ Branch 1 taken 8448398 times.
✓ Branch 2 taken 12077 times.
 8460475 if (!front_iter->valid)
350 8448398 front_iter = front_list->erase(front_iter);
351 else
352 12077 ++front_iter;
353 }
354
355
2/2
✓ Branch 4 taken 8446667 times.
✓ Branch 5 taken 46 times.
 8446713 for (front_iter = append.begin(); front_iter != append.end(); ++front_iter) {
356
1/2
✓ Branch 2 taken 8446667 times.
✗ Branch 3 not taken.
 8446667 front_list->push_back(*front_iter);
357 }
358 46 }
359
360 } // namespace coal
361