GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_interval_tree.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 220 360 61.1%
Branches: 222 559 39.7%
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_interval_tree.h"
39
40 namespace coal {
41
42 //==============================================================================
43 ✗ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) {
44 // must sorted before
45 ✗ setup();
46
47 EndPoint p;
48 ✗ p.value = obj->getAABB().min_[0];
49 ✗ auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p);
50 ✗ p.value = obj->getAABB().max_[0];
51 ✗ auto end1 = std::upper_bound(start1, endpoints[0].end(), p);
52
53 ✗ if (start1 < end1) {
54 ✗ size_t start_id = (size_t)(start1 - endpoints[0].begin());
55 ✗ size_t end_id = (size_t)(end1 - endpoints[0].begin());
56 ✗ size_t cur_id = (size_t)(start_id);
57
58 ✗ for (size_t i = start_id; i < end_id; ++i) {
59 ✗ if (endpoints[0][(size_t)i].obj != obj) {
60 ✗ if (i == cur_id)
61 ✗ cur_id++;
62 else {
63 ✗ endpoints[0][(size_t)cur_id] = endpoints[0][(size_t)i];
64 ✗ cur_id++;
65 }
66 }
67 }
68 ✗ if (cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2);
69 }
70
71 ✗ p.value = obj->getAABB().min_[1];
72 ✗ auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p);
73 ✗ p.value = obj->getAABB().max_[1];
74 ✗ auto end2 = std::upper_bound(start2, endpoints[1].end(), p);
75
76 ✗ if (start2 < end2) {
77 ✗ size_t start_id = (size_t)(start2 - endpoints[1].begin());
78 ✗ size_t end_id = (size_t)(end2 - endpoints[1].begin());
79 ✗ size_t cur_id = (size_t)(start_id);
80
81 ✗ for (size_t i = start_id; i < end_id; ++i) {
82 ✗ if (endpoints[1][i].obj != obj) {
83 ✗ if (i == cur_id)
84 ✗ cur_id++;
85 else {
86 ✗ endpoints[1][cur_id] = endpoints[1][i];
87 ✗ cur_id++;
88 }
89 }
90 }
91 ✗ if (cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2);
92 }
93
94 ✗ p.value = obj->getAABB().min_[2];
95 ✗ auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p);
96 ✗ p.value = obj->getAABB().max_[2];
97 ✗ auto end3 = std::upper_bound(start3, endpoints[2].end(), p);
98
99 ✗ if (start3 < end3) {
100 ✗ size_t start_id = (size_t)(start3 - endpoints[2].begin());
101 ✗ size_t end_id = (size_t)(end3 - endpoints[2].begin());
102 ✗ size_t cur_id = (size_t)(start_id);
103
104 ✗ for (size_t i = start_id; i < end_id; ++i) {
105 ✗ if (endpoints[2][i].obj != obj) {
106 ✗ if (i == cur_id)
107 ✗ cur_id++;
108 else {
109 ✗ endpoints[2][cur_id] = endpoints[2][i];
110 ✗ cur_id++;
111 }
112 }
113 }
114 ✗ if (cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2);
115 }
116
117 // update the interval tree
118 ✗ if (obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) {
119 ✗ SAPInterval* ivl1 = obj_interval_maps[0][obj];
120 ✗ SAPInterval* ivl2 = obj_interval_maps[1][obj];
121 ✗ SAPInterval* ivl3 = obj_interval_maps[2][obj];
122
123 ✗ interval_trees[0]->deleteNode(ivl1);
124 ✗ interval_trees[1]->deleteNode(ivl2);
125 ✗ interval_trees[2]->deleteNode(ivl3);
126
127 ✗ delete ivl1;
128 ✗ delete ivl2;
129 ✗ delete ivl3;
130
131 ✗ obj_interval_maps[0].erase(obj);
132 ✗ obj_interval_maps[1].erase(obj);
133 ✗ obj_interval_maps[2].erase(obj);
134 }
135 }
136
137 //==============================================================================
138
4/4
✓ Branch 2 taken 153 times.
✓ Branch 3 taken 51 times.
✓ Branch 5 taken 153 times.
✓ Branch 6 taken 51 times.
 357 IntervalTreeCollisionManager::IntervalTreeCollisionManager() : setup_(false) {
139
2/2
✓ Branch 0 taken 153 times.
✓ Branch 1 taken 51 times.
 204 for (int i = 0; i < 3; ++i) interval_trees[i] = nullptr;
140 51 }
141
142 //==============================================================================
143
6/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 150 times.
✓ Branch 4 taken 50 times.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 150 times.
✓ Branch 9 taken 50 times.
 800 IntervalTreeCollisionManager::~IntervalTreeCollisionManager() { clear(); }
