GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_SSaP.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 188 253 74.3%
Branches: 174 349 49.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-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 #include "coal/broadphase/broadphase_SSaP.h"
39 #include "coal/tracy.hh"
40
41 namespace coal {
42
43 /** @brief Functor sorting objects according to the AABB lower x bound */
44 struct SortByXLow {
45 218669 bool operator()(const CollisionObject* a, const CollisionObject* b) const {
46
2/2
✓ Branch 4 taken 131640 times.
✓ Branch 5 taken 87029 times.
 218669 if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true;
47 87029 return false;
48 }
49 };
50
51 /** @brief Functor sorting objects according to the AABB lower y bound */
52 struct SortByYLow {
53 192423 bool operator()(const CollisionObject* a, const CollisionObject* b) const {
54
2/2
✓ Branch 4 taken 100261 times.
✓ Branch 5 taken 92162 times.
 192423 if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true;
55 92162 return false;
56 }
57 };
58
59 /** @brief Functor sorting objects according to the AABB lower z bound */
60 struct SortByZLow {
61 202733 bool operator()(const CollisionObject* a, const CollisionObject* b) const {
62
2/2
✓ Branch 4 taken 128772 times.
✓ Branch 5 taken 73961 times.
 202733 if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true;
63 73961 return false;
64 }
65 };
66
67 /** @brief Dummy collision object with a point AABB */
68 class COAL_DLLAPI DummyCollisionObject : public CollisionObject {
69 public:
70 6958 DummyCollisionObject(const AABB& aabb_)
71
1/2
✓ Branch 2 taken 6958 times.
✗ Branch 3 not taken.
 6958 : CollisionObject(shared_ptr<CollisionGeometry>()) {
72
1/2
✓ Branch 1 taken 6958 times.
✗ Branch 2 not taken.
 6958 this->aabb = aabb_;
73 6958 }
74
75 void computeLocalAABB() {}
76 };
77
78 //==============================================================================
79 ✗ void SSaPCollisionManager::unregisterObject(CollisionObject* obj) {
80 ✗ setup();
81
82 ✗ DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
83
84 ✗ auto pos_start1 = objs_x.begin();
85 auto pos_end1 =
86 ✗ std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
87
88 ✗ while (pos_start1 < pos_end1) {
89 ✗ if (*pos_start1 == obj) {
90 ✗ objs_x.erase(pos_start1);
91 ✗ break;
92 }
93 ✗ ++pos_start1;
94 }
95
96 ✗ auto pos_start2 = objs_y.begin();
97 auto pos_end2 =
98 ✗ std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
99
100 ✗ while (pos_start2 < pos_end2) {
101 ✗ if (*pos_start2 == obj) {
102 ✗ objs_y.erase(pos_start2);
103 ✗ break;
104 }
105 ✗ ++pos_start2;
106 }
107
108 ✗ auto pos_start3 = objs_z.begin();
109 auto pos_end3 =
110 ✗ std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
111
112 ✗ while (pos_start3 < pos_end3) {
113 ✗ if (*pos_start3 == obj) {
114 ✗ objs_z.erase(pos_start3);
115 ✗ break;
116 }
117 ✗ ++pos_start3;
118 }
119 }
120
121 //==============================================================================
122 51 SSaPCollisionManager::SSaPCollisionManager() : setup_(false) {
123 // Do nothing
124 51 }
125
126 //==============================================================================
127 12671 void SSaPCollisionManager::registerObject(CollisionObject* obj) {
128 12671 objs_x.push_back(obj);
129 12671 objs_y.push_back(obj);
130 12671 objs_z.push_back(obj);
131 12671 setup_ = false;
132 12671 }
133
134 //==============================================================================
135 64 void SSaPCollisionManager::setup() {
136
1/2
✓ Branch 0 taken 64 times.
✗ Branch 1 not taken.
 64 if (!setup_) {
137 64 std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
138 64 std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
139 64 std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
140 64 setup_ = true;
141 }
142 64 }
143
144 //==============================================================================
145 13 void SSaPCollisionManager::update() {
146 13 setup_ = false;
147 13 setup();
148 13 }
149
150 //==============================================================================
151 ✗ void SSaPCollisionManager::clear() {
152 ✗ objs_x.clear();
153 ✗ objs_y.clear();
154 ✗ objs_z.clear();
155 ✗ setup_ = false;
156 }
157
158 //==============================================================================
159 ✗ void SSaPCollisionManager::getObjects(
160 std::vector<CollisionObject*>& objs) const {
161 ✗ objs.resize(objs_x.size());
162 ✗ std::copy(objs_x.begin(), objs_x.end(), objs.begin());
163 }
164
165 //==============================================================================
166 3762 bool SSaPCollisionManager::checkColl(
167 std::vector<CollisionObject*>::const_iterator pos_start,
168 std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj,
169 CollisionCallBackBase* callback) const {
170
2/2
✓ Branch 1 taken 175182 times.
✓ Branch 2 taken 3761 times.
 178943 while (pos_start < pos_end) {
171
1/2
✓ Branch 1 taken 175182 times.
✗ Branch 2 not taken.
 175182 if (*pos_start != obj) // no collision between the same object
172 {
173
2/2
✓ Branch 4 taken 19 times.
✓ Branch 5 taken 175163 times.
 175182 if ((*pos_start)->getAABB().overlap(obj->getAABB())) {
174
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 18 times.
 19 if ((*callback)(*pos_start, obj)) return true;
175 }
176 }
177 175181 pos_start++;
178 }
179 3761 return false;
180 }
181
182 //==============================================================================
183 3196 bool SSaPCollisionManager::checkDis(
184 typename std::vector<CollisionObject*>::const_iterator pos_start,
185 typename std::vector<CollisionObject*>::const_iterator pos_end,
186 CollisionObject* obj, DistanceCallBackBase* callback,
187 Scalar& min_dist) const {
188
2/2
✓ Branch 1 taken 921115 times.
✓ Branch 2 taken 3196 times.
 924311 while (pos_start < pos_end) {
189
2/2
✓ Branch 1 taken 918079 times.
✓ Branch 2 taken 3036 times.
 921115 if (*pos_start != obj) // no distance between the same object
190 {
191
2/2
✓ Branch 4 taken 2975 times.
✓ Branch 5 taken 915104 times.
 918079 if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) {
192
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2975 times.
 2975 if ((*callback)(*pos_start, obj, min_dist)) return true;
193 }
194 }
195 921115 pos_start++;
196 }
197
198 3196 return false;
199 }
200
201 //==============================================================================
202 3822 void SSaPCollisionManager::collide(CollisionObject* obj,
203 CollisionCallBackBase* callback) const {
204 COAL_TRACY_ZONE_SCOPED_N(
205 "coal::SSaPCollisionManager::collide(CollisionObject*, "
206 "CollisionCallBackBase*)");
207 3822 callback->init();
208
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
 3822 if (size() == 0) return;
209
210 3762 collide_(obj, callback);
211 }
212
213 //==============================================================================
214 3762 bool SSaPCollisionManager::collide_(CollisionObject* obj,
215 CollisionCallBackBase* callback) const {
216 static const unsigned int CUTOFF = 100;
217
218
2/4
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
 3762 DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
219 3762 bool coll_res = false;
220
221 3762 const auto pos_start1 = objs_x.begin();
222 const auto pos_end1 =
223
1/2
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
 3762 std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
224 3762 long d1 = pos_end1 - pos_start1;
225
226
2/2
✓ Branch 0 taken 1215 times.
✓ Branch 1 taken 2547 times.
 3762 if (d1 > CUTOFF) {
227 1215 const auto pos_start2 = objs_y.begin();
228 const auto pos_end2 =
229
1/2
✓ Branch 2 taken 1215 times.
✗ Branch 3 not taken.
 1215 std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
230 1215 long d2 = pos_end2 - pos_start2;
231
232
2/2
✓ Branch 0 taken 820 times.
✓ Branch 1 taken 395 times.
 1215 if (d2 > CUTOFF) {
233 820 const auto pos_start3 = objs_z.begin();
234 const auto pos_end3 =
235
1/2
✓ Branch 2 taken 820 times.
✗ Branch 3 not taken.
 820 std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
236 820 long d3 = pos_end3 - pos_start3;
237
238
2/2
✓ Branch 0 taken 562 times.
✓ Branch 1 taken 258 times.
 820 if (d3 > CUTOFF) {
239
4/4
✓ Branch 0 taken 291 times.
✓ Branch 1 taken 271 times.
✓ Branch 2 taken 212 times.
✓ Branch 3 taken 79 times.
 562 if (d3 <= d2 && d3 <= d1)
240
1/2
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
 212 coll_res = checkColl(pos_start3, pos_end3, obj, callback);
241 else {
242
4/4
✓ Branch 0 taken 272 times.
✓ Branch 1 taken 78 times.
✓ Branch 2 taken 191 times.
✓ Branch 3 taken 81 times.
 350 if (d2 <= d3 && d2 <= d1)
243
1/2
✓ Branch 1 taken 191 times.
✗ Branch 2 not taken.
 191 coll_res = checkColl(pos_start2, pos_end2, obj, callback);
244 else
245
1/2
✓ Branch 1 taken 159 times.
✗ Branch 2 not taken.
 159 coll_res = checkColl(pos_start1, pos_end1, obj, callback);
246 }
247 } else
248
1/2
✓ Branch 1 taken 258 times.
✗ Branch 2 not taken.
 258 coll_res = checkColl(pos_start3, pos_end3, obj, callback);
249 } else
250
1/2
✓ Branch 1 taken 395 times.
✗ Branch 2 not taken.
 395 coll_res = checkColl(pos_start2, pos_end2, obj, callback);
251 } else
252
1/2
✓ Branch 1 taken 2547 times.
✗ Branch 2 not taken.
 2547 coll_res = checkColl(pos_start1, pos_end1, obj, callback);
253
254 3762 return coll_res;
255 3762 }
256
257 //==============================================================================
258 80 void SSaPCollisionManager::distance(CollisionObject* obj,
259 DistanceCallBackBase* callback) const {
260 COAL_TRACY_ZONE_SCOPED_N(
261 "coal::SSaPCollisionManager::distance(CollisionObject*, "
262 "DistanceCallBackBase*)");
263
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 callback->init();
264
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 80 times.
 80 if (size() == 0) return;
265
266 80 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
267
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 distance_(obj, callback, min_dist);
268 }
269
270 //==============================================================================
271 3070 bool SSaPCollisionManager::distance_(CollisionObject* obj,
272 DistanceCallBackBase* callback,
273 Scalar& min_dist) const {
274 static const unsigned int CUTOFF = 100;
275
3/6
✓ Branch 3 taken 3070 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3070 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3070 times.
✗ Branch 10 not taken.
 3070 Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
276
1/2
✓ Branch 2 taken 3070 times.
✗ Branch 3 not taken.
 3070 Vec3s dummy_vector = obj->getAABB().max_;
277
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 86 times.
 3070 if (min_dist < (std::numeric_limits<Scalar>::max)())
278
2/4
✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2984 times.
✗ Branch 5 not taken.
 2984 dummy_vector += Vec3s(min_dist, min_dist, min_dist);
279
280 typename std::vector<CollisionObject*>::const_iterator pos_start1 =
281 3070 objs_x.begin();
282 typename std::vector<CollisionObject*>::const_iterator pos_start2 =
283 3070 objs_y.begin();
284 typename std::vector<CollisionObject*>::const_iterator pos_start3 =
285 3070 objs_z.begin();
286 typename std::vector<CollisionObject*>::const_iterator pos_end1 =
287 3070 objs_x.end();
288 typename std::vector<CollisionObject*>::const_iterator pos_end2 =
289 3070 objs_y.end();
290 typename std::vector<CollisionObject*>::const_iterator pos_end3 =
291 3070 objs_z.end();
292
293 3070 int status = 1;
294 Scalar old_min_distance;
295
296 while (1) {
297 3196 old_min_distance = min_dist;
298
2/4
✓ Branch 1 taken 3196 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3196 times.
✗ Branch 5 not taken.
 3196 DummyCollisionObject dummyHigh((AABB(dummy_vector)));
299
300 pos_end1 =
301
1/2
✓ Branch 2 taken 3196 times.
✗ Branch 3 not taken.
 3196 std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
302 3196 long d1 = pos_end1 - pos_start1;
303
304 3196 bool dist_res = false;
305
306
2/2
✓ Branch 0 taken 2757 times.
✓ Branch 1 taken 439 times.
 3196 if (d1 > CUTOFF) {
307 pos_end2 =
308
1/2
✓ Branch 2 taken 2757 times.
✗ Branch 3 not taken.
 2757 std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
309 2757 long d2 = pos_end2 - pos_start2;
310
311
2/2
✓ Branch 0 taken 2741 times.
✓ Branch 1 taken 16 times.
 2757 if (d2 > CUTOFF) {
312
1/2
✓ Branch 2 taken 2741 times.
✗ Branch 3 not taken.
 2741 pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh,
313 SortByZLow());
314 2741 long d3 = pos_end3 - pos_start3;
315
316
2/2
✓ Branch 0 taken 2291 times.
✓ Branch 1 taken 450 times.
 2741 if (d3 > CUTOFF) {
317
4/4
✓ Branch 0 taken 2265 times.
✓ Branch 1 taken 26 times.
✓ Branch 2 taken 1136 times.
✓ Branch 3 taken 1129 times.
 2291 if (d3 <= d2 && d3 <= d1)
318
1/2
✓ Branch 1 taken 1136 times.
✗ Branch 2 not taken.
 1136 dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
319 else {
320
4/4
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 1127 times.
✓ Branch 2 taken 20 times.
✓ Branch 3 taken 8 times.
 1155 if (d2 <= d3 && d2 <= d1)
321 20 dist_res =
322
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 checkDis(pos_start2, pos_end2, obj, callback, min_dist);
323 else
324 dist_res =
325
1/2
✓ Branch 1 taken 1135 times.
✗ Branch 2 not taken.
 1135 checkDis(pos_start1, pos_end1, obj, callback, min_dist);
326 }
327 } else
328
1/2
✓ Branch 1 taken 450 times.
✗ Branch 2 not taken.
 450 dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
329 } else
330
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist);
331 } else {
332
1/2
✓ Branch 1 taken 439 times.
✗ Branch 2 not taken.
 439 dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist);
333 }
334
335
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3196 times.
 3196 if (dist_res) return true;
336
337
2/2
✓ Branch 0 taken 3110 times.
✓ Branch 1 taken 86 times.
 3196 if (status == 1) {
338
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 126 times.
 3110 if (old_min_distance < (std::numeric_limits<Scalar>::max)())
339 2984 break;
340 else {
341 // from infinity to a finite one, only need one additional loop
342 // to check the possible missed ones to the right of the objs array
343
2/2
✓ Branch 0 taken 86 times.
✓ Branch 1 taken 40 times.
 126 if (min_dist < old_min_distance) {
344 dummy_vector =
345
3/6
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 86 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 86 times.
✗ Branch 9 not taken.
 86 obj->getAABB().max_ + Vec3s(min_dist, min_dist, min_dist);
346 86 status = 0;
347 } else // need more loop
348 {
349 80 if (dummy_vector.isApprox(
350
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 obj->getAABB().max_,
351
2/2
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 36 times.
 80 std::numeric_limits<Scalar>::epsilon() * 100))
352
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 dummy_vector = dummy_vector + delta;
353 else
354
3/6
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 36 times.
✗ Branch 9 not taken.
 36 dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
355 }
356 }
357
358 // yes, following is wrong, will result in too large distance.
359 // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
360 // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
361 // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
362
1/2
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
 86 } else if (status == 0)
363 86 break;
364
2/3
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3070 times.
 3322 }
365
366 3070 return false;
367 }
368
369 //==============================================================================
370 35 int SSaPCollisionManager::selectOptimalAxis(
371 const std::vector<CollisionObject*>& objs_x,
372 const std::vector<CollisionObject*>& objs_y,
373 const std::vector<CollisionObject*>& objs_z,
374 typename std::vector<CollisionObject*>::const_iterator& it_beg,
375 typename std::vector<CollisionObject*>::const_iterator& it_end) {
376 /// simple sweep and prune method
377 35 Scalar delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
378 35 (objs_x[0])->getAABB().min_[0];
379 35 Scalar delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
380 35 (objs_y[0])->getAABB().min_[1];
381 35 Scalar delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
382 35 (objs_z[0])->getAABB().min_[2];
383
384 35 int axis = 0;
385
4/4
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 3 times.
 35 if (delta_y > delta_x && delta_y > delta_z)
386 3 axis = 1;
387
3/4
✓ Branch 0 taken 32 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 21 times.
✓ Branch 3 taken 11 times.
 32 else if (delta_z > delta_y && delta_z > delta_x)
388 21 axis = 2;
389
390
3/4
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 35 switch (axis) {
391 11 case 0:
392 11 it_beg = objs_x.begin();
393 11 it_end = objs_x.end();
394 11 break;
395 3 case 1:
396 3 it_beg = objs_y.begin();
397 3 it_end = objs_y.end();
398 3 break;
399 21 case 2:
400 21 it_beg = objs_z.begin();
401 21 it_end = objs_z.end();
402 21 break;
403 }
404
405 35 return axis;
406 }
407
408 //==============================================================================
409 37 void SSaPCollisionManager::collide(CollisionCallBackBase* callback) const {
410 COAL_TRACY_ZONE_SCOPED_N(
411 "coal::SSaPCollisionManager::collide(CollisionCallBackBase*)");
412
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 37 callback->init();
413
3/4
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 29 times.
 40 if (size() == 0) return;
414
415 29 typename std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
416
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 29 int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end);
417
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 15 times.
 29 int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
418
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 3 times.
 29 int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
419
420 29 run_pos = pos;
421
422
5/6
✓ Branch 1 taken 1944 times.
✓ Branch 2 taken 26 times.
✓ Branch 4 taken 1944 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1944 times.
✓ Branch 7 taken 26 times.
 1970 while ((run_pos < pos_end) && (pos < pos_end)) {
423 1944 CollisionObject* obj = *(pos++);
424
425 while (1) {
426
3/6
✓ Branch 3 taken 1944 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1944 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1944 times.
 1944 if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) {
427 ✗ run_pos++;
428 ✗ if (run_pos == pos_end) break;
429 ✗ continue;
430 } else {
431 1944 run_pos++;
432 1944 break;
433 }
434 }
435
436
2/2
✓ Branch 1 taken 1918 times.
✓ Branch 2 taken 26 times.
 1944 if (run_pos < pos_end) {
437 1918 typename std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
438
439
4/6
✓ Branch 3 taken 7731 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 7731 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 5835 times.
✓ Branch 10 taken 1896 times.
 7731 while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) {
440 5835 CollisionObject* obj2 = *run_pos2;
441 5835 run_pos2++;
442
443
6/8
✓ Branch 2 taken 5835 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5835 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 3087 times.
✓ Branch 9 taken 2748 times.
✓ Branch 10 taken 176 times.
✓ Branch 11 taken 5659 times.
 8922 if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) &&
444
4/6
✓ Branch 2 taken 3087 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3087 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 176 times.
✓ Branch 9 taken 2911 times.
 3087 (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) {
445
6/8
✓ Branch 2 taken 176 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 176 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 98 times.
✓ Branch 9 taken 78 times.
✓ Branch 10 taken 7 times.
✓ Branch 11 taken 169 times.
 274 if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) &&
446
4/6
✓ Branch 2 taken 98 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 98 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7 times.
✓ Branch 9 taken 91 times.
 98 (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) {
447
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4 times.
 7 if ((*callback)(obj, obj2)) return;
448 }
449 }
450
451
2/2
✓ Branch 1 taken 19 times.
✓ Branch 2 taken 5813 times.
 5832 if (run_pos2 == pos_end) break;
452 }
453 }
454 }
455 }
456
457 //==============================================================================
458 6 void SSaPCollisionManager::distance(DistanceCallBackBase* callback) const {
459 COAL_TRACY_ZONE_SCOPED_N(
460 "coal::SSaPCollisionManager::distance(DistanceCallBackBase*)");
461
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 callback->init();
462
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 6 if (size() == 0) return;
463
464 6 typename std::vector<CollisionObject*>::const_iterator it, it_end;
465
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
466
467 6 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
468
2/2
✓ Branch 2 taken 2990 times.
✓ Branch 3 taken 6 times.
 2996 for (; it != it_end; ++it) {
469
2/4
✓ Branch 2 taken 2990 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2990 times.
 2990 if (distance_(*it, callback, min_dist)) return;
470 }
471 }
472
473 //==============================================================================
474 ✗ void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
475 CollisionCallBackBase* callback) const {
476 COAL_TRACY_ZONE_SCOPED_N(
477 "coal::SSaPCollisionManager::collide(BroadPhaseCollisionManager*, "
478 "CollisionCallBackBase*)");
479 ✗ callback->init();
480 ✗ SSaPCollisionManager* other_manager =
481 static_cast<SSaPCollisionManager*>(other_manager_);
482
483 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
484
485 ✗ if (this == other_manager) {
486 ✗ collide(callback);
487 ✗ return;
488 }
489
490 ✗ typename std::vector<CollisionObject*>::const_iterator it, end;
491 ✗ if (this->size() < other_manager->size()) {
492 ✗ for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
493 ✗ if (other_manager->collide_(*it, callback)) return;
494 } else {
495 ✗ for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
496 ✗ it != end; ++it)
497 ✗ if (collide_(*it, callback)) return;
498 }
499 }
500
501 //==============================================================================
502 ✗ void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
503 DistanceCallBackBase* callback) const {
504 COAL_TRACY_ZONE_SCOPED_N(
505 "coal::SSaPCollisionManager::distance(BroadPhaseCollisionManager*, "
506 "DistanceCallBackBase*)");
507 ✗ callback->init();
508 ✗ SSaPCollisionManager* other_manager =
509 static_cast<SSaPCollisionManager*>(other_manager_);
510
511 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
512
513 ✗ if (this == other_manager) {
514 ✗ distance(callback);
515 ✗ return;
516 }
517
518 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
519 ✗ typename std::vector<CollisionObject*>::const_iterator it, end;
520 ✗ if (this->size() < other_manager->size()) {
521 ✗ for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
522 ✗ if (other_manager->distance_(*it, callback, min_dist)) return;
523 } else {
524 ✗ for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
525 ✗ it != end; ++it)
526 ✗ if (distance_(*it, callback, min_dist)) return;
527 }
528 }
529
530 //==============================================================================
531 ✗ bool SSaPCollisionManager::empty() const { return objs_x.empty(); }
532
533 //==============================================================================
534 3945 size_t SSaPCollisionManager::size() const { return objs_x.size(); }
535
536 } // namespace coal
537