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