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_SaP.h" |
||
39 |
|||
40 |
namespace hpp { |
||
41 |
namespace fcl { |
||
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 |
✓✓ | 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 |
✓✗✓✓ |
500 |
SaPCollisionManager::~SaPCollisionManager() { clear(); } |
95 |
|||
96 |
//============================================================================== |
||
97 |
51 |
void SaPCollisionManager::registerObjects( |
|
98 |
const std::vector<CollisionObject*>& other_objs) { |
||
99 |
✓✓ | 51 |
if (other_objs.empty()) return; |
100 |
|||
101 |
✗✓ | 43 |
if (size() > 0) |
102 |
BroadPhaseCollisionManager::registerObjects(other_objs); |
||
103 |
else { |
||
104 |
✓✗ | 86 |
std::vector<EndPoint*> endpoints(2 * other_objs.size()); |
105 |
|||
106 |
✓✓ | 12714 |
for (size_t i = 0; i < other_objs.size(); ++i) { |
107 |
✓✗✓✗ |
12671 |
SaPAABB* sapaabb = new SaPAABB(); |
108 |
12671 |
sapaabb->obj = other_objs[i]; |
|
109 |
✓✗ | 12671 |
sapaabb->lo = new EndPoint(); |
110 |
✓✗ | 12671 |
sapaabb->hi = new EndPoint(); |
111 |
✓✗ | 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 |
✓✗ | 12671 |
AABB_arr.push_back(sapaabb); |
119 |
✓✗ | 12671 |
obj_aabb_map[other_objs[i]] = sapaabb; |
120 |
} |
||
121 |
|||
122 |
FCL_REAL scale[3]; |
||
123 |
✓✓ | 172 |
for (int coord = 0; coord < 3; ++coord) { |
124 |
✓✗ | 129 |
std::sort( |
125 |
endpoints.begin(), endpoints.end(), |
||
126 |
✓✗ | 129 |
std::bind(std::less<FCL_REAL>(), |
127 |
✓✗ | 129 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>( |
128 |
&EndPoint::getVal), |
||
129 |
std::placeholders::_1, coord), |
||
130 |
✓✗ | 129 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) 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 |
✓✓ | 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 |
✓✗ | 129 |
scale[coord] = endpoints.back()->aabb->cached.max_[coord] - |
147 |
✓✗ | 129 |
endpoints[0]->aabb->cached.min_[coord]; |
148 |
} |
||
149 |
|||
150 |
43 |
int axis = 0; |
|
151 |
✓✓ | 43 |
if (scale[axis] < scale[1]) axis = 1; |
152 |
✓✓ | 43 |
if (scale[axis] < scale[2]) axis = 2; |
153 |
|||
154 |
43 |
EndPoint* pos = elist[axis]; |
|
155 |
|||
156 |
✓✓ | 16173 |
while (pos != nullptr) { |
157 |
16130 |
EndPoint* pos_next = nullptr; |
|
158 |
16130 |
SaPAABB* aabb = pos->aabb; |
|
159 |
16130 |
EndPoint* pos_it = pos->next[axis]; |
|
160 |
|||
161 |
✓✓ | 5276416 |
while (pos_it != nullptr) { |
162 |
✓✓ | 5272957 |
if (pos_it->aabb == aabb) { |
163 |
✓✓ | 12671 |
if (pos_next == nullptr) pos_next = pos_it; |
164 |
12671 |
break; |
|
165 |
} |
||
166 |
|||
167 |
✓✓ | 5260286 |
if (pos_it->minmax == 0) { |
168 |
✓✓ | 2629944 |
if (pos_next == nullptr) pos_next = pos_it; |
169 |
✓✗✓✓ |
2629944 |
if (pos_it->aabb->cached.overlap(aabb->cached)) |
170 |
✓✗ | 47321 |
overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj); |
171 |
} |
||
172 |
5260286 |
pos_it = pos_it->next[axis]; |
|
173 |
} |
||
174 |
|||
175 |
16130 |
pos = pos_next; |
|
176 |
} |
||
177 |
} |
||
178 |
|||
179 |
43 |
updateVelist(); |
|
180 |
} |
||
181 |
|||
182 |
//============================================================================== |
||
183 |
void SaPCollisionManager::registerObject(CollisionObject* obj) { |
||
184 |
SaPAABB* curr = new SaPAABB; |
||
185 |
curr->cached = obj->getAABB(); |
||
186 |
curr->obj = obj; |
||
187 |
curr->lo = new EndPoint; |
||
188 |
curr->lo->minmax = 0; |
||
189 |
curr->lo->aabb = curr; |
||
190 |
|||
191 |
curr->hi = new EndPoint; |
||
192 |
curr->hi->minmax = 1; |
||
193 |
curr->hi->aabb = curr; |
||
194 |
|||
195 |
for (int coord = 0; coord < 3; ++coord) { |
||
196 |
EndPoint* current = elist[coord]; |
||
197 |
|||
198 |
// first insert the lo end point |
||
199 |
if (current == nullptr) // empty list |
||
200 |
{ |
||
201 |
elist[coord] = curr->lo; |
||
202 |
curr->lo->prev[coord] = curr->lo->next[coord] = nullptr; |
||
203 |
} else // otherwise, find the correct location in the list and insert |
||
204 |
{ |
||
205 |
EndPoint* curr_lo = curr->lo; |
||
206 |
FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; |
||
207 |
while ((current->getVal()[coord] < curr_lo_val) && |
||
208 |
(current->next[coord] != nullptr)) |
||
209 |
current = current->next[coord]; |
||
210 |
|||
211 |
if (current->getVal()[coord] >= curr_lo_val) { |
||
212 |
curr_lo->prev[coord] = current->prev[coord]; |
||
213 |
curr_lo->next[coord] = current; |
||
214 |
if (current->prev[coord] == nullptr) |
||
215 |
elist[coord] = curr_lo; |
||
216 |
else |
||
217 |
current->prev[coord]->next[coord] = curr_lo; |
||
218 |
|||
219 |
current->prev[coord] = curr_lo; |
||
220 |
} else { |
||
221 |
curr_lo->prev[coord] = current; |
||
222 |
curr_lo->next[coord] = nullptr; |
||
223 |
current->next[coord] = curr_lo; |
||
224 |
} |
||
225 |
} |
||
226 |
|||
227 |
// now insert hi end point |
||
228 |
current = curr->lo; |
||
229 |
|||
230 |
EndPoint* curr_hi = curr->hi; |
||
231 |
FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; |
||
232 |
|||
233 |
if (coord == 0) { |
||
234 |
while ((current->getVal()[coord] < curr_hi_val) && |
||
235 |
(current->next[coord] != nullptr)) { |
||
236 |
if (current != curr->lo) |
||
237 |
if (current->aabb->cached.overlap(curr->cached)) |
||
238 |
overlap_pairs.emplace_back(current->aabb->obj, obj); |
||
239 |
|||
240 |
current = current->next[coord]; |
||
241 |
} |
||
242 |
} else { |
||
243 |
while ((current->getVal()[coord] < curr_hi_val) && |
||
244 |
(current->next[coord] != nullptr)) |
||
245 |
current = current->next[coord]; |
||
246 |
} |
||
247 |
|||
248 |
if (current->getVal()[coord] >= curr_hi_val) { |
||
249 |
curr_hi->prev[coord] = current->prev[coord]; |
||
250 |
curr_hi->next[coord] = current; |
||
251 |
if (current->prev[coord] == nullptr) |
||
252 |
elist[coord] = curr_hi; |
||
253 |
else |
||
254 |
current->prev[coord]->next[coord] = curr_hi; |
||
255 |
|||
256 |
current->prev[coord] = curr_hi; |
||
257 |
} else { |
||
258 |
curr_hi->prev[coord] = current; |
||
259 |
curr_hi->next[coord] = nullptr; |
||
260 |
current->next[coord] = curr_hi; |
||
261 |
} |
||
262 |
} |
||
263 |
|||
264 |
AABB_arr.push_back(curr); |
||
265 |
|||
266 |
obj_aabb_map[obj] = curr; |
||
267 |
|||
268 |
updateVelist(); |
||
269 |
} |
||
270 |
|||
271 |
//============================================================================== |
||
272 |
64 |
void SaPCollisionManager::setup() { |
|
273 |
✓✗✓✓ |
64 |
if (size() == 0) return; |
274 |
|||
275 |
FCL_REAL scale[3]; |
||
276 |
✓✗✓✗ |
56 |
scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); |
277 |
✓✗✓✗ |
56 |
scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1); |
278 |
; |
||
279 |
✓✗✓✗ |
56 |
scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); |
280 |
56 |
int axis = 0; |
|
281 |
✓✓ | 56 |
if (scale[axis] < scale[1]) axis = 1; |
282 |
✓✓ | 56 |
if (scale[axis] < scale[2]) axis = 2; |
283 |
56 |
optimal_axis = axis; |
|
284 |
} |
||
285 |
|||
286 |
//============================================================================== |
||
287 |
1344 |
void SaPCollisionManager::update_(SaPAABB* updated_aabb) { |
|
288 |
✓✗✗✓ |
1344 |
if (updated_aabb->cached == updated_aabb->obj->getAABB()) return; |
289 |
|||
290 |
1344 |
SaPAABB* current = updated_aabb; |
|
291 |
|||
292 |
✓✗ | 1344 |
Vec3f new_min = current->obj->getAABB().min_; |
293 |
✓✗ | 1344 |
Vec3f new_max = current->obj->getAABB().max_; |
294 |
|||
295 |
✓✗ | 1344 |
SaPAABB dummy; |
296 |
✓✗ | 1344 |
dummy.cached = current->obj->getAABB(); |
297 |
|||
298 |
✓✓ | 5376 |
for (int coord = 0; coord < 3; ++coord) { |
299 |
int direction; // -1 reverse, 0 nochange, 1 forward |
||
300 |
EndPoint* temp; |
||
301 |
|||
302 |
✓✗✓✗ ✓✓ |
4032 |
if (current->lo->getVal((size_t)coord) > new_min[coord]) |
303 |
2043 |
direction = -1; |
|
304 |
✓✗✓✗ ✓✗ |
1989 |
else if (current->lo->getVal((size_t)coord) < new_min[coord]) |
305 |
1989 |
direction = 1; |
|
306 |
else |
||
307 |
direction = 0; |
||
308 |
|||
309 |
✓✓ | 4032 |
if (direction == -1) { |
310 |
// first update the "lo" endpoint of the interval |
||
311 |
✓✓ | 2043 |
if (current->lo->prev[coord] != nullptr) { |
312 |
2033 |
temp = current->lo; |
|
313 |
✓✓✓✓ |
78994 |
while ((temp != nullptr) && |
314 |
✓✗✓✗ ✓✓ |
39485 |
(temp->getVal((size_t)coord) > new_min[coord])) { |
315 |
✓✓ | 37476 |
if (temp->minmax == 1) |
316 |
✓✗✓✓ |
17677 |
if (temp->aabb->cached.overlap(dummy.cached)) |
317 |
✓✗✓✗ |
8 |
addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
318 |
37476 |
temp = temp->prev[coord]; |
|
319 |
} |
||
320 |
|||
321 |
✓✓ | 2033 |
if (temp == nullptr) { |
322 |
24 |
current->lo->prev[coord]->next[coord] = current->lo->next[coord]; |
|
323 |
24 |
current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; |
|
324 |
24 |
current->lo->prev[coord] = nullptr; |
|
325 |
24 |
current->lo->next[coord] = elist[coord]; |
|
326 |
24 |
elist[coord]->prev[coord] = current->lo; |
|
327 |
24 |
elist[coord] = current->lo; |
|
328 |
} else { |
||
329 |
2009 |
current->lo->prev[coord]->next[coord] = current->lo->next[coord]; |
|
330 |
2009 |
current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; |
|
331 |
2009 |
current->lo->prev[coord] = temp; |
|
332 |
2009 |
current->lo->next[coord] = temp->next[coord]; |
|
333 |
2009 |
temp->next[coord]->prev[coord] = current->lo; |
|
334 |
2009 |
temp->next[coord] = current->lo; |
|
335 |
} |
||
336 |
} |
||
337 |
|||
338 |
✓✗✓✗ |
2043 |
current->lo->getVal((size_t)coord) = new_min[coord]; |
339 |
|||
340 |
// update hi end point |
||
341 |
2043 |
temp = current->hi; |
|
342 |
✓✗✓✗ ✓✓ |
39640 |
while (temp->getVal((size_t)coord) > new_max[coord]) { |
343 |
✓✓✓✓ |
55458 |
if ((temp->minmax == 0) && |
344 |
✓✗✓✓ |
17861 |
(temp->aabb->cached.overlap(current->cached))) |
345 |
✓✗✓✗ |
9 |
removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
346 |
37597 |
temp = temp->prev[coord]; |
|
347 |
} |
||
348 |
|||
349 |
2043 |
current->hi->prev[coord]->next[coord] = current->hi->next[coord]; |
|
350 |
✓✓ | 2043 |
if (current->hi->next[coord] != nullptr) |
351 |
2027 |
current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; |
|
352 |
2043 |
current->hi->prev[coord] = temp; |
|
353 |
2043 |
current->hi->next[coord] = temp->next[coord]; |
|
354 |
✓✓ | 2043 |
if (temp->next[coord] != nullptr) |
355 |
2034 |
temp->next[coord]->prev[coord] = current->hi; |
|
356 |
2043 |
temp->next[coord] = current->hi; |
|
357 |
|||
358 |
✓✗✓✗ |
2043 |
current->hi->getVal((size_t)coord) = new_max[coord]; |
359 |
✓✗ | 1989 |
} else if (direction == 1) { |
360 |
// here, we first update the "hi" endpoint. |
||
361 |
✓✓ | 1989 |
if (current->hi->next[coord] != nullptr) { |
362 |
1976 |
temp = current->hi; |
|
363 |
✓✓✓✓ |
73398 |
while ((temp->next[coord] != nullptr) && |
364 |
✓✗✓✗ ✓✓ |
36678 |
(temp->getVal((size_t)coord) < new_max[coord])) { |
365 |
✓✓ | 34744 |
if (temp->minmax == 0) |
366 |
✓✗✓✓ |
16310 |
if (temp->aabb->cached.overlap(dummy.cached)) |
367 |
✓✗✓✗ |
4 |
addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
368 |
34744 |
temp = temp->next[coord]; |
|
369 |
} |
||
370 |
|||
371 |
✓✗✓✗ ✓✓ |
1976 |
if (temp->getVal((size_t)coord) < new_max[coord]) { |
372 |
32 |
current->hi->prev[coord]->next[coord] = current->hi->next[coord]; |
|
373 |
32 |
current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; |
|
374 |
32 |
current->hi->prev[coord] = temp; |
|
375 |
32 |
current->hi->next[coord] = nullptr; |
|
376 |
32 |
temp->next[coord] = current->hi; |
|
377 |
} else { |
||
378 |
1944 |
current->hi->prev[coord]->next[coord] = current->hi->next[coord]; |
|
379 |
1944 |
current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; |
|
380 |
1944 |
current->hi->prev[coord] = temp->prev[coord]; |
|
381 |
1944 |
current->hi->next[coord] = temp; |
|
382 |
1944 |
temp->prev[coord]->next[coord] = current->hi; |
|
383 |
1944 |
temp->prev[coord] = current->hi; |
|
384 |
} |
||
385 |
} |
||
386 |
|||
387 |
✓✗✓✗ |
1989 |
current->hi->getVal((size_t)coord) = new_max[coord]; |
388 |
|||
389 |
// then, update the "lo" endpoint of the interval. |
||
390 |
1989 |
temp = current->lo; |
|
391 |
|||
392 |
✓✗✓✗ ✓✓ |
36927 |
while (temp->getVal((size_t)coord) < new_min[coord]) { |
393 |
✓✓✓✓ |
51428 |
if ((temp->minmax == 1) && |
394 |
✓✗✓✓ |
16490 |
(temp->aabb->cached.overlap(current->cached))) |
395 |
✓✗✓✗ |
8 |
removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); |
396 |
34938 |
temp = temp->next[coord]; |
|
397 |
} |
||
398 |
|||
399 |
✓✓ | 1989 |
if (current->lo->prev[coord] != nullptr) |
400 |
1971 |
current->lo->prev[coord]->next[coord] = current->lo->next[coord]; |
|
401 |
else |
||
402 |
18 |
elist[coord] = current->lo->next[coord]; |
|
403 |
1989 |
current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; |
|
404 |
1989 |
current->lo->prev[coord] = temp->prev[coord]; |
|
405 |
1989 |
current->lo->next[coord] = temp; |
|
406 |
✓✓ | 1989 |
if (temp->prev[coord] != nullptr) |
407 |
1978 |
temp->prev[coord]->next[coord] = current->lo; |
|
408 |
else |
||
409 |
11 |
elist[coord] = current->lo; |
|
410 |
1989 |
temp->prev[coord] = current->lo; |
|
411 |
✓✗✓✗ |
1989 |
current->lo->getVal((size_t)coord) = new_min[coord]; |
412 |
} |
||
413 |
} |
||
414 |
} |
||
415 |
|||
416 |
//============================================================================== |
||
417 |
56 |
void SaPCollisionManager::updateVelist() { |
|
418 |
✓✓ | 224 |
for (int coord = 0; coord < 3; ++coord) { |
419 |
168 |
velist[coord].resize(size() * 2); |
|
420 |
168 |
EndPoint* current = elist[coord]; |
|
421 |
168 |
size_t id = 0; |
|
422 |
✓✓ | 84258 |
while (current) { |
423 |
84090 |
velist[coord][id] = current; |
|
424 |
84090 |
current = current->next[coord]; |
|
425 |
84090 |
id++; |
|
426 |
} |
||
427 |
} |
||
428 |
56 |
} |
|
429 |
|||
430 |
//============================================================================== |
||
431 |
void SaPCollisionManager::update(CollisionObject* updated_obj) { |
||
432 |
update_(obj_aabb_map[updated_obj]); |
||
433 |
|||
434 |
updateVelist(); |
||
435 |
|||
436 |
setup(); |
||
437 |
} |
||
438 |
|||
439 |
//============================================================================== |
||
440 |
void SaPCollisionManager::update( |
||
441 |
const std::vector<CollisionObject*>& updated_objs) { |
||
442 |
for (size_t i = 0; i < updated_objs.size(); ++i) |
||
443 |
update_(obj_aabb_map[updated_objs[i]]); |
||
444 |
|||
445 |
updateVelist(); |
||
446 |
|||
447 |
setup(); |
||
448 |
} |
||
449 |
|||
450 |
//============================================================================== |
||
451 |
13 |
void SaPCollisionManager::update() { |
|
452 |
✓✓ | 1357 |
for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { |
453 |
✓✗ | 1344 |
update_(*it); |
454 |
} |
||
455 |
|||
456 |
13 |
updateVelist(); |
|
457 |
|||
458 |
13 |
setup(); |
|
459 |
13 |
} |
|
460 |
|||
461 |
//============================================================================== |
||
462 |
50 |
void SaPCollisionManager::clear() { |
|
463 |
✓✓ | 12421 |
for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { |
464 |
✓✗ | 12371 |
delete (*it)->hi; |
465 |
✓✗ | 12371 |
delete (*it)->lo; |
466 |
✓✗ | 12371 |
delete *it; |
467 |
12371 |
*it = nullptr; |
|
468 |
} |
||
469 |
|||
470 |
50 |
AABB_arr.clear(); |
|
471 |
50 |
overlap_pairs.clear(); |
|
472 |
|||
473 |
50 |
elist[0] = nullptr; |
|
474 |
50 |
elist[1] = nullptr; |
|
475 |
50 |
elist[2] = nullptr; |
|
476 |
|||
477 |
50 |
velist[0].clear(); |
|
478 |
50 |
velist[1].clear(); |
|
479 |
50 |
velist[2].clear(); |
|
480 |
|||
481 |
50 |
obj_aabb_map.clear(); |
|
482 |
50 |
} |
|
483 |
|||
484 |
//============================================================================== |
||
485 |
void SaPCollisionManager::getObjects( |
||
486 |
std::vector<CollisionObject*>& objs) const { |
||
487 |
objs.resize(AABB_arr.size()); |
||
488 |
size_t i = 0; |
||
489 |
for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; |
||
490 |
++it, ++i) { |
||
491 |
objs[i] = (*it)->obj; |
||
492 |
} |
||
493 |
} |
||
494 |
|||
495 |
//============================================================================== |
||
496 |
3762 |
bool SaPCollisionManager::collide_(CollisionObject* obj, |
|
497 |
CollisionCallBackBase* callback) const { |
||
498 |
3762 |
int axis = optimal_axis; |
|
499 |
3762 |
const AABB& obj_aabb = obj->getAABB(); |
|
500 |
|||
501 |
✓✗ | 3762 |
FCL_REAL min_val = obj_aabb.min_[axis]; |
502 |
// FCL_REAL max_val = obj_aabb.max_[axis]; |
||
503 |
|||
504 |
EndPoint dummy; |
||
505 |
✓✗ | 3762 |
SaPAABB dummy_aabb; |
506 |
✓✗ | 3762 |
dummy_aabb.cached = obj_aabb; |
507 |
3762 |
dummy.minmax = 1; |
|
508 |
3762 |
dummy.aabb = &dummy_aabb; |
|
509 |
|||
510 |
// compute stop_pos by binary search, this is cheaper than check it in while |
||
511 |
// iteration linearly |
||
512 |
const auto res_it = std::upper_bound( |
||
513 |
velist[axis].begin(), velist[axis].end(), &dummy, |
||
514 |
✓✗ | 3762 |
std::bind(std::less<FCL_REAL>(), |
515 |
✓✗ | 3762 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>( |
516 |
&EndPoint::getVal), |
||
517 |
std::placeholders::_1, axis), |
||
518 |
✓✗ | 3762 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>( |
519 |
&EndPoint::getVal), |
||
520 |
✓✗ | 3762 |
std::placeholders::_2, axis))); |
521 |
|||
522 |
3762 |
EndPoint* end_pos = nullptr; |
|
523 |
✓✓ | 3762 |
if (res_it != velist[axis].end()) end_pos = *res_it; |
524 |
|||
525 |
3762 |
EndPoint* pos = elist[axis]; |
|
526 |
|||
527 |
✓✓ | 603498 |
while (pos != end_pos) { |
528 |
✓✗ | 599736 |
if (pos->aabb->obj != obj) { |
529 |
✓✓✓✓ |
902850 |
if ((pos->minmax == 0) && |
530 |
✓✗✓✓ |
303114 |
(pos->aabb->hi->getVal((size_t)axis) >= min_val)) { |
531 |
✓✗✓✓ |
13026 |
if (pos->aabb->cached.overlap(obj->getAABB())) |
532 |
✓✗✗✓ |
10 |
if ((*callback)(obj, pos->aabb->obj)) return true; |
533 |
} |
||
534 |
} |
||
535 |
599736 |
pos = pos->next[axis]; |
|
536 |
} |
||
537 |
|||
538 |
3762 |
return false; |
|
539 |
} |
||
540 |
|||
541 |
//============================================================================== |
||
542 |
12 |
void SaPCollisionManager::addToOverlapPairs(const SaPPair& p) { |
|
543 |
12 |
bool repeated = false; |
|
544 |
✓✓ | 14 |
for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; |
545 |
2 |
++it) { |
|
546 |
✓✗✓✓ |
9 |
if (*it == p) { |
547 |
7 |
repeated = true; |
|
548 |
7 |
break; |
|
549 |
} |
||
550 |
} |
||
551 |
|||
552 |
✓✓ | 12 |
if (!repeated) overlap_pairs.push_back(p); |
553 |
12 |
} |
|
554 |
|||
555 |
//============================================================================== |
||
556 |
17 |
void SaPCollisionManager::removeFromOverlapPairs(const SaPPair& p) { |
|
557 |
✓✓ | 24 |
for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; |
558 |
7 |
++it) { |
|
559 |
✓✗✓✓ |
11 |
if (*it == p) { |
560 |
4 |
overlap_pairs.erase(it); |
|
561 |
4 |
break; |
|
562 |
} |
||
563 |
} |
||
564 |
|||
565 |
// or overlap_pairs.remove_if(isNotValidPair(p)); |
||
566 |
17 |
} |
|
567 |
|||
568 |
//============================================================================== |
||
569 |
3822 |
void SaPCollisionManager::collide(CollisionObject* obj, |
|
570 |
CollisionCallBackBase* callback) const { |
||
571 |
3822 |
callback->init(); |
|
572 |
✓✓ | 3822 |
if (size() == 0) return; |
573 |
|||
574 |
3762 |
collide_(obj, callback); |
|
575 |
} |
||
576 |
|||
577 |
//============================================================================== |
||
578 |
3070 |
bool SaPCollisionManager::distance_(CollisionObject* obj, |
|
579 |
DistanceCallBackBase* callback, |
||
580 |
FCL_REAL& min_dist) const { |
||
581 |
✓✗✓✗ ✓✗ |
3070 |
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; |
582 |
✓✗ | 3070 |
AABB aabb = obj->getAABB(); |
583 |
|||
584 |
✓✓ | 3070 |
if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) { |
585 |
✓✗ | 2984 |
Vec3f min_dist_delta(min_dist, min_dist, min_dist); |
586 |
✓✗ | 2984 |
aabb.expand(min_dist_delta); |
587 |
} |
||
588 |
|||
589 |
3070 |
int axis = optimal_axis; |
|
590 |
|||
591 |
3070 |
int status = 1; |
|
592 |
FCL_REAL old_min_distance; |
||
593 |
|||
594 |
3070 |
EndPoint* start_pos = elist[axis]; |
|
595 |
|||
596 |
while (1) { |
||
597 |
3203 |
old_min_distance = min_dist; |
|
598 |
✓✗ | 3203 |
FCL_REAL min_val = aabb.min_[axis]; |
599 |
// FCL_REAL max_val = aabb.max_[axis]; |
||
600 |
|||
601 |
EndPoint dummy; |
||
602 |
✓✗ | 3203 |
SaPAABB dummy_aabb; |
603 |
✓✗ | 3203 |
dummy_aabb.cached = aabb; |
604 |
3203 |
dummy.minmax = 1; |
|
605 |
3203 |
dummy.aabb = &dummy_aabb; |
|
606 |
|||
607 |
const auto res_it = std::upper_bound( |
||
608 |
velist[axis].begin(), velist[axis].end(), &dummy, |
||
609 |
✓✗ | 3203 |
std::bind(std::less<FCL_REAL>(), |
610 |
✓✗ | 3203 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>( |
611 |
&EndPoint::getVal), |
||
612 |
std::placeholders::_1, axis), |
||
613 |
✓✗ | 3203 |
std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>( |
614 |
&EndPoint::getVal), |
||
615 |
✓✗ | 3203 |
std::placeholders::_2, axis))); |
616 |
|||
617 |
3203 |
EndPoint* end_pos = nullptr; |
|
618 |
✓✓ | 3203 |
if (res_it != velist[axis].end()) end_pos = *res_it; |
619 |
|||
620 |
3203 |
EndPoint* pos = start_pos; |
|
621 |
|||
622 |
✓✓ | 3295491 |
while (pos != end_pos) { |
623 |
// can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and |
||
624 |
// then update start_pos to end_pos. but this seems slower. |
||
625 |
✓✓✓✓ |
4943848 |
if ((pos->minmax == 0) && |
626 |
✓✗✓✓ |
1651560 |
(pos->aabb->hi->getVal((size_t)axis) >= min_val)) { |
627 |
29656 |
CollisionObject* curr_obj = pos->aabb->obj; |
|
628 |
✓✓ | 29656 |
if (curr_obj != obj) { |
629 |
✓✓ | 26626 |
if (!this->enable_tested_set_) { |
630 |
✓✗✓✓ |
22134 |
if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { |
631 |
✓✗✗✓ |
1038 |
if ((*callback)(curr_obj, obj, min_dist)) return true; |
632 |
} |
||
633 |
} else { |
||
634 |
✓✗✓✓ |
4492 |
if (!this->inTestedSet(curr_obj, obj)) { |
635 |
✓✗✓✓ |
2240 |
if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) { |
636 |
✓✗✗✓ |
476 |
if ((*callback)(curr_obj, obj, min_dist)) return true; |
637 |
} |
||
638 |
|||
639 |
✓✗ | 2240 |
this->insertTestedSet(curr_obj, obj); |
640 |
} |
||
641 |
} |
||
642 |
} |
||
643 |
} |
||
644 |
|||
645 |
3292288 |
pos = pos->next[axis]; |
|
646 |
} |
||
647 |
|||
648 |
✓✓ | 3203 |
if (status == 1) { |
649 |
✓✓ | 3117 |
if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) |
650 |
2984 |
break; |
|
651 |
else { |
||
652 |
✓✓ | 133 |
if (min_dist < old_min_distance) { |
653 |
✓✗ | 86 |
Vec3f min_dist_delta(min_dist, min_dist, min_dist); |
654 |
✓✗✓✗ |
86 |
aabb = AABB(obj->getAABB(), min_dist_delta); |
655 |
86 |
status = 0; |
|
656 |
} else { |
||
657 |
✓✗✓✓ |
47 |
if (aabb == obj->getAABB()) |
658 |
✓✗ | 12 |
aabb.expand(delta); |
659 |
else |
||
660 |
✓✗ | 35 |
aabb.expand(obj->getAABB(), 2.0); |
661 |
} |
||
662 |
} |
||
663 |
✓✗ | 86 |
} else if (status == 0) |
664 |
86 |
break; |
|
665 |
133 |
} |
|
666 |
|||
667 |
3070 |
return false; |
|
668 |
} |
||
669 |
|||
670 |
//============================================================================== |
||
671 |
80 |
void SaPCollisionManager::distance(CollisionObject* obj, |
|
672 |
DistanceCallBackBase* callback) const { |
||
673 |
✓✗ | 80 |
callback->init(); |
674 |
✓✗✗✓ |
80 |
if (size() == 0) return; |
675 |
|||
676 |
80 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
|
677 |
|||
678 |
✓✗ | 80 |
distance_(obj, callback, min_dist); |
679 |
} |
||
680 |
|||
681 |
//============================================================================== |
||
682 |
37 |
void SaPCollisionManager::collide(CollisionCallBackBase* callback) const { |
|
683 |
37 |
callback->init(); |
|
684 |
✓✓ | 37 |
if (size() == 0) return; |
685 |
|||
686 |
✓✓ | 34 |
for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; |
687 |
5 |
++it) { |
|
688 |
6 |
CollisionObject* obj1 = it->obj1; |
|
689 |
6 |
CollisionObject* obj2 = it->obj2; |
|
690 |
|||
691 |
✓✗✓✓ |
6 |
if ((*callback)(obj1, obj2)) return; |
692 |
} |
||
693 |
} |
||
694 |
|||
695 |
//============================================================================== |
||
696 |
6 |
void SaPCollisionManager::distance(DistanceCallBackBase* callback) const { |
|
697 |
✓✗ | 6 |
callback->init(); |
698 |
✓✗✗✓ |
6 |
if (size() == 0) return; |
699 |
|||
700 |
6 |
this->enable_tested_set_ = true; |
|
701 |
6 |
this->tested_set.clear(); |
|
702 |
|||
703 |
6 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
|
704 |
|||
705 |
✓✓ | 2996 |
for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { |
706 |
✓✗✗✓ |
2990 |
if (distance_((*it)->obj, callback, min_dist)) break; |
707 |
} |
||
708 |
|||
709 |
6 |
this->enable_tested_set_ = false; |
|
710 |
6 |
this->tested_set.clear(); |
|
711 |
} |
||
712 |
|||
713 |
//============================================================================== |
||
714 |
void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, |
||
715 |
CollisionCallBackBase* callback) const { |
||
716 |
callback->init(); |
||
717 |
SaPCollisionManager* other_manager = |
||
718 |
static_cast<SaPCollisionManager*>(other_manager_); |
||
719 |
|||
720 |
if ((size() == 0) || (other_manager->size() == 0)) return; |
||
721 |
|||
722 |
if (this == other_manager) { |
||
723 |
collide(callback); |
||
724 |
return; |
||
725 |
} |
||
726 |
|||
727 |
if (this->size() < other_manager->size()) { |
||
728 |
for (auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it) { |
||
729 |
if (other_manager->collide_((*it)->obj, callback)) return; |
||
730 |
} |
||
731 |
} else { |
||
732 |
for (auto it = other_manager->AABB_arr.cbegin(), |
||
733 |
end = other_manager->AABB_arr.cend(); |
||
734 |
it != end; ++it) { |
||
735 |
if (collide_((*it)->obj, callback)) return; |
||
736 |
} |
||
737 |
} |
||
738 |
} |
||
739 |
|||
740 |
//============================================================================== |
||
741 |
void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, |
||
742 |
DistanceCallBackBase* callback) const { |
||
743 |
callback->init(); |
||
744 |
SaPCollisionManager* other_manager = |
||
745 |
static_cast<SaPCollisionManager*>(other_manager_); |
||
746 |
|||
747 |
if ((size() == 0) || (other_manager->size() == 0)) return; |
||
748 |
|||
749 |
if (this == other_manager) { |
||
750 |
distance(callback); |
||
751 |
return; |
||
752 |
} |
||
753 |
|||
754 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
||
755 |
|||
756 |
if (this->size() < other_manager->size()) { |
||
757 |
for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) { |
||
758 |
if (other_manager->distance_((*it)->obj, callback, min_dist)) return; |
||
759 |
} |
||
760 |
} else { |
||
761 |
for (auto it = other_manager->AABB_arr.cbegin(), |
||
762 |
end = other_manager->AABB_arr.cend(); |
||
763 |
it != end; ++it) { |
||
764 |
if (distance_((*it)->obj, callback, min_dist)) return; |
||
765 |
} |
||
766 |
} |
||
767 |
} |
||
768 |
|||
769 |
//============================================================================== |
||
770 |
bool SaPCollisionManager::empty() const { return AABB_arr.empty(); } |
||
771 |
|||
772 |
//============================================================================== |
||
773 |
4220 |
size_t SaPCollisionManager::size() const { return AABB_arr.size(); } |
|
774 |
|||
775 |
//============================================================================== |
||
776 |
const Vec3f& SaPCollisionManager::EndPoint::getVal() const { |
||
777 |
if (minmax) |
||
778 |
return aabb->cached.max_; |
||
779 |
else |
||
780 |
return aabb->cached.min_; |
||
781 |
} |
||
782 |
|||
783 |
//============================================================================== |
||
784 |
Vec3f& SaPCollisionManager::EndPoint::getVal() { |
||
785 |
if (minmax) |
||
786 |
return aabb->cached.max_; |
||
787 |
else |
||
788 |
return aabb->cached.min_; |
||
789 |
} |
||
790 |
|||
791 |
//============================================================================== |
||
792 |
2084654 |
FCL_REAL SaPCollisionManager::EndPoint::getVal(size_t i) const { |
|
793 |
✓✓ | 2084654 |
if (minmax) |
794 |
1100829 |
return aabb->cached.max_[(int)i]; |
|
795 |
else |
||
796 |
983825 |
return aabb->cached.min_[(int)i]; |
|
797 |
} |
||
798 |
|||
799 |
//============================================================================== |
||
800 |
2123801 |
FCL_REAL& SaPCollisionManager::EndPoint::getVal(size_t i) { |
|
801 |
✓✓ | 2123801 |
if (minmax) |
802 |
2036126 |
return aabb->cached.max_[(int)i]; |
|
803 |
else |
||
804 |
87675 |
return aabb->cached.min_[(int)i]; |
|
805 |
} |
||
806 |
|||
807 |
//============================================================================== |
||
808 |
47350 |
SaPCollisionManager::SaPPair::SaPPair(CollisionObject* a, CollisionObject* b) { |
|
809 |
✓✓ | 47350 |
if (a < b) { |
810 |
25371 |
obj1 = a; |
|
811 |
25371 |
obj2 = b; |
|
812 |
} else { |
||
813 |
21979 |
obj1 = b; |
|
814 |
21979 |
obj2 = a; |
|
815 |
} |
||
816 |
47350 |
} |
|
817 |
|||
818 |
//============================================================================== |
||
819 |
20 |
bool SaPCollisionManager::SaPPair::operator==(const SaPPair& other) const { |
|
820 |
✓✓✓✗ |
20 |
return ((obj1 == other.obj1) && (obj2 == other.obj2)); |
821 |
} |
||
822 |
|||
823 |
//============================================================================== |
||
824 |
SaPCollisionManager::isUnregistered::isUnregistered(CollisionObject* obj_) |
||
825 |
: obj(obj_) {} |
||
826 |
|||
827 |
//============================================================================== |
||
828 |
bool SaPCollisionManager::isUnregistered::operator()( |
||
829 |
const SaPPair& pair) const { |
||
830 |
return (pair.obj1 == obj) || (pair.obj2 == obj); |
||
831 |
} |
||
832 |
|||
833 |
//============================================================================== |
||
834 |
SaPCollisionManager::isNotValidPair::isNotValidPair(CollisionObject* obj1_, |
||
835 |
CollisionObject* obj2_) |
||
836 |
: obj1(obj1_), obj2(obj2_) { |
||
837 |
// Do nothing |
||
838 |
} |
||
839 |
|||
840 |
//============================================================================== |
||
841 |
bool SaPCollisionManager::isNotValidPair::operator()(const SaPPair& pair) { |
||
842 |
return (pair.obj1 == obj1) && (pair.obj2 == obj2); |
||
843 |
} |
||
844 |
|||
845 |
} // namespace fcl |
||
846 |
|||
847 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |