GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_SaP.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 326 468 69.7%
Branches: 306 614 49.8%
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_SaP.h"
39 #include "coal/tracy.hh"
40
41 namespace coal {
42
43 //==============================================================================
44 ✗ void SaPCollisionManager::unregisterObject(CollisionObject* obj) {
45 ✗ auto it = AABB_arr.begin();
46 ✗ for (auto end = AABB_arr.end(); it != end; ++it) {
47 ✗ if ((*it)->obj == obj) break;
48 }
49
50 ✗ AABB_arr.erase(it);
51 ✗ obj_aabb_map.erase(obj);
52
53 ✗ if (it == AABB_arr.end()) return;
54
55 ✗ SaPAABB* curr = *it;
56 ✗ *it = nullptr;
57
58 ✗ for (int coord = 0; coord < 3; ++coord) {
59 // first delete the lo endpoint of the interval.
60 ✗ if (curr->lo->prev[coord] == nullptr)
61 ✗ elist[coord] = curr->lo->next[coord];
62 else
63 ✗ curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
64
65 ✗ curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
66
67 // then, delete the "hi" endpoint.
68 ✗ if (curr->hi->prev[coord] == nullptr)
69 ✗ elist[coord] = curr->hi->next[coord];
70 else
71 ✗ curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
72
73 ✗ if (curr->hi->next[coord] != nullptr)
74 ✗ curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
75 }
76
77 ✗ delete curr->lo;
78 ✗ delete curr->hi;
79 ✗ delete curr;
80
81 ✗ overlap_pairs.remove_if(isUnregistered(obj));
82 }
83
84 //==============================================================================
85
2/2
✓ Branch 2 taken 153 times.
✓ Branch 3 taken 51 times.
 204 SaPCollisionManager::SaPCollisionManager() {
86 51 elist[0] = nullptr;
87 51 elist[1] = nullptr;
88 51 elist[2] = nullptr;
89
90 51 optimal_axis = 0;
91 51 }
92
93 //==============================================================================
94
3/4
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 150 times.
✓ Branch 7 taken 50 times.
 500 SaPCollisionManager::~SaPCollisionManager() { clear(); }
95
96 //==============================================================================
97 51 void SaPCollisionManager::registerObjects(
98 const std::vector<CollisionObject*>& other_objs) {
99
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 43 times.
 51 if (other_objs.empty()) return;
100
101
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 43 times.
 43 if (size() > 0)
102 ✗ BroadPhaseCollisionManager::registerObjects(other_objs);
103 else {
104
1/2
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
 43 std::vector<EndPoint*> endpoints(2 * other_objs.size());
105
106
2/2
✓ Branch 1 taken 12671 times.
✓ Branch 2 taken 43 times.
 12714 for (size_t i = 0; i < other_objs.size(); ++i) {
107
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
 12671 SaPAABB* sapaabb = new SaPAABB();
108 12671 sapaabb->obj = other_objs[i];
109
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 sapaabb->lo = new EndPoint();
110
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 sapaabb->hi = new EndPoint();
111
1/2
✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
 12671 sapaabb->cached = other_objs[i]->getAABB();
112 12671 endpoints[2 * i] = sapaabb->lo;
113 12671 endpoints[2 * i + 1] = sapaabb->hi;
114 12671 sapaabb->lo->minmax = 0;
115 12671 sapaabb->hi->minmax = 1;
116 12671 sapaabb->lo->aabb = sapaabb;
117 12671 sapaabb->hi->aabb = sapaabb;
118
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 AABB_arr.push_back(sapaabb);
119
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 obj_aabb_map[other_objs[i]] = sapaabb;
120 }
121
122 Scalar scale[3];
123
2/2
✓ Branch 0 taken 129 times.
✓ Branch 1 taken 43 times.
 172 for (int coord = 0; coord < 3; ++coord) {
124
1/2
✓ Branch 3 taken 129 times.
✗ Branch 4 not taken.
 129 std::sort(
125 endpoints.begin(), endpoints.end(),
126
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 std::bind(std::less<Scalar>(),
127
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
128 &EndPoint::getVal),
129 std::placeholders::_1, coord),
130
1/2
✓ Branch 1 taken 129 times.
✗ Branch 2 not taken.
 129 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
131 &EndPoint::getVal),
132 std::placeholders::_2, coord)));
133
134 129 endpoints[0]->prev[coord] = nullptr;
135 129 endpoints[0]->next[coord] = endpoints[1];
136
2/2
✓ Branch 1 taken 75768 times.
✓ Branch 2 taken 129 times.
 75897 for (size_t i = 1; i < endpoints.size() - 1; ++i) {
137 75768 endpoints[i]->prev[coord] = endpoints[i - 1];
138 75768 endpoints[i]->next[coord] = endpoints[i + 1];
139 }
140 129 endpoints[endpoints.size() - 1]->prev[coord] =
141 129 endpoints[endpoints.size() - 2];
142 129 endpoints[endpoints.size() - 1]->next[coord] = nullptr;
143
144 129 elist[coord] = endpoints[0];
145
146
1/2
✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
 129 scale[coord] = endpoints.back()->aabb->cached.max_[coord] -
147
1/2
✓ Branch 2 taken 129 times.
✗ Branch 3 not taken.
 129 endpoints[0]->aabb->cached.min_[coord];
148 }
149
150 43 int axis = 0;
151
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 23 times.
 43 if (scale[axis] < scale[1]) axis = 1;
152
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 22 times.
 43 if (scale[axis] < scale[2]) axis = 2;
153
154 43 EndPoint* pos = elist[axis];
155
156
2/2
✓ Branch 0 taken 16282 times.
✓ Branch 1 taken 43 times.
 16325 while (pos != nullptr) {
157 16282 EndPoint* pos_next = nullptr;
158 16282 SaPAABB* aabb = pos->aabb;
159 16282 EndPoint* pos_it = pos->next[axis];
160
161
2/2
✓ Branch 0 taken 5458053 times.
✓ Branch 1 taken 3611 times.
 5461664 while (pos_it != nullptr) {
162
2/2
✓ Branch 0 taken 12671 times.
✓ Branch 1 taken 5445382 times.
 5458053 if (pos_it->aabb == aabb) {
163
2/2
✓ Branch 0 taken 3611 times.
✓ Branch 1 taken 9060 times.
 12671 if (pos_next == nullptr) pos_next = pos_it;
164 12671 break;
165 }
166
167
2/2
✓ Branch 0 taken 2722182 times.
✓ Branch 1 taken 2723200 times.
 5445382 if (pos_it->minmax == 0) {
168
2/2
✓ Branch 0 taken 12628 times.
✓ Branch 1 taken 2709554 times.
 2722182 if (pos_next == nullptr) pos_next = pos_it;
169
3/4
✓ Branch 1 taken 2722182 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 78455 times.
✓ Branch 4 taken 2643727 times.
 2722182 if (pos_it->aabb->cached.overlap(aabb->cached))
170
1/2
✓ Branch 1 taken 78455 times.
✗ Branch 2 not taken.
 78455 overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj);
171 }
172 5445382 pos_it = pos_it->next[axis];
173 }
174
175 16282 pos = pos_next;
176 }
177 43 }
178
179 43 updateVelist();
180 }
181
182 //==============================================================================
183 ✗ void SaPCollisionManager::registerObject(CollisionObject* obj) {
184 // Initialize a new SaPAABB associated with current Collision Object
185 ✗ SaPAABB* new_sap = new SaPAABB;
186 ✗ new_sap->cached = obj->getAABB();
187 ✗ new_sap->obj = obj;
188
189 ✗ new_sap->lo = new EndPoint;
190 ✗ new_sap->lo->minmax = 0;
191 ✗ new_sap->lo->aabb = new_sap;
192
193 ✗ new_sap->hi = new EndPoint;
194 ✗ new_sap->hi->minmax = 1;
195 ✗ new_sap->hi->aabb = new_sap;
196 ✗ for (int coord = 0; coord < 3; ++coord) {
197 ✗ EndPoint* current = elist[coord];
198
199 // first insert the lo end point
200 ✗ if (current == nullptr) // empty list
201 {
202 ✗ elist[coord] = new_sap->lo;
203 ✗ new_sap->lo->prev[coord] = new_sap->lo->next[coord] = nullptr;
204 } else // otherwise, find the correct location in the list and insert
205 {
206 ✗ EndPoint* curr_lo = new_sap->lo;
207 ✗ Scalar curr_lo_val = curr_lo->getVal()[coord];
208 ✗ while ((current->getVal()[coord] < curr_lo_val) &&
209 ✗ (current->next[coord] != nullptr))
210 ✗ current = current->next[coord];
211
212 ✗ if (current->getVal()[coord] >= curr_lo_val) {
213 ✗ curr_lo->prev[coord] = current->prev[coord];
214 ✗ curr_lo->next[coord] = current;
215 ✗ if (current->prev[coord] == nullptr) // current was the first box
216 ✗ elist[coord] =
217 curr_lo; // new_sap->lo becomes the new first box on the axis
218 else
219 ✗ current->prev[coord]->next[coord] = curr_lo;
220
221 ✗ current->prev[coord] =
222 curr_lo; // new_sap->lo becomes the predecessor of current
223 } else { // current->next[coord] == nullptr, so the current is just the
224 // cell before current->
225 ✗ curr_lo->prev[coord] = current;
226 ✗ curr_lo->next[coord] = nullptr;
227 ✗ current->next[coord] = curr_lo;
228 }
229 }
230
231 // now insert hi end point
232 ✗ current = new_sap->lo;
233
234 ✗ EndPoint* curr_hi = new_sap->hi;
235 ✗ Scalar curr_hi_val = curr_hi->getVal()[coord];
236
237 ✗ if (coord == 0) {
238 ✗ while ((current->getVal()[coord] < curr_hi_val) &&
239 ✗ (current->next[coord] != nullptr)) {
240 ✗ if (current != new_sap->lo)
241 ✗ if (current->aabb->cached.overlap(new_sap->cached))
242 ✗ overlap_pairs.emplace_back(current->aabb->obj, obj);
243
244 ✗ current = current->next[coord];
245 }
246 } else {
247 ✗ while ((current->getVal()[coord] < curr_hi_val) &&
248 ✗ (current->next[coord] != nullptr))
249 ✗ current = current->next[coord];
250 }
251
252 ✗ if (current->getVal()[coord] >= curr_hi_val) {
253 ✗ curr_hi->prev[coord] = current->prev[coord];
254 ✗ curr_hi->next[coord] = current;
255 ✗ if (current->prev[coord] != nullptr)
256 ✗ current->prev[coord]->next[coord] = curr_hi;
257
258 ✗ current->prev[coord] = curr_hi;
259 } else {
260 ✗ curr_hi->prev[coord] = current;
261 ✗ curr_hi->next[coord] = nullptr;
262 ✗ current->next[coord] = curr_hi;
263 }
264 }
265
266 ✗ AABB_arr.push_back(new_sap);
267
268 ✗ obj_aabb_map[obj] = new_sap;
269
270 ✗ updateVelist();
271 }
272
273 //==============================================================================
274 64 void SaPCollisionManager::setup() {
275
3/4
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 56 times.
 64 if (size() == 0) return;
276
277 Scalar scale[3];
278
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
 56 scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
279
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
 56 scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);
280
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
 56 scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
281
282 56 int axis = 0;
283
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 32 times.
 56 if (scale[axis] < scale[1]) axis = 1;
284
2/2
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 31 times.
 56 if (scale[axis] < scale[2]) axis = 2;
285 56 optimal_axis = axis;
286 }
287
288 //==============================================================================
289 1344 void SaPCollisionManager::update_(SaPAABB* updated_aabb) {
290
2/4
✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1344 times.
 1344 if (updated_aabb->cached == updated_aabb->obj->getAABB()) return;
291
292 1344 SaPAABB* current = updated_aabb;
293
294
1/2
✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
 1344 const AABB current_aabb = current->obj->getAABB();
295
296 1344 const Vec3s& new_min = current_aabb.min_;
297 1344 const Vec3s& new_max = current_aabb.max_;
298
299
2/2
✓ Branch 0 taken 4032 times.
✓ Branch 1 taken 1344 times.
 5376 for (int coord = 0; coord < 3; ++coord) {
300 int direction; // -1 reverse, 0 nochange, 1 forward
301 EndPoint* temp;
302
303
4/6
✓ Branch 1 taken 4032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4032 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2040 times.
✓ Branch 7 taken 1992 times.
 4032 if (current->lo->getVal(coord) > new_min[coord])
304 2040 direction = -1;
305
3/6
✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1992 times.
✗ Branch 7 not taken.
 1992 else if (current->lo->getVal(coord) < new_min[coord])
306 1992 direction = 1;
307 else
308 ✗ direction = 0;
309
310
2/2
✓ Branch 0 taken 2040 times.
✓ Branch 1 taken 1992 times.
 4032 if (direction == -1) {
311 // first update the "lo" endpoint of the interval
312
2/2
✓ Branch 0 taken 2027 times.
✓ Branch 1 taken 13 times.
 2040 if (current->lo->prev[coord] != nullptr) {
313 2027 temp = current->lo->prev[coord];
314
8/10
✓ Branch 0 taken 11345 times.
✓ Branch 1 taken 12 times.
✓ Branch 3 taken 11345 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11345 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 9330 times.
✓ Branch 9 taken 2015 times.
✓ Branch 10 taken 9330 times.
✓ Branch 11 taken 2027 times.
 11357 while ((temp != nullptr) && (temp->getVal(coord) > new_min[coord])) {
315
2/2
✓ Branch 0 taken 4657 times.
✓ Branch 1 taken 4673 times.
 9330 if (temp->minmax == 1)
316
3/4
✓ Branch 1 taken 4657 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 4653 times.
 4657 if (temp->aabb->cached.overlap(current_aabb))
317
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
318 9330 temp = temp->prev[coord];
319 }
320
321
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 2015 times.
 2027 if (temp == nullptr) {
322 12 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
323 12 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
324 12 current->lo->prev[coord] = nullptr;
325 12 current->lo->next[coord] = elist[coord];
326 12 elist[coord]->prev[coord] = current->lo;
327 12 elist[coord] = current->lo;
328 } else {
329 2015 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
330 2015 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
331 2015 current->lo->prev[coord] = temp;
332 2015 current->lo->next[coord] = temp->next[coord];
333 2015 temp->next[coord]->prev[coord] = current->lo;
334 2015 temp->next[coord] = current->lo;
335 }
336 }
337
338 // Update the value of the lower bound along axis coord
339
2/4
✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
 2040 current->lo->getVal(coord) = new_min[coord];
340
341 // update hi end point
342
1/2
✓ Branch 0 taken 2040 times.
✗ Branch 1 not taken.
 2040 if (current->hi->prev[coord] != nullptr) {
343 2040 temp = current->hi->prev[coord];
344
345
7/10
✓ Branch 0 taken 11420 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 11420 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11420 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 9380 times.
✓ Branch 9 taken 2040 times.
✓ Branch 10 taken 9380 times.
✓ Branch 11 taken 2040 times.
 11420 while ((temp != nullptr) && (temp->getVal(coord) > new_max[coord])) {
346
4/4
✓ Branch 0 taken 4654 times.
✓ Branch 1 taken 4726 times.
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 9375 times.
 14034 if ((temp->minmax == 0) &&
347
3/4
✓ Branch 1 taken 4654 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 4649 times.
 4654 (temp->aabb->cached.overlap(current->cached)))
348
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
349 9380 temp = temp->prev[coord];
350 }
351
352 2040 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
353
2/2
✓ Branch 0 taken 2020 times.
✓ Branch 1 taken 20 times.
 2040 if (current->hi->next[coord] != nullptr)
354 2020 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
355 2040 current->hi->prev[coord] = temp; // Wrong line
356 2040 current->hi->next[coord] = temp->next[coord];
357
2/2
✓ Branch 0 taken 2028 times.
✓ Branch 1 taken 12 times.
 2040 if (temp->next[coord] != nullptr)
358 2028 temp->next[coord]->prev[coord] = current->hi;
359 2040 temp->next[coord] = current->hi;
360
361
2/4
✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
 2040 current->hi->getVal(coord) = new_max[coord];
362 }
363
364
2/4
✓ Branch 1 taken 2040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2040 times.
✗ Branch 5 not taken.
 2040 current->hi->getVal(coord) = new_max[coord];
365
1/2
✓ Branch 0 taken 1992 times.
✗ Branch 1 not taken.
 1992 } else if (direction == 1) {
366 // here, we first update the "hi" endpoint.
367
2/2
✓ Branch 0 taken 1974 times.
✓ Branch 1 taken 18 times.
 1992 if (current->hi->next[coord] != nullptr) {
368 1974 temp = current->hi->next[coord];
369
4/4
✓ Branch 0 taken 10624 times.
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 8674 times.
✓ Branch 3 taken 1974 times.
 21272 while ((temp->next[coord] != nullptr) &&
370
4/6
✓ Branch 1 taken 10624 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10624 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8674 times.
✓ Branch 7 taken 1950 times.
 10624 (temp->getVal(coord) < new_max[coord])) {
371
2/2
✓ Branch 0 taken 4353 times.
✓ Branch 1 taken 4321 times.
 8674 if (temp->minmax == 0)
372
3/4
✓ Branch 1 taken 4353 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 4346 times.
 4353 if (temp->aabb->cached.overlap(current_aabb))
373
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
374 8674 temp = temp->next[coord];
375 }
376
377
4/6
✓ Branch 1 taken 1974 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1974 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✓ Branch 7 taken 1961 times.
 1974 if (temp->getVal(coord) < new_max[coord]) {
378 13 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
379 13 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
380 13 current->hi->prev[coord] = temp;
381 13 current->hi->next[coord] = nullptr;
382 13 temp->next[coord] = current->hi;
383 } else {
384 1961 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
385 1961 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
386 1961 current->hi->prev[coord] = temp->prev[coord];
387 1961 current->hi->next[coord] = temp;
388 1961 temp->prev[coord]->next[coord] = current->hi;
389 1961 temp->prev[coord] = current->hi;
390 }
391 }
392
393
2/4
✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
 1992 current->hi->getVal(coord) = new_max[coord];
394
395 // then, update the "lo" endpoint of the interval.
396
1/2
✓ Branch 0 taken 1992 times.
✗ Branch 1 not taken.
 1992 if (current->lo->next[coord] != nullptr) {
397 1992 temp = current->lo->next[coord];
398
399
4/4
✓ Branch 0 taken 10578 times.
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 8600 times.
✓ Branch 3 taken 1992 times.
 21170 while ((temp->next[coord] != nullptr) &&
400
4/6
✓ Branch 1 taken 10578 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10578 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8600 times.
✓ Branch 7 taken 1978 times.
 10578 (temp->getVal(coord) < new_min[coord])) {
401
4/4
✓ Branch 0 taken 4360 times.
✓ Branch 1 taken 4240 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 8597 times.
 12960 if ((temp->minmax == 1) &&
402
3/4
✓ Branch 1 taken 4360 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4357 times.
 4360 (temp->aabb->cached.overlap(current->cached)))
403
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
404 8600 temp = temp->next[coord];
405 }
406
407
2/2
✓ Branch 0 taken 1969 times.
✓ Branch 1 taken 23 times.
 1992 if (current->lo->prev[coord] != nullptr)
408 1969 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
409 else
410 23 elist[coord] = current->lo->next[coord];
411 1992 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
412 1992 current->lo->prev[coord] = temp->prev[coord];
413 1992 current->lo->next[coord] = temp;
414
2/2
✓ Branch 0 taken 1974 times.
✓ Branch 1 taken 18 times.
 1992 if (temp->prev[coord] != nullptr)
415 1974 temp->prev[coord]->next[coord] = current->lo;
416 else
417 18 elist[coord] = current->lo;
418 1992 temp->prev[coord] = current->lo;
419
2/4
✓ Branch 1 taken 1992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1992 times.
✗ Branch 5 not taken.
 1992 current->lo->getVal(coord) = new_min[coord];
420 }
421 }
422 }
423 }
424
425 //==============================================================================
426 56 void SaPCollisionManager::updateVelist() {
427
2/2
✓ Branch 0 taken 168 times.
✓ Branch 1 taken 56 times.
 224 for (int coord = 0; coord < 3; ++coord) {
428 168 velist[coord].resize(size() * 2);
429 168 EndPoint* current = elist[coord];
430 168 size_t id = 0;
431
2/2
✓ Branch 0 taken 84090 times.
✓ Branch 1 taken 168 times.
 84258 while (current) {
432 84090 velist[coord][id] = current;
433 84090 current = current->next[coord];
434 84090 id++;
435 }
436 }
437 56 }
438
439 //==============================================================================
440 ✗ void SaPCollisionManager::update(CollisionObject* updated_obj) {
441 ✗ update_(obj_aabb_map[updated_obj]);
442
443 ✗ updateVelist();
444
445 ✗ setup();
446 }
447
448 //==============================================================================
449 ✗ void SaPCollisionManager::update(
450 const std::vector<CollisionObject*>& updated_objs) {
451 ✗ for (size_t i = 0; i < updated_objs.size(); ++i)
452 ✗ update_(obj_aabb_map[updated_objs[i]]);
453
454 ✗ updateVelist();
455
456 ✗ setup();
457 }
458
459 //==============================================================================
460 13 void SaPCollisionManager::update() {
461
2/2
✓ Branch 4 taken 1344 times.
✓ Branch 5 taken 13 times.
 1357 for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
462
1/2
✓ Branch 2 taken 1344 times.
✗ Branch 3 not taken.
 1344 update_(*it);
463 }
464
465 13 updateVelist();
466
467 13 setup();
468 13 }
469
470 //==============================================================================
471 50 void SaPCollisionManager::clear() {
472
2/2
✓ Branch 3 taken 12371 times.
✓ Branch 4 taken 50 times.
 12421 for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) {
473
1/2
✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
 12371 delete (*it)->hi;
474
1/2
✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
 12371 delete (*it)->lo;
475
1/2
✓ Branch 1 taken 12371 times.
✗ Branch 2 not taken.
 12371 delete *it;
476 12371 *it = nullptr;
477 }
478
479 50 AABB_arr.clear();
480 50 overlap_pairs.clear();
481
482 50 elist[0] = nullptr;
483 50 elist[1] = nullptr;
484 50 elist[2] = nullptr;
485
486 50 velist[0].clear();
487 50 velist[1].clear();
488 50 velist[2].clear();
489
490 50 obj_aabb_map.clear();
491 50 }
492
493 //==============================================================================
494 ✗ void SaPCollisionManager::getObjects(
495 std::vector<CollisionObject*>& objs) const {
496 ✗ objs.resize(AABB_arr.size());
497 ✗ size_t i = 0;
498 ✗ for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end;
499 ✗ ++it, ++i) {
500 ✗ objs[i] = (*it)->obj;
501 }
502 }
503
504 //==============================================================================
505 3762 bool SaPCollisionManager::collide_(CollisionObject* obj,
506 CollisionCallBackBase* callback) const {
507 3762 int axis = optimal_axis;
508 3762 const AABB& obj_aabb = obj->getAABB();
509
510
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 Scalar min_val = obj_aabb.min_[axis];
511 // Scalar max_val = obj_aabb.max_[axis];
512
513 EndPoint dummy;
514
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 SaPAABB dummy_aabb;
515
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 dummy_aabb.cached = obj_aabb;
516 3762 dummy.minmax = 1;
517 3762 dummy.aabb = &dummy_aabb;
518
519 // compute stop_pos by binary search, this is cheaper than check it in while
520 // iteration linearly
521
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 const auto res_it = std::upper_bound(
522 3762 velist[axis].begin(), velist[axis].end(), &dummy,
523
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 std::bind(std::less<Scalar>(),
524
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
525 &EndPoint::getVal),
526 std::placeholders::_1, axis),
527
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
528 &EndPoint::getVal),
529 std::placeholders::_2, axis)));
530
531 3762 EndPoint* end_pos = nullptr;
532
2/2
✓ Branch 2 taken 3721 times.
✓ Branch 3 taken 41 times.
 3762 if (res_it != velist[axis].end()) end_pos = *res_it;