144
145 //==============================================================================
146 12671 void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) {
147 EndPoint p, q;
148
149 12671 p.obj = obj;
150 12671 q.obj = obj;
151 12671 p.minmax = 0;
152 12671 q.minmax = 1;
153
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 p.value = obj->getAABB().min_[0];
154
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 q.value = obj->getAABB().max_[0];
155
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[0].push_back(p);
156
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[0].push_back(q);
157
158
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 p.value = obj->getAABB().min_[1];
159
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 q.value = obj->getAABB().max_[1];
160
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[1].push_back(p);
161
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[1].push_back(q);
162
163
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 p.value = obj->getAABB().min_[2];
164
1/2
✓ Branch 2 taken 12671 times.
✗ Branch 3 not taken.
 12671 q.value = obj->getAABB().max_[2];
165
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[2].push_back(p);
166
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 endpoints[2].push_back(q);
167 12671 setup_ = false;
168 12671 }
169
170 //==============================================================================
171 64 void IntervalTreeCollisionManager::setup() {
172
1/2
✓ Branch 0 taken 64 times.
✗ Branch 1 not taken.
 64 if (!setup_) {
173 64 std::sort(endpoints[0].begin(), endpoints[0].end());
174 64 std::sort(endpoints[1].begin(), endpoints[1].end());
175 64 std::sort(endpoints[2].begin(), endpoints[2].end());
176
177
4/4
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 153 times.
✓ Branch 4 taken 192 times.
✓ Branch 5 taken 64 times.
 256 for (int i = 0; i < 3; ++i) delete interval_trees[i];
178
179
3/4
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 192 times.
✓ Branch 5 taken 64 times.
 256 for (int i = 0; i < 3; ++i) interval_trees[i] = new detail::IntervalTree;
180
181
2/2
✓ Branch 1 taken 28030 times.
✓ Branch 2 taken 64 times.
 28094 for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
182 28030 EndPoint p = endpoints[0][i];
183 28030 CollisionObject* obj = p.obj;
184
2/2
✓ Branch 0 taken 14015 times.
✓ Branch 1 taken 14015 times.
 28030 if (p.minmax == 0) {
185
1/2
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
 14015 SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0],
186
3/6
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
 14015 obj->getAABB().max_[0], obj);
187
1/2
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
 14015 SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1],
188
3/6
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
 14015 obj->getAABB().max_[1], obj);
189
1/2
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
 14015 SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2],
190
3/6
✓ Branch 2 taken 14015 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14015 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14015 times.
✗ Branch 9 not taken.
 14015 obj->getAABB().max_[2], obj);
191
192
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 interval_trees[0]->insert(ivl1);
193
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 interval_trees[1]->insert(ivl2);
194
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 interval_trees[2]->insert(ivl3);
195
196
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 obj_interval_maps[0][obj] = ivl1;
197
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 obj_interval_maps[1][obj] = ivl2;
198
1/2
✓ Branch 1 taken 14015 times.
✗ Branch 2 not taken.
 14015 obj_interval_maps[2][obj] = ivl3;
199 }
200 }
201
202 64 setup_ = true;
203 }
204 64 }
205
206 //==============================================================================
207 13 void IntervalTreeCollisionManager::update() {
208 13 setup_ = false;
209
210
2/2
✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
 2701 for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
211
2/2
✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
 2688 if (endpoints[0][i].minmax == 0)
212 1344 endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
213 else
214 1344 endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
215 }
216
217
2/2
✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
 2701 for (size_t i = 0, size = endpoints[1].size(); i < size; ++i) {
218
2/2
✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
 2688 if (endpoints[1][i].minmax == 0)
219 1344 endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
220 else
221 1344 endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
222 }
223
224
2/2
✓ Branch 1 taken 2688 times.
✓ Branch 2 taken 13 times.
 2701 for (size_t i = 0, size = endpoints[2].size(); i < size; ++i) {
225
2/2
✓ Branch 1 taken 1344 times.
✓ Branch 2 taken 1344 times.
 2688 if (endpoints[2][i].minmax == 0)
226 1344 endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
227 else
228 1344 endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
229 }
230
231 13 setup();
232 13 }
233
234 //==============================================================================
235 ✗ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) {
236 ✗ AABB old_aabb;
237 ✗ const AABB& new_aabb = updated_obj->getAABB();
238 ✗ for (int i = 0; i < 3; ++i) {
239 ✗ const auto it = obj_interval_maps[i].find(updated_obj);
240 ✗ interval_trees[i]->deleteNode(it->second);
241 ✗ old_aabb.min_[i] = it->second->low;
242 ✗ old_aabb.max_[i] = it->second->high;
243 ✗ it->second->low = new_aabb.min_[i];
244 ✗ it->second->high = new_aabb.max_[i];
245 ✗ interval_trees[i]->insert(it->second);
246 }
247
248 EndPoint dummy;
249 ✗ typename std::vector<EndPoint>::iterator it;
250 ✗ for (int i = 0; i < 3; ++i) {
251 ✗ dummy.value = old_aabb.min_[i];
252 ✗ it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
253 ✗ for (; it != endpoints[i].end(); ++it) {
254 ✗ if (it->obj == updated_obj && it->minmax == 0) {
255 ✗ it->value = new_aabb.min_[i];
256 ✗ break;
257 }
258 }
259
260 ✗ dummy.value = old_aabb.max_[i];
261 ✗ it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
262 ✗ for (; it != endpoints[i].end(); ++it) {
263 ✗ if (it->obj == updated_obj && it->minmax == 0) {
264 ✗ it->value = new_aabb.max_[i];
265 ✗ break;
266 }
267 }
268
269 ✗ std::sort(endpoints[i].begin(), endpoints[i].end());
270 }
271 }
272
273 //==============================================================================
274 ✗ void IntervalTreeCollisionManager::update(
275 const std::vector<CollisionObject*>& updated_objs) {
276 ✗ for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
277 }
278
279 //==============================================================================
280 50 void IntervalTreeCollisionManager::clear() {
281 50 endpoints[0].clear();
282 50 endpoints[1].clear();
283 50 endpoints[2].clear();
284
285
1/2
✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
 50 delete interval_trees[0];
286 50 interval_trees[0] = nullptr;
287
1/2
✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
 50 delete interval_trees[1];
288 50 interval_trees[1] = nullptr;
289
1/2
✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
 50 delete interval_trees[2];
290 50 interval_trees[2] = nullptr;
291
292
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 50 times.
 200 for (int i = 0; i < 3; ++i) {
293 300 for (auto it = obj_interval_maps[i].cbegin(),
294 150 end = obj_interval_maps[i].cend();
295
2/2
✓ Branch 2 taken 37113 times.
✓ Branch 3 taken 150 times.
 37263 it != end; ++it) {
296
1/2
✓ Branch 1 taken 37113 times.
✗ Branch 2 not taken.
 37113 delete it->second;
297 }
298 }
299
300
2/2
✓ Branch 1 taken 150 times.
✓ Branch 2 taken 50 times.
 200 for (int i = 0; i < 3; ++i) obj_interval_maps[i].clear();
301
302 50 setup_ = false;
303 50 }
304
305 //==============================================================================
306 ✗ void IntervalTreeCollisionManager::getObjects(
307 std::vector<CollisionObject*>& objs) const {
308 ✗ objs.resize(endpoints[0].size() / 2);
309 ✗ size_t j = 0;
310 ✗ for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
311 ✗ if (endpoints[0][i].minmax == 0) {
312 ✗ objs[j] = endpoints[0][i].obj;
313 ✗ j++;
314 }
315 }
316 }
317
318 //==============================================================================
319 3822 void IntervalTreeCollisionManager::collide(
320 CollisionObject* obj, CollisionCallBackBase* callback) const {
321 3822 callback->init();
322
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
 3822 if (size() == 0) return;
323 3762 collide_(obj, callback);
324 }
325
326 //==============================================================================
327 3762 bool IntervalTreeCollisionManager::collide_(
328 CollisionObject* obj, CollisionCallBackBase* callback) const {
329 static const unsigned int CUTOFF = 100;
330
331
3/6
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3762 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3762 times.
✗ Branch 8 not taken.
 3762 std::deque<detail::SimpleInterval*> results0, results1, results2;
332
333 results0 =
334
3/6
✓ Branch 2 taken 3762 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3762 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3762 times.
✗ Branch 10 not taken.
 3762 interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
335
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3762 times.
 3762 if (results0.size() > CUTOFF) {
336 ✗ results1 = interval_trees[1]->query(obj->getAABB().min_[1],
337 ✗ obj->getAABB().max_[1]);
338 ✗ if (results1.size() > CUTOFF) {
339 ✗ results2 = interval_trees[2]->query(obj->getAABB().min_[2],
340 ✗ obj->getAABB().max_[2]);
341 ✗ if (results2.size() > CUTOFF) {
342 ✗ size_t d1 = results0.size();
343 ✗ size_t d2 = results1.size();
344 ✗ size_t d3 = results2.size();
345
346 ✗ if (d1 >= d2 && d1 >= d3)
347 ✗ return checkColl(results0.begin(), results0.end(), obj, callback);
348 ✗ else if (d2 >= d1 && d2 >= d3)
349 ✗ return checkColl(results1.begin(), results1.end(), obj, callback);
350 else
351 ✗ return checkColl(results2.begin(), results2.end(), obj, callback);
352 } else
353 ✗ return checkColl(results2.begin(), results2.end(), obj, callback);
354 } else
355 ✗ return checkColl(results1.begin(), results1.end(), obj, callback);
356 } else
357
1/2
✓ Branch 5 taken 3762 times.
✗ Branch 6 not taken.
 3762 return checkColl(results0.begin(), results0.end(), obj, callback);
358 3762 }
359
360 //==============================================================================
361 80 void IntervalTreeCollisionManager::distance(
362 CollisionObject* obj, DistanceCallBackBase* callback) const {
363
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 callback->init();
364
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;
365 80 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
366
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 distance_(obj, callback, min_dist);
367 }
368
369 //==============================================================================
370 6060 bool IntervalTreeCollisionManager::distance_(CollisionObject* obj,
371 DistanceCallBackBase* callback,
372 Scalar& min_dist) const {
373 static const unsigned int CUTOFF = 100;
374
375
3/6
✓ Branch 3 taken 6060 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6060 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6060 times.
✗ Branch 10 not taken.
 6060 Vec3s delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
376
1/2
✓ Branch 2 taken 6060 times.
✗ Branch 3 not taken.
 6060 AABB aabb = obj->getAABB();
377
2/2
✓ Branch 1 taken 5974 times.
✓ Branch 2 taken 86 times.
 6060 if (min_dist < (std::numeric_limits<Scalar>::max)()) {
378
1/2
✓ Branch 1 taken 5974 times.
✗ Branch 2 not taken.
 5974 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
379
1/2
✓ Branch 1 taken 5974 times.
✗ Branch 2 not taken.
 5974 aabb.expand(min_dist_delta);
380 }
381
382 6060 int status = 1;
383 Scalar old_min_distance;
384
385 while (1) {
386 6156 bool dist_res = false;
387
388 6156 old_min_distance = min_dist;
389
390
3/6
✓ Branch 1 taken 6156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6156 times.
✗ Branch 8 not taken.
 6156 std::deque<detail::SimpleInterval*> results0, results1, results2;
391
392
3/6
✓ Branch 1 taken 6156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6156 times.
✗ Branch 8 not taken.
 6156 results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
393
2/2
✓ Branch 1 taken 4743 times.
✓ Branch 2 taken 1413 times.
 6156 if (results0.size() > CUTOFF) {
394
3/6
✓ Branch 1 taken 4743 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4743 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4743 times.
✗ Branch 8 not taken.
 4743 results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
395
2/2
✓ Branch 1 taken 4737 times.
✓ Branch 2 taken 6 times.
 4743 if (results1.size() > CUTOFF) {
396
3/6
✓ Branch 1 taken 4737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4737 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4737 times.
✗ Branch 8 not taken.
 4737 results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
397
2/2
✓ Branch 1 taken 43 times.
✓ Branch 2 taken 4694 times.
 4737 if (results2.size() > CUTOFF) {
398 43 size_t d1 = results0.size();
399 43 size_t d2 = results1.size();
400 43 size_t d3 = results2.size();
401
402
4/4
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 17 times.
✓ Branch 3 taken 3 times.
 43 if (d1 >= d2 && d1 >= d3)
403
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 dist_res = checkDist(results0.begin(), results0.end(), obj,
404 callback, min_dist);
405
4/4
✓ Branch 0 taken 23 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 11 times.
 26 else if (d2 >= d1 && d2 >= d3)
406
1/2
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 12 dist_res = checkDist(results1.begin(), results1.end(), obj,
407 callback, min_dist);
408 else
409
1/2
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 dist_res = checkDist(results2.begin(), results2.end(), obj,
410 callback, min_dist);
411 } else
412
1/2
✓ Branch 5 taken 4694 times.
✗ Branch 6 not taken.
 4694 dist_res = checkDist(results2.begin(), results2.end(), obj, callback,
413 min_dist);
414 } else
415
1/2
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 dist_res = checkDist(results1.begin(), results1.end(), obj, callback,
416 min_dist);
417 } else
418 dist_res =
419
1/2
✓ Branch 5 taken 1413 times.
✗ Branch 6 not taken.
 1413 checkDist(results0.begin(), results0.end(), obj, callback, min_dist);
420
421
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6156 times.
 6156 if (dist_res) return true;
422
423 6156 results0.clear();
424 6156 results1.clear();
425 6156 results2.clear();
426
427
2/2
✓ Branch 0 taken 6070 times.
✓ Branch 1 taken 86 times.
 6156 if (status == 1) {
428
2/2
✓ Branch 1 taken 5974 times.
✓ Branch 2 taken 96 times.
 6070 if (old_min_distance < (std::numeric_limits<Scalar>::max)())
429 5974 break;
430 else {
431
2/2
✓ Branch 0 taken 86 times.
✓ Branch 1 taken 10 times.
 96 if (min_dist < old_min_distance) {
432
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
433
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);
434 86 status = 0;
435 } else {
436
3/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 7 times.
 10 if (aabb == obj->getAABB())
437
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 aabb.expand(delta);
438 else
439
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 aabb.expand(obj->getAABB(), 2.0);
440 }
441 }
442
1/2
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
 86 } else if (status == 0)
443 86 break;
444
6/9
✓ Branch 1 taken 96 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6060 times.
✓ Branch 5 taken 96 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6060 times.
✓ Branch 9 taken 96 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6060 times.
 18372 }
445
446 6060 return false;
447 }
448
449 //==============================================================================
450 37 void IntervalTreeCollisionManager::collide(
451 CollisionCallBackBase* callback) const {
452
1/2
✓ Branch 1 taken 37 times.
✗ Branch 2 not taken.
 37 callback->init();
453
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;
454
455 29 std::set<CollisionObject*> active;
456 29 std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
457 29 size_t n = endpoints[0].size();
458 29 Scalar diff_x = endpoints[0][0].value - endpoints[0][n - 1].value;
459 29 Scalar diff_y = endpoints[1][0].value - endpoints[1][n - 1].value;
460 29 Scalar diff_z = endpoints[2][0].value - endpoints[2][n - 1].value;
461
462 29 int axis = 0;
463
4/4
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 5 times.
 29 if (diff_y > diff_x && diff_y > diff_z)
464 9 axis = 1;
465
4/4
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 4 times.
 20 else if (diff_z > diff_y && diff_z > diff_x)
466 8 axis = 2;
467
468
2/2
✓ Branch 0 taken 3958 times.
✓ Branch 1 taken 26 times.
 3984 for (unsigned int i = 0; i < n; ++i) {
469 3958 const EndPoint& endpoint = endpoints[axis][i];
470 3958 CollisionObject* index = endpoint.obj;
471
2/2
✓ Branch 0 taken 1989 times.
✓ Branch 1 taken 1969 times.
 3958 if (endpoint.minmax == 0) {
472 1989 auto iter = active.begin();
473 1989 auto end = active.end();
474
2/2
✓ Branch 2 taken 6062 times.
✓ Branch 3 taken 1986 times.
 8048 for (; iter != end; ++iter) {
475 6062 CollisionObject* active_index = *iter;
476 6062 const AABB& b0 = active_index->getAABB();
477 6062 const AABB& b1 = index->getAABB();
478
479 6062 int axis2 = (axis + 1) % 3;
480 6062 int axis3 = (axis + 2) % 3;
481
482
8/10
✓ Branch 1 taken 6062 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 186 times.
✓ Branch 4 taken 5876 times.
✓ Branch 6 taken 186 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7 times.
✓ Branch 9 taken 179 times.
✓ Branch 10 taken 7 times.
✓ Branch 11 taken 6055 times.
 6062 if (b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) {
483 std::pair<typename std::set<std::pair<CollisionObject*,
484 CollisionObject*> >::iterator,
485 bool>
486 7 insert_res;
487
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 6 times.
 7 if (active_index < index)
488
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 insert_res = overlap.insert(std::make_pair(active_index, index));
489 else
490
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 insert_res = overlap.insert(std::make_pair(index, active_index));
491
492
1/2
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
 7 if (insert_res.second) {
493
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 4 times.
 7 if ((*callback)(active_index, index)) return;
494 }
495 }
496 }
497
1/2
✓ Branch 1 taken 1986 times.
✗ Branch 2 not taken.
 1986 active.insert(index);
498 } else
499
1/2
✓ Branch 1 taken 1969 times.
✗ Branch 2 not taken.
 1969 active.erase(index);
500 }
501
4/4
✓ Branch 1 taken 26 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 26 times.
✓ Branch 5 taken 3 times.
 32 }
