GCC Code Coverage Report

Directory: ./
File: include/coal/broadphase/broadphase_spatialhash-inl.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 139 227 61.2%
Branches: 159 392 40.6%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** @author Jia Pan */
37
38 #ifndef COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
39 #define COAL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
40
41 #include "coal/broadphase/broadphase_spatialhash.h"
42
43 namespace coal {
44
45 //==============================================================================
46 template <typename HashTable>
47 51 SpatialHashingCollisionManager<HashTable>::SpatialHashingCollisionManager(
48 Scalar cell_size, const Vec3s& scene_min, const Vec3s& scene_max,
49 unsigned int default_table_size)
50
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 : scene_limit(AABB(scene_min, scene_max)),
51
3/6
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 51 times.
✗ Branch 13 not taken.
 102 hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
52
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 51 hash_table->init(default_table_size);
53 51 }
54
55 //==============================================================================
56 template <typename HashTable>
57 200 SpatialHashingCollisionManager<HashTable>::~SpatialHashingCollisionManager() {
58 100 delete hash_table;
59
1/2
✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
 300 }
60
61 //==============================================================================
62 template <typename HashTable>
63 12671 void SpatialHashingCollisionManager<HashTable>::registerObject(
64 CollisionObject* obj) {
65
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 objs.push_back(obj);
66
67 12671 const AABB& obj_aabb = obj->getAABB();
68
1/2
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
 12671 AABB overlap_aabb;
69
70
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
 12671 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
71
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12671 times.
 12671 if (!scene_limit.contain(obj_aabb))
72 ✗ objs_partially_penetrating_scene_limit.push_back(obj);
73
74
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
 12671 hash_table->insert(overlap_aabb, obj);
75 } else {
76 ✗ objs_outside_scene_limit.push_back(obj);
77 }
78
79
2/4
✓ Branch 1 taken 12671 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12671 times.
✗ Branch 5 not taken.
 12671 obj_aabb_map[obj] = obj_aabb;
80 12671 }
81
82 //==============================================================================
83 template <typename HashTable>
84 ✗ void SpatialHashingCollisionManager<HashTable>::unregisterObject(
85 CollisionObject* obj) {
86 ✗ objs.remove(obj);
87
88 ✗ const AABB& obj_aabb = obj->getAABB();
89 ✗ AABB overlap_aabb;
90
91 ✗ if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
92 ✗ if (!scene_limit.contain(obj_aabb))
93 ✗ objs_partially_penetrating_scene_limit.remove(obj);
94
95 ✗ hash_table->remove(overlap_aabb, obj);
96 } else {
97 ✗ objs_outside_scene_limit.remove(obj);
98 }
99
100 ✗ obj_aabb_map.erase(obj);
101 }
102
103 //==============================================================================
104 template <typename HashTable>
105 51 void SpatialHashingCollisionManager<HashTable>::setup() {
106 // Do nothing
107 51 }
108
109 //==============================================================================
110 template <typename HashTable>
111 13 void SpatialHashingCollisionManager<HashTable>::update() {
112 13 hash_table->clear();
113 13 objs_partially_penetrating_scene_limit.clear();
114 13 objs_outside_scene_limit.clear();
115
116
2/2
✓ Branch 4 taken 1344 times.
✓ Branch 5 taken 13 times.
 1357 for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
117 1344 CollisionObject* obj = *it;
118 1344 const AABB& obj_aabb = obj->getAABB();
119
1/2
✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
 1344 AABB overlap_aabb;
120
121
3/4
✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1333 times.
✓ Branch 4 taken 11 times.
 1344 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
122
3/4
✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44 times.
✓ Branch 4 taken 1289 times.
 1333 if (!scene_limit.contain(obj_aabb))
123
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 objs_partially_penetrating_scene_limit.push_back(obj);
124
125
2/4
✓ Branch 1 taken 1333 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1333 times.
✗ Branch 5 not taken.
 1333 hash_table->insert(overlap_aabb, obj);
126 } else {
127
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 objs_outside_scene_limit.push_back(obj);
128 }
129
130
2/4
✓ Branch 1 taken 1344 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1344 times.
✗ Branch 5 not taken.
 1344 obj_aabb_map[obj] = obj_aabb;
131 }
132 13 }
133
134 //==============================================================================
135 template <typename HashTable>
136 ✗ void SpatialHashingCollisionManager<HashTable>::update(
137 CollisionObject* updated_obj) {
138 ✗ const AABB& new_aabb = updated_obj->getAABB();
139 ✗ const AABB& old_aabb = obj_aabb_map[updated_obj];
140
141 ✗ AABB old_overlap_aabb;
142 const auto is_old_aabb_overlapping =
143 ✗ scene_limit.overlap(old_aabb, old_overlap_aabb);
144 ✗ if (is_old_aabb_overlapping)
145 ✗ hash_table->remove(old_overlap_aabb, updated_obj);
146
147 ✗ AABB new_overlap_aabb;
148 const auto is_new_aabb_overlapping =
149 ✗ scene_limit.overlap(new_aabb, new_overlap_aabb);
150 ✗ if (is_new_aabb_overlapping)
151 ✗ hash_table->insert(new_overlap_aabb, updated_obj);
152
153 ObjectStatus old_status;
154 ✗ if (is_old_aabb_overlapping) {
155 ✗ if (scene_limit.contain(old_aabb))
156 ✗ old_status = Inside;
157 else
158 ✗ old_status = PartiallyPenetrating;
159 } else {
160 ✗ old_status = Outside;
161 }
162
163 ✗ if (is_new_aabb_overlapping) {
164 ✗ if (scene_limit.contain(new_aabb)) {
165 ✗ if (old_status == PartiallyPenetrating) {
166 // Status change: PartiallyPenetrating --> Inside
167 // Required action(s):
168 // - remove object from "objs_partially_penetrating_scene_limit"
169
170 ✗ auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
171 objs_partially_penetrating_scene_limit.end(),
172 updated_obj);
173 ✗ objs_partially_penetrating_scene_limit.erase(find_it);
174 ✗ } else if (old_status == Outside) {
175 // Status change: Outside --> Inside
176 // Required action(s):
177 // - remove object from "objs_outside_scene_limit"
178
179 ✗ auto find_it = std::find(objs_outside_scene_limit.begin(),
180 objs_outside_scene_limit.end(), updated_obj);
181 ✗ objs_outside_scene_limit.erase(find_it);
182 }
183 } else {
184 ✗ if (old_status == Inside) {
185 // Status change: Inside --> PartiallyPenetrating
186 // Required action(s):
187 // - add object to "objs_partially_penetrating_scene_limit"
188
189 ✗ objs_partially_penetrating_scene_limit.push_back(updated_obj);
190 ✗ } else if (old_status == Outside) {
191 // Status change: Outside --> PartiallyPenetrating
192 // Required action(s):
193 // - remove object from "objs_outside_scene_limit"
194 // - add object to "objs_partially_penetrating_scene_limit"
195
196 ✗ auto find_it = std::find(objs_outside_scene_limit.begin(),
197 objs_outside_scene_limit.end(), updated_obj);
198 ✗ objs_outside_scene_limit.erase(find_it);
199
200 ✗ objs_partially_penetrating_scene_limit.push_back(updated_obj);
201 }
202 }
203 } else {
204 ✗ if (old_status == Inside) {
205 // Status change: Inside --> Outside
206 // Required action(s):
207 // - add object to "objs_outside_scene_limit"
208
209 ✗ objs_outside_scene_limit.push_back(updated_obj);
210 ✗ } else if (old_status == PartiallyPenetrating) {
211 // Status change: PartiallyPenetrating --> Outside
212 // Required action(s):
213 // - remove object from "objs_partially_penetrating_scene_limit"
214 // - add object to "objs_outside_scene_limit"
215
216 auto find_it =
217 ✗ std::find(objs_partially_penetrating_scene_limit.begin(),
218 objs_partially_penetrating_scene_limit.end(), updated_obj);
219 ✗ objs_partially_penetrating_scene_limit.erase(find_it);
220
221 ✗ objs_outside_scene_limit.push_back(updated_obj);
222 }
223 }
224
225 ✗ obj_aabb_map[updated_obj] = new_aabb;
226 }
227
228 //==============================================================================
229 template <typename HashTable>
230 ✗ void SpatialHashingCollisionManager<HashTable>::update(
231 const std::vector<CollisionObject*>& updated_objs) {
232 ✗ for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
233 }
234
235 //==============================================================================
236 template <typename HashTable>
237 ✗ void SpatialHashingCollisionManager<HashTable>::clear() {
238 ✗ objs.clear();
239 ✗ hash_table->clear();
240 ✗ objs_outside_scene_limit.clear();
241 ✗ obj_aabb_map.clear();
242 }
243
244 //==============================================================================
245 template <typename HashTable>
246 ✗ void SpatialHashingCollisionManager<HashTable>::getObjects(
247 std::vector<CollisionObject*>& objs_) const {
248 ✗ objs_.resize(objs.size());
249 ✗ std::copy(objs.begin(), objs.end(), objs_.begin());
250 }
251
252 //==============================================================================
253 template <typename HashTable>
254 3822 void SpatialHashingCollisionManager<HashTable>::collide(
255 CollisionObject* obj, CollisionCallBackBase* callback) const {
256
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3762 times.
 3822 if (size() == 0) return;
257 3762 collide_(obj, callback);
258 }
259
260 //==============================================================================
261 template <typename HashTable>
262 80 void SpatialHashingCollisionManager<HashTable>::distance(
263 CollisionObject* obj, DistanceCallBackBase* callback) const {
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 80 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
266
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 distance_(obj, callback, min_dist);
267 }
268
269 //==============================================================================
270 template <typename HashTable>
271 3762 bool SpatialHashingCollisionManager<HashTable>::collide_(
272 CollisionObject* obj, CollisionCallBackBase* callback) const {
273 3762 const auto& obj_aabb = obj->getAABB();
274
1/2
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
 3762 AABB overlap_aabb;
275
276
3/4
✓ Branch 1 taken 3762 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3455 times.
✓ Branch 4 taken 307 times.
 3762 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
277
2/4
✓ Branch 1 taken 3455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3455 times.
✗ Branch 5 not taken.
 3455 const auto query_result = hash_table->query(overlap_aabb);
278
2/2
✓ Branch 5 taken 207 times.
✓ Branch 6 taken 3454 times.
 3661 for (const auto& obj2 : query_result) {
279
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 207 times.
 207 if (obj == obj2) continue;
280
281
3/4
✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 206 times.
 207 if ((*callback)(obj, obj2)) return true;
282 }
283
284
3/4
✓ Branch 1 taken 3454 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 145 times.
✓ Branch 4 taken 3309 times.
 3454 if (!scene_limit.contain(obj_aabb)) {
285
2/2
✓ Branch 5 taken 86 times.
✓ Branch 6 taken 145 times.
 231 for (const auto& obj2 : objs_outside_scene_limit) {
286
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 86 times.
 86 if (obj == obj2) continue;
287
288
2/4
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 86 times.
 86 if ((*callback)(obj, obj2)) return true;
289 }
290 }
291
2/2
✓ Branch 1 taken 3454 times.
✓ Branch 2 taken 1 times.
 3455 } else {
292
2/2
✓ Branch 5 taken 239 times.
✓ Branch 6 taken 307 times.
 546 for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
293
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 239 times.
 239 if (obj == obj2) continue;
294
295
2/4
✓ Branch 1 taken 239 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 239 times.
 239 if ((*callback)(obj, obj2)) return true;
296 }
297
298
2/2
✓ Branch 5 taken 89 times.
✓ Branch 6 taken 307 times.
 396 for (const auto& obj2 : objs_outside_scene_limit) {
299
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 89 times.
 89 if (obj == obj2) continue;
300
301
2/4
✓ Branch 1 taken 89 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 89 times.
 89 if ((*callback)(obj, obj2)) return true;
302 }
303 }
304
305 3761 return false;
306 }
307
308 //==============================================================================
309 template <typename HashTable>
310 3070 bool SpatialHashingCollisionManager<HashTable>::distance_(
311 CollisionObject* obj, DistanceCallBackBase* callback,
312 Scalar& min_dist) const {
313
2/4
✓ Branch 3 taken 3070 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3070 times.
✗ Branch 7 not taken.
 3070 auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
314
1/2
✓ Branch 2 taken 3070 times.
✗ Branch 3 not taken.
 3070 auto aabb = obj->getAABB();
315
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 86 times.
 3070 if (min_dist < (std::numeric_limits<Scalar>::max)()) {
316
1/2
✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
 2984 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
317
1/2
✓ Branch 1 taken 2984 times.
✗ Branch 2 not taken.
 2984 aabb.expand(min_dist_delta);
318 }
319
320
1/2
✓ Branch 1 taken 3070 times.
✗ Branch 2 not taken.
 3070 AABB overlap_aabb;
321
322 3070 auto status = 1;
323 Scalar old_min_distance;
324
325 331 while (1) {
326 3401 old_min_distance = min_dist;
327
328
3/4
✓ Branch 1 taken 3401 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3396 times.
✓ Branch 4 taken 5 times.
 3401 if (scene_limit.overlap(aabb, overlap_aabb)) {
329
4/8
✓ Branch 1 taken 3396 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3396 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3396 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3396 times.
 3396 if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
330 callback, min_dist)) {
331 ✗ return true;
332 }
333
334
3/4
✓ Branch 1 taken 3396 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3087 times.
✓ Branch 4 taken 309 times.
 3396 if (!scene_limit.contain(aabb)) {
335
2/4
✓ Branch 1 taken 3087 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3087 times.
 3087 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
336 min_dist)) {
337 ✗ return true;
338 }
339 }
340 } else {
341
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 5 if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
342 callback, min_dist)) {
343 ✗ return true;
344 }
345
346
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 5 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
347 min_dist)) {
348 ✗ return true;
349 }
350 }
351
352
2/2
✓ Branch 0 taken 3315 times.
✓ Branch 1 taken 86 times.
 3401 if (status == 1) {
353
2/2
✓ Branch 1 taken 2984 times.
✓ Branch 2 taken 331 times.
 3315 if (old_min_distance < (std::numeric_limits<Scalar>::max)()) {
354 2984 break;
355 } else {
356
2/2
✓ Branch 0 taken 86 times.
✓ Branch 1 taken 245 times.
 331 if (min_dist < old_min_distance) {
357
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 Vec3s min_dist_delta(min_dist, min_dist, min_dist);
358
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);
359 86 status = 0;
360 } else {
361
3/4
✓ Branch 2 taken 245 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 45 times.
✓ Branch 5 taken 200 times.
 245 if (aabb == obj->getAABB())
362
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45 times.
✗ Branch 5 not taken.
 45 aabb.expand(delta);
363 else
364
1/2
✓ Branch 2 taken 200 times.
✗ Branch 3 not taken.
 200 aabb.expand(obj->getAABB(), 2.0);
365 }
366 }
367
1/2
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
 86 } else if (status == 0) {
368 86 break;
369 }
370 }
371
372 3070 return false;
373 }
374
375 //==============================================================================
376 template <typename HashTable>
377 37 void SpatialHashingCollisionManager<HashTable>::collide(
378 CollisionCallBackBase* callback) const {
379
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 29 times.
 37 if (size() == 0) return;
380
381
2/2
✓ Branch 5 taken 2148 times.
✓ Branch 6 taken 26 times.
 2174 for (const auto& obj1 : objs) {
382 2148 const auto& obj_aabb = obj1->getAABB();
383
1/2
✓ Branch 1 taken 2148 times.
✗ Branch 2 not taken.
 2148 AABB overlap_aabb;
384
385
3/4
✓ Branch 1 taken 2148 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2138 times.
✓ Branch 4 taken 10 times.
 2148 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
386
2/4
✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2138 times.
✗ Branch 5 not taken.
 2138 auto query_result = hash_table->query(overlap_aabb);
387
2/2
✓ Branch 5 taken 2382 times.
✓ Branch 6 taken 2135 times.
 4517 for (const auto& obj2 : query_result) {
388
2/2
✓ Branch 0 taken 122 times.
✓ Branch 1 taken 2260 times.
 2382 if (obj1 < obj2) {
389
3/4
✓ Branch 1 taken 122 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 119 times.
 122 if ((*callback)(obj1, obj2)) return;
390 }
391 }
392
393
3/4
✓ Branch 1 taken 2135 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
✓ Branch 4 taken 2095 times.
 2135 if (!scene_limit.contain(obj_aabb)) {
394
2/2
✓ Branch 5 taken 33 times.
✓ Branch 6 taken 40 times.
 73 for (const auto& obj2 : objs_outside_scene_limit) {
395
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 17 times.
 33 if (obj1 < obj2) {
396
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
 16 if ((*callback)(obj1, obj2)) return;
397 }
398 }
399 }
400
2/2
✓ Branch 1 taken 2135 times.
✓ Branch 2 taken 3 times.
 2138 } else {
401
2/2
✓ Branch 5 taken 30 times.
✓ Branch 6 taken 10 times.
 40 for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
402
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 17 times.
 30 if (obj1 < obj2) {
403
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 13 times.
 13 if ((*callback)(obj1, obj2)) return;
404 }
405 }
406
407
2/2
✓ Branch 5 taken 23 times.
✓ Branch 6 taken 10 times.
 33 for (const auto& obj2 : objs_outside_scene_limit) {
408
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 16 times.
 23 if (obj1 < obj2) {
409
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
 7 if ((*callback)(obj1, obj2)) return;
410 }
411 }
412 }
413 }
414 }
415
416 //==============================================================================
417 template <typename HashTable>
418 6 void SpatialHashingCollisionManager<HashTable>::distance(
419 DistanceCallBackBase* callback) const {
420
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;
421
422 6 this->enable_tested_set_ = true;
423 6 this->tested_set.clear();
424
425 6 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
426
427
2/2
✓ Branch 5 taken 2990 times.
✓ Branch 6 taken 6 times.
 2996 for (const auto& obj : objs) {
428
2/4
✓ Branch 1 taken 2990 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2990 times.
 2990 if (distance_(obj, callback, min_dist)) break;
429 }
430
431 6 this->enable_tested_set_ = false;
432 6 this->tested_set.clear();
433 }
434
435 //==============================================================================
436 template <typename HashTable>
437 ✗ void SpatialHashingCollisionManager<HashTable>::collide(
438 BroadPhaseCollisionManager* other_manager_,
439 CollisionCallBackBase* callback) const {
440 ✗ auto* other_manager =
441 static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
442
443 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
444
445 ✗ if (this == other_manager) {
446 ✗ collide(callback);
447 ✗ return;
448 }
449
450 ✗ if (this->size() < other_manager->size()) {
451 ✗ for (const auto& obj : objs) {
452 ✗ if (other_manager->collide_(obj, callback)) return;
453 }
454 } else {
455 ✗ for (const auto& obj : other_manager->objs) {
456 ✗ if (collide_(obj, callback)) return;
457 }
458 }
459 }
460
461 //==============================================================================
462 template <typename HashTable>
463 ✗ void SpatialHashingCollisionManager<HashTable>::distance(
464 BroadPhaseCollisionManager* other_manager_,
465 DistanceCallBackBase* callback) const {
466 ✗ auto* other_manager =
467 static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
468
469 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
470
471 ✗ if (this == other_manager) {
472 ✗ distance(callback);
473 ✗ return;
474 }
475
476 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
477
478 ✗ if (this->size() < other_manager->size()) {
479 ✗ for (const auto& obj : objs)
480 ✗ if (other_manager->distance_(obj, callback, min_dist)) return;
481 } else {
482 ✗ for (const auto& obj : other_manager->objs)
483 ✗ if (distance_(obj, callback, min_dist)) return;
484 }
485 }
486
487 //==============================================================================
488 template <typename HashTable>
489 ✗ bool SpatialHashingCollisionManager<HashTable>::empty() const {
490 ✗ return objs.empty();
491 }
492
493 //==============================================================================
494 template <typename HashTable>
495 3945 size_t SpatialHashingCollisionManager<HashTable>::size() const {
496 3945 return objs.size();
497 }
498
499 //==============================================================================
500 template <typename HashTable>
501 51 void SpatialHashingCollisionManager<HashTable>::computeBound(
502 std::vector<CollisionObject*>& objs, Vec3s& l, Vec3s& u) {
503
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 AABB bound;
504
3/4
✓ Branch 3 taken 12671 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12671 times.
✓ Branch 7 taken 51 times.
 12722 for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
505
506
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 l = bound.min_;
507
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 u = bound.max_;
508 51 }
509
510 //==============================================================================
511 template <typename HashTable>
512 template <typename Container>
513 6493 bool SpatialHashingCollisionManager<HashTable>::distanceObjectToObjects(
514 CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
515 Scalar& min_dist) const {
516 15463 for (auto& obj2 : objs) {
517 8970 if (obj == obj2) continue;
518
519 5942 if (!this->enable_tested_set_) {
520 1470 if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
521 961 if ((*callback)(obj, obj2, min_dist)) return true;
522 }
523 } else {
524 4472 if (!this->inTestedSet(obj, obj2)) {
525 2236 if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
526 242 if ((*callback)(obj, obj2, min_dist)) return true;
527 }
528
529 2236 this->insertTestedSet(obj, obj2);
530 }
531 }
532 }
533
534 6493 return false;
535 }
536
537 } // namespace coal
538
539 #endif
540