533
534 3762 EndPoint* pos = elist[axis];
535
536
2/2
✓ Branch 0 taken 599528 times.
✓ Branch 1 taken 3761 times.
 603289 while (pos != end_pos) {
537
1/2
✓ Branch 0 taken 599528 times.
✗ Branch 1 not taken.
 599528 if (pos->aabb->obj != obj) {
538
7/8
✓ Branch 0 taken 303272 times.
✓ Branch 1 taken 296256 times.
✓ Branch 3 taken 303272 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 14014 times.
✓ Branch 6 taken 289258 times.
✓ Branch 7 taken 14014 times.
✓ Branch 8 taken 585514 times.
 599528 if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) {
539
3/4
✓ Branch 2 taken 14014 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 19 times.
✓ Branch 5 taken 13995 times.
 14014 if (pos->aabb->cached.overlap(obj->getAABB()))
540
3/4
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 18 times.
 19 if ((*callback)(obj, pos->aabb->obj)) return true;
541 }
542 }
543 599527 pos = pos->next[axis];
544 }
545
546 3761 return false;
547 }
548
549 //==============================================================================
550 11 void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) {
551 11 bool repeated = false;
552
2/2
✓ Branch 3 taken 18 times.
✓ Branch 4 taken 8 times.
 26 for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end;
553 15 ++it) {
554
3/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 15 times.
 18 if (*it == p) {
555 3 repeated = true;
556 3 break;
557 }
558 }
559
560
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3 times.
 11 if (!repeated) overlap_pairs.push_back(p);
561 11 }
562
563 //==============================================================================
564 8 void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) {
565
2/2
✓ Branch 3 taken 12 times.
✓ Branch 4 taken 2 times.
 14 for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end;
566 6 ++it) {
567
3/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 6 times.
 12 if (*it == p) {
568 6 overlap_pairs.erase(it);
569 6 break;
570 }
571 }
572
573 // or overlap_pairs.remove_if(isNotValidPair(p));
574 8 }
575
576 //==============================================================================
577 3822 void SaPCollisionManager::collide(CollisionObject* obj,
578 CollisionCallBackBase* callback) const {
579 COAL_TRACY_ZONE_SCOPED_N(
580 "coal::SaPCollisionManager::collide(CollisionObject*, "
581 "CollisionCallBackBase*)");
582 3822 callback->init();
583
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
 3822 if (size() == 0) return;
584
585 3762 collide_(obj, callback);
586 }
587
588 //==============================================================================
589 3070 bool SaPCollisionManager::distance_(CollisionObject* obj,
590 DistanceCallBackBase* callback,
591 Scalar& min_dist) const {
592
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;
593
1/2
✓ Branch 2 taken 3070 times.
✗ Branch 3 not taken.
 3070 AABB aabb = obj->getAABB();
594
595
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 86 times.
 3070 if (min_dist < (std::numeric_limits<Scalar>::max)()) {
596
1/2
✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
 2984 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
597
1/2
✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
 2984 aabb.expand(min_dist_delta);
598 }
599
600 3070 int axis = optimal_axis;
601
602 3070 int status = 1;
603 Scalar old_min_distance;
604
605 3070 EndPoint* start_pos = elist[axis];
606
607 while (1) {
608 3200 old_min_distance = min_dist;
609
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 Scalar min_val = aabb.min_[axis];
610 // Scalar max_val = aabb.max_[axis];
611
612 EndPoint dummy;
613
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 SaPAABB dummy_aabb;
614
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 dummy_aabb.cached = aabb;
615 3200 dummy.minmax = 1;
616 3200 dummy.aabb = &dummy_aabb;
617
618
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 const auto res_it = std::upper_bound(
619 3200 velist[axis].begin(), velist[axis].end(), &dummy,
620
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 std::bind(std::less<Scalar>(),
621
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
622 &EndPoint::getVal),
623 std::placeholders::_1, axis),
624
1/2
✓ Branch 1 taken 3200 times.
✗ Branch 2 not taken.
 3200 std::bind(static_cast<Scalar (EndPoint::*)(int) const>(
625 &EndPoint::getVal),
626 std::placeholders::_2, axis)));
627
628 3200 EndPoint* end_pos = nullptr;
629
2/2
✓ Branch 2 taken 3180 times.
✓ Branch 3 taken 20 times.
 3200 if (res_it != velist[axis].end()) end_pos = *res_it;