502
503 //==============================================================================
504 6 void IntervalTreeCollisionManager::distance(
505 DistanceCallBackBase* callback) const {
506
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 callback->init();
507
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;
508
509 6 this->enable_tested_set_ = true;
510 6 this->tested_set.clear();
511 6 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
512
513
2/2
✓ Branch 1 taken 5980 times.
✓ Branch 2 taken 6 times.
 5986 for (size_t i = 0; i < endpoints[0].size(); ++i)
514
2/4
✓ Branch 2 taken 5980 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 5980 times.
 5980 if (distance_(endpoints[0][i].obj, callback, min_dist)) break;
515
516 6 this->enable_tested_set_ = false;
517 6 this->tested_set.clear();
518 }
519
520 //==============================================================================
521 ✗ void IntervalTreeCollisionManager::collide(
522 BroadPhaseCollisionManager* other_manager_,
523 CollisionCallBackBase* callback) const {
524 ✗ callback->init();
525 ✗ IntervalTreeCollisionManager* other_manager =
526 static_cast<IntervalTreeCollisionManager*>(other_manager_);
527
528 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
529
530 ✗ if (this == other_manager) {
531 ✗ collide(callback);
532 ✗ return;
533 }
534
535 ✗ if (this->size() < other_manager->size()) {
536 ✗ for (size_t i = 0, size = endpoints[0].size(); i < size; ++i)
537 ✗ if (other_manager->collide_(endpoints[0][i].obj, callback)) return;
538 } else {
539 ✗ for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
540 ✗ if (collide_(other_manager->endpoints[0][i].obj, callback)) return;
541 }
542 }
543
544 //==============================================================================
545 ✗ void IntervalTreeCollisionManager::distance(
546 BroadPhaseCollisionManager* other_manager_,
547 DistanceCallBackBase* callback) const {
548 ✗ callback->init();
549 ✗ IntervalTreeCollisionManager* other_manager =
550 static_cast<IntervalTreeCollisionManager*>(other_manager_);
551
552 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
553
554 ✗ if (this == other_manager) {
555 ✗ distance(callback);
556 ✗ return;
557 }
558
559 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
560
561 ✗ if (this->size() < other_manager->size()) {
562 ✗ for (size_t i = 0, size = endpoints[0].size(); i < size; ++i)
563 ✗ if (other_manager->distance_(endpoints[0][i].obj, callback, min_dist))
564 ✗ return;
565 } else {
566 ✗ for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
567 ✗ if (distance_(other_manager->endpoints[0][i].obj, callback, min_dist))
568 ✗ return;
569 }
570 }
571
572 //==============================================================================
573 ✗ bool IntervalTreeCollisionManager::empty() const {
574 ✗ return endpoints[0].empty();
575 }
576
577 //==============================================================================
578 3945 size_t IntervalTreeCollisionManager::size() const {
579 3945 return endpoints[0].size() / 2;
580 }
581
582 //==============================================================================
583 3762 bool IntervalTreeCollisionManager::checkColl(
584 typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
585 typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
586 CollisionObject* obj, CollisionCallBackBase* callback) const {
587
2/2
✓ Branch 1 taken 14431 times.
✓ Branch 2 taken 3761 times.
 18192 while (pos_start < pos_end) {
588 14431 SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
589
1/2
✓ Branch 0 taken 14431 times.
✗ Branch 1 not taken.
 14431 if (ivl->obj != obj) {
590
2/2
✓ Branch 3 taken 19 times.
✓ Branch 4 taken 14412 times.
 14431 if (ivl->obj->getAABB().overlap(obj->getAABB())) {
591
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 18 times.
 19 if ((*callback)(ivl->obj, obj)) return true;
592 }
593 }
594
595 14430 pos_start++;
596 }
597
598 3761 return false;
599 }
600
601 //==============================================================================
602 6156 bool IntervalTreeCollisionManager::checkDist(
603 typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
604 typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
605 CollisionObject* obj, DistanceCallBackBase* callback,
606 Scalar& min_dist) const {
607
2/2
✓ Branch 1 taken 90503 times.
✓ Branch 2 taken 6156 times.
 96659 while (pos_start < pos_end) {
608 90503 SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
609
2/2
✓ Branch 0 taken 84517 times.
✓ Branch 1 taken 5986 times.
 90503 if (ivl->obj != obj) {
610
2/2
✓ Branch 0 taken 24428 times.
✓ Branch 1 taken 60089 times.
 84517 if (!this->enable_tested_set_) {
611
2/2
✓ Branch 3 taken 1565 times.
✓ Branch 4 taken 22863 times.
 24428 if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) {
612
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1565 times.
 1565 if ((*callback)(ivl->obj, obj, min_dist)) return true;
613 }
614 } else {
615
2/2
✓ Branch 1 taken 18452 times.
✓ Branch 2 taken 41637 times.
 60089 if (!this->inTestedSet(ivl->obj, obj)) {
616
2/2
✓ Branch 3 taken 286 times.
✓ Branch 4 taken 18166 times.
 18452 if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) {
617
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 286 times.
 286 if ((*callback)(ivl->obj, obj, min_dist)) return true;
618 }
619
620 18452 this->insertTestedSet(ivl->obj, obj);
621 }
622 }
623 }
624
625 90503 pos_start++;
626 }
627
628 6156 return false;
629 }
630
631 //==============================================================================
632 1086788 bool IntervalTreeCollisionManager::EndPoint::operator<(
633 const typename IntervalTreeCollisionManager::EndPoint& p) const {
634 1086788 return value < p.value;
635 }
636
637 //==============================================================================
638 42045 IntervalTreeCollisionManager::SAPInterval::SAPInterval(Scalar low_,
639 Scalar high_,
640 42045 CollisionObject* obj_)
641 42045 : detail::SimpleInterval() {
642 42045 this->low = low_;
643 42045 this->high = high_;
644 42045 obj = obj_;
645 42045 }
646
647 } // namespace coal
648