630
631 3200 EndPoint* pos = start_pos;
632
633
2/2
✓ Branch 0 taken 3259389 times.
✓ Branch 1 taken 3200 times.
 3262589 while (pos != end_pos) {
634 // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and
635 // then update start_pos to end_pos. but this seems slower.
636
7/8
✓ Branch 0 taken 1635306 times.
✓ Branch 1 taken 1624083 times.
✓ Branch 3 taken 1635306 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 28806 times.
✓ Branch 6 taken 1606500 times.
✓ Branch 7 taken 28806 times.
✓ Branch 8 taken 3230583 times.
 3259389 if ((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) {
637 28806 CollisionObject* curr_obj = pos->aabb->obj;
638
2/2
✓ Branch 0 taken 25776 times.
✓ Branch 1 taken 3030 times.
 28806 if (curr_obj != obj) {
639
2/2
✓ Branch 0 taken 21284 times.
✓ Branch 1 taken 4492 times.
 25776 if (!this->enable_tested_set_) {
640
3/4
✓ Branch 2 taken 21284 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1399 times.
✓ Branch 5 taken 19885 times.
 21284 if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) {
641
2/4
✓ Branch 1 taken 1399 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1399 times.
 1399 if ((*callback)(curr_obj, obj, min_dist)) return true;
642 }
643 } else {
644
3/4
✓ Branch 1 taken 4492 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2240 times.
✓ Branch 4 taken 2252 times.
 4492 if (!this->inTestedSet(curr_obj, obj)) {
645
3/4
✓ Branch 2 taken 2240 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 243 times.
✓ Branch 5 taken 1997 times.
 2240 if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) {
646
2/4
✓ Branch 1 taken 243 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 243 times.
 243 if ((*callback)(curr_obj, obj, min_dist)) return true;
647 }
648
649
1/2
✓ Branch 1 taken 2240 times.
✗ Branch 2 not taken.
 2240 this->insertTestedSet(curr_obj, obj);
650 }
651 }
652 }
653 }
654
655 3259389 pos = pos->next[axis];
656 }
657
658
2/2
✓ Branch 0 taken 3114 times.
✓ Branch 1 taken 86 times.
 3200 if (status == 1) {
659
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 130 times.
 3114 if (old_min_distance < (std::numeric_limits<Scalar>::max)())
660 2984 break;
661 else {
662
2/2
✓ Branch 0 taken 86 times.
✓ Branch 1 taken 44 times.
 130 if (min_dist < old_min_distance) {
663
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
664
2/4
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 86 times.
✗ Branch 6 not taken.
 86 aabb = AABB(obj->getAABB(), min_dist_delta);
665 86 status = 0;
666 } else {
667
3/4
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 35 times.
 44 if (aabb == obj->getAABB())
668
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 aabb.expand(delta);
669 else
670
1/2
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
 35 aabb.expand(obj->getAABB(), 2.0);
671 }
672 }
673
1/2
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
 86 } else if (status == 0)
674 86 break;
675 130 }
676
677 3070 return false;
678 }
679
680 //==============================================================================
681 80 void SaPCollisionManager::distance(CollisionObject* obj,
682 DistanceCallBackBase* callback) const {
683 COAL_TRACY_ZONE_SCOPED_N(
684 "coal::SaPCollisionManager::distance(CollisionObject*, "
685 "DistanceCallBackBase*)");
686
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 callback->init();
687
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;
688
689 80 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
690
691
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 distance_(obj, callback, min_dist);
692 }
693
694 //==============================================================================
695 37 void SaPCollisionManager::collide(CollisionCallBackBase* callback) const {
696 COAL_TRACY_ZONE_SCOPED_N(
697 "coal::SaPCollisionManager::collide(CollisionCallBackBase*)");
698 37 callback->init();
699
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 29 times.
 37 if (size() == 0) return;
700
701
2/2
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 26 times.
 33 for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end;
702 4 ++it) {
703 7 CollisionObject* obj1 = it->obj1;
704 7 CollisionObject* obj2 = it->obj2;
705
706
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4 times.
 7 if ((*callback)(obj1, obj2)) return;
707 }
708 }
709
710 //==============================================================================
711 6 void SaPCollisionManager::distance(DistanceCallBackBase* callback) const {
712 COAL_TRACY_ZONE_SCOPED_N(
713 "coal::SaPCollisionManager::distance(DistanceCallBackBase*)");
714
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 callback->init();
715
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;
716
717 6 this->enable_tested_set_ = true;
718 6 this->tested_set.clear();
719
720 6 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
721
722
2/2
✓ Branch 4 taken 2990 times.
✓ Branch 5 taken 6 times.
 2996 for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
723
2/4
✓ Branch 2 taken 2990 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2990 times.
 2990 if (distance_((*it)->obj, callback, min_dist)) break;
724 }
725
726 6 this->enable_tested_set_ = false;
727 6 this->tested_set.clear();
728 }
729
730 //==============================================================================
731 ✗ void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
732 CollisionCallBackBase* callback) const {
733 COAL_TRACY_ZONE_SCOPED_N(
734 "coal::SaPCollisionManager::collide(BroadPhaseCollisionManager*, "
735 "CollisionCallBackBase*)");
736 ✗ callback->init();
737 ✗ SaPCollisionManager* other_manager =
738 static_cast<SaPCollisionManager*>(other_manager_);
739
740 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
741
742 ✗ if (this == other_manager) {
743 ✗ collide(callback);
744 ✗ return;
745 }
746
747 ✗ if (this->size() < other_manager->size()) {
748 ✗ for (auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) {
749 ✗ if (other_manager->collide_((*it)->obj, callback)) return;
750 }
751 } else {
752 ✗ for (auto it = other_manager->AABB_arr.cbegin(),
753 ✗ end = other_manager->AABB_arr.cend();
754 ✗ it != end; ++it) {
755 ✗ if (collide_((*it)->obj, callback)) return;
756 }
757 }
758 }
759
760 //==============================================================================
761 ✗ void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
762 DistanceCallBackBase* callback) const {
763 COAL_TRACY_ZONE_SCOPED_N(
764 "coal::SaPCollisionManager::distance(BroadPhaseCollisionManager*, "
765 "DistanceCallBackBase*)");
766 ✗ callback->init();
767 ✗ SaPCollisionManager* other_manager =
768 static_cast<SaPCollisionManager*>(other_manager_);
769
770 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
771
772 ✗ if (this == other_manager) {
773 ✗ distance(callback);
774 ✗ return;
775 }
776
777 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
778
779 ✗ if (this->size() < other_manager->size()) {
780 ✗ for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
781 ✗ if (other_manager->distance_((*it)->obj, callback, min_dist)) return;
782 }
783 } else {
784 ✗ for (auto it = other_manager->AABB_arr.cbegin(),
785 ✗ end = other_manager->AABB_arr.cend();
786 ✗ it != end; ++it) {
787 ✗ if (distance_((*it)->obj, callback, min_dist)) return;
788 }
789 }
790 }
791
792 //==============================================================================
793 ✗ bool SaPCollisionManager::empty() const { return AABB_arr.empty(); }
794
795 //==============================================================================
796 4220 size_t SaPCollisionManager::size() const { return AABB_arr.size(); }
797
798 //==============================================================================
799 ✗ const Vec3s& SaPCollisionManager::EndPoint::getVal() const {
800 ✗ if (minmax)
801 ✗ return aabb->cached.max_;
802 else
803 ✗ return aabb->cached.min_;
804 }
805
806 //==============================================================================
807 ✗ Vec3s& SaPCollisionManager::EndPoint::getVal() {
808 ✗ if (minmax)
809 ✗ return aabb->cached.max_;
810 else
811 ✗ return aabb->cached.min_;
812 }
813
814 //==============================================================================
815 2109752 Scalar SaPCollisionManager::EndPoint::getVal(int i) const {
816
2/2
✓ Branch 0 taken 1081916 times.
✓ Branch 1 taken 1027836 times.
 2109752 if (minmax)
817 1081916 return aabb->cached.max_[i];
818 else
819 1027836 return aabb->cached.min_[i];
820 }
821
822 //==============================================================================
823 2000983 Scalar& SaPCollisionManager::EndPoint::getVal(int i) {
824
2/2
✓ Branch 0 taken 1967773 times.
✓ Branch 1 taken 33210 times.
 2000983 if (minmax)
825 1967773 return aabb->cached.max_[i];
826 else
827 33210 return aabb->cached.min_[i];
828 }
829
830 //==============================================================================
831 78474 SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) {
832
2/2
✓ Branch 0 taken 37794 times.
✓ Branch 1 taken 40680 times.
 78474 if (a < b) {
833 37794 obj1 = a;
834 37794 obj2 = b;
835 } else {
836 40680 obj1 = b;
837 40680 obj2 = a;
838 }
839 78474 }
840
841 //==============================================================================
842 30 bool SaPCollisionManager::SaPPair::operator==(const SaPPair& other) const {
843
3/4
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 30 return ((obj1 == other.obj1) && (obj2 == other.obj2));
844 }
845
846 //==============================================================================
847 ✗ SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_)
848 ✗ : obj(obj_) {}
849
850 //==============================================================================
851 ✗ bool SaPCollisionManager::isUnregistered::operator()(
852 const SaPPair& pair) const {
853 ✗ return (pair.obj1 == obj) || (pair.obj2 == obj);
854 }
855
856 //==============================================================================
857 ✗ SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_,
858 ✗ CollisionObject* obj2_)
859 ✗ : obj1(obj1_), obj2(obj2_) {
860 // Do nothing
861 }
862
863 //==============================================================================
864 ✗ bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) {
865 ✗ return (pair.obj1 == obj1) && (pair.obj2 == obj2);
866 }
867
868 } // namespace coal
869