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 FCL_BROAD_PHASE_SSAP_INL_H
39 #define FCL_BROAD_PHASE_SSAP_INL_H
40
41 #include "fcl/broadphase/broadphase_SSaP.h"
42
43 namespace fcl
44 {
45
46 //==============================================================================
47 extern template
48 class FCL_EXPORT SSaPCollisionManager<double>;
49
50 /** @brief Functor sorting objects according to the AABB<S> lower x bound */
51 template <typename S>
52 struct SortByXLow
53 {
operatorSortByXLow54 bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
55 {
56 if(a->getAABB().min_[0] < b->getAABB().min_[0])
57 return true;
58 return false;
59 }
60 };
61
62 /** @brief Functor sorting objects according to the AABB<S> lower y bound */
63 template <typename S>
64 struct SortByYLow
65 {
operatorSortByYLow66 bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
67 {
68 if(a->getAABB().min_[1] < b->getAABB().min_[1])
69 return true;
70 return false;
71 }
72 };
73
74 /** @brief Functor sorting objects according to the AABB<S> lower z bound */
75 template <typename S>
76 struct SortByZLow
77 {
operatorSortByZLow78 bool operator()(const CollisionObject<S>* a, const CollisionObject<S>* b) const
79 {
80 if(a->getAABB().min_[2] < b->getAABB().min_[2])
81 return true;
82 return false;
83 }
84 };
85
86 /** @brief Dummy collision object with a point AABB<S> */
87 template <typename S>
88 class FCL_EXPORT DummyCollisionObject : public CollisionObject<S>
89 {
90 public:
DummyCollisionObject(const AABB<S> & aabb_)91 DummyCollisionObject(const AABB<S>& aabb_) : CollisionObject<S>(std::shared_ptr<CollisionGeometry<S>>())
92 {
93 this->aabb = aabb_;
94 }
95
computeLocalAABB()96 void computeLocalAABB() {}
97 };
98
99 //==============================================================================
100 template <typename S>
unregisterObject(CollisionObject<S> * obj)101 void SSaPCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
102 {
103 setup();
104
105 DummyCollisionObject<S> dummyHigh(AABB<S>(obj->getAABB().max_));
106
107 auto pos_start1 = objs_x.begin();
108 auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
109
110 while(pos_start1 < pos_end1)
111 {
112 if(*pos_start1 == obj)
113 {
114 objs_x.erase(pos_start1);
115 break;
116 }
117 ++pos_start1;
118 }
119
120 auto pos_start2 = objs_y.begin();
121 auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
122
123 while(pos_start2 < pos_end2)
124 {
125 if(*pos_start2 == obj)
126 {
127 objs_y.erase(pos_start2);
128 break;
129 }
130 ++pos_start2;
131 }
132
133 auto pos_start3 = objs_z.begin();
134 auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
135
136 while(pos_start3 < pos_end3)
137 {
138 if(*pos_start3 == obj)
139 {
140 objs_z.erase(pos_start3);
141 break;
142 }
143 ++pos_start3;
144 }
145 }
146
147 //==============================================================================
148 template <typename S>
SSaPCollisionManager()149 SSaPCollisionManager<S>::SSaPCollisionManager() : setup_(false)
150 {
151 // Do nothing
152 }
153
154 //==============================================================================
155 template <typename S>
registerObject(CollisionObject<S> * obj)156 void SSaPCollisionManager<S>::registerObject(CollisionObject<S>* obj)
157 {
158 objs_x.push_back(obj);
159 objs_y.push_back(obj);
160 objs_z.push_back(obj);
161 setup_ = false;
162 }
163
164 //==============================================================================
165 template <typename S>
setup()166 void SSaPCollisionManager<S>::setup()
167 {
168 if(!setup_)
169 {
170 std::sort(objs_x.begin(), objs_x.end(), SortByXLow<S>());
171 std::sort(objs_y.begin(), objs_y.end(), SortByYLow<S>());
172 std::sort(objs_z.begin(), objs_z.end(), SortByZLow<S>());
173 setup_ = true;
174 }
175 }
176
177 //==============================================================================
178 template <typename S>
update()179 void SSaPCollisionManager<S>::update()
180 {
181 setup_ = false;
182 setup();
183 }
184
185 //==============================================================================
186 template <typename S>
clear()187 void SSaPCollisionManager<S>::clear()
188 {
189 objs_x.clear();
190 objs_y.clear();
191 objs_z.clear();
192 setup_ = false;
193 }
194
195 //==============================================================================
196 template <typename S>
getObjects(std::vector<CollisionObject<S> * > & objs)197 void SSaPCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
198 {
199 objs.resize(objs_x.size());
200 std::copy(objs_x.begin(), objs_x.end(), objs.begin());
201 }
202
203 //==============================================================================
204 template <typename S>
checkColl(typename std::vector<CollisionObject<S> * >::const_iterator pos_start,typename std::vector<CollisionObject<S> * >::const_iterator pos_end,CollisionObject<S> * obj,void * cdata,CollisionCallBack<S> callback)205 bool SSaPCollisionManager<S>::checkColl(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end,
206 CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
207 {
208 while(pos_start < pos_end)
209 {
210 if(*pos_start != obj) // no collision between the same object
211 {
212 if((*pos_start)->getAABB().overlap(obj->getAABB()))
213 {
214 if(callback(*pos_start, obj, cdata))
215 return true;
216 }
217 }
218 pos_start++;
219 }
220 return false;
221 }
222
223 //==============================================================================
224 template <typename S>
checkDis(typename std::vector<CollisionObject<S> * >::const_iterator pos_start,typename std::vector<CollisionObject<S> * >::const_iterator pos_end,CollisionObject<S> * obj,void * cdata,DistanceCallBack<S> callback,S & min_dist)225 bool SSaPCollisionManager<S>::checkDis(typename std::vector<CollisionObject<S>*>::const_iterator pos_start, typename std::vector<CollisionObject<S>*>::const_iterator pos_end, CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
226 {
227 while(pos_start < pos_end)
228 {
229 if(*pos_start != obj) // no distance between the same object
230 {
231 if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist)
232 {
233 if(callback(*pos_start, obj, cdata, min_dist))
234 return true;
235 }
236 }
237 pos_start++;
238 }
239
240 return false;
241 }
242
243 //==============================================================================
244 template <typename S>
collide(CollisionObject<S> * obj,void * cdata,CollisionCallBack<S> callback)245 void SSaPCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
246 {
247 if(size() == 0) return;
248
249 collide_(obj, cdata, callback);
250 }
251
252 //==============================================================================
253 template <typename S>
collide_(CollisionObject<S> * obj,void * cdata,CollisionCallBack<S> callback)254 bool SSaPCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
255 {
256 static const unsigned int CUTOFF = 100;
257
258 DummyCollisionObject<S> dummyHigh(AABB<S>(obj->getAABB().max_));
259 bool coll_res = false;
260
261 const auto pos_start1 = objs_x.begin();
262 const auto pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
263 unsigned int d1 = pos_end1 - pos_start1;
264
265 if(d1 > CUTOFF)
266 {
267 const auto pos_start2 = objs_y.begin();
268 const auto pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
269 unsigned int d2 = pos_end2 - pos_start2;
270
271 if(d2 > CUTOFF)
272 {
273 const auto pos_start3 = objs_z.begin();
274 const auto pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
275 unsigned int d3 = pos_end3 - pos_start3;
276
277 if(d3 > CUTOFF)
278 {
279 if(d3 <= d2 && d3 <= d1)
280 coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
281 else
282 {
283 if(d2 <= d3 && d2 <= d1)
284 coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
285 else
286 coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
287 }
288 }
289 else
290 coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback);
291 }
292 else
293 coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback);
294 }
295 else
296 coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback);
297
298 return coll_res;
299 }
300
301 //==============================================================================
302 template <typename S>
distance(CollisionObject<S> * obj,void * cdata,DistanceCallBack<S> callback)303 void SSaPCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
304 {
305 if(size() == 0) return;
306
307 S min_dist = std::numeric_limits<S>::max();
308 distance_(obj, cdata, callback, min_dist);
309 }
310
311 //==============================================================================
312 template <typename S>
distance_(CollisionObject<S> * obj,void * cdata,DistanceCallBack<S> callback,S & min_dist)313 bool SSaPCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
314 {
315 static const unsigned int CUTOFF = 100;
316 Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
317 Vector3<S> dummy_vector = obj->getAABB().max_;
318 if(min_dist < std::numeric_limits<S>::max())
319 dummy_vector += Vector3<S>(min_dist, min_dist, min_dist);
320
321 typename std::vector<CollisionObject<S>*>::const_iterator pos_start1 = objs_x.begin();
322 typename std::vector<CollisionObject<S>*>::const_iterator pos_start2 = objs_y.begin();
323 typename std::vector<CollisionObject<S>*>::const_iterator pos_start3 = objs_z.begin();
324 typename std::vector<CollisionObject<S>*>::const_iterator pos_end1 = objs_x.end();
325 typename std::vector<CollisionObject<S>*>::const_iterator pos_end2 = objs_y.end();
326 typename std::vector<CollisionObject<S>*>::const_iterator pos_end3 = objs_z.end();
327
328 int status = 1;
329 S old_min_distance;
330
331 while(1)
332 {
333 old_min_distance = min_dist;
334 DummyCollisionObject<S> dummyHigh((AABB<S>(dummy_vector)));
335
336 pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow<S>());
337 unsigned int d1 = pos_end1 - pos_start1;
338
339 bool dist_res = false;
340
341 if(d1 > CUTOFF)
342 {
343 pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow<S>());
344 unsigned int d2 = pos_end2 - pos_start2;
345
346 if(d2 > CUTOFF)
347 {
348 pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow<S>());
349 unsigned int d3 = pos_end3 - pos_start3;
350
351 if(d3 > CUTOFF)
352 {
353 if(d3 <= d2 && d3 <= d1)
354 dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
355 else
356 {
357 if(d2 <= d3 && d2 <= d1)
358 dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
359 else
360 dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
361 }
362 }
363 else
364 dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist);
365 }
366 else
367 dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist);
368 }
369 else
370 {
371 dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist);
372 }
373
374 if(dist_res) return true;
375
376 if(status == 1)
377 {
378 if(old_min_distance < std::numeric_limits<S>::max())
379 break;
380 else
381 {
382 // from infinity to a finite one, only need one additional loop
383 // to check the possible missed ones to the right of the objs array
384 if(min_dist < old_min_distance)
385 {
386 dummy_vector = obj->getAABB().max_ + Vector3<S>(min_dist, min_dist, min_dist);
387 status = 0;
388 }
389 else // need more loop
390 {
391 if(dummy_vector.isApprox(obj->getAABB().max_, std::numeric_limits<S>::epsilon() * 100))
392 dummy_vector = dummy_vector + delta;
393 else
394 dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
395 }
396 }
397
398 // yes, following is wrong, will result in too large distance.
399 // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
400 // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
401 // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
402 }
403 else if(status == 0)
404 break;
405 }
406
407 return false;
408 }
409
410 //==============================================================================
411 template <typename S>
selectOptimalAxis(const std::vector<CollisionObject<S> * > & objs_x,const std::vector<CollisionObject<S> * > & objs_y,const std::vector<CollisionObject<S> * > & objs_z,typename std::vector<CollisionObject<S> * >::const_iterator & it_beg,typename std::vector<CollisionObject<S> * >::const_iterator & it_end)412 size_t SSaPCollisionManager<S>::selectOptimalAxis(
413 const std::vector<CollisionObject<S>*>& objs_x,
414 const std::vector<CollisionObject<S>*>& objs_y,
415 const std::vector<CollisionObject<S>*>& objs_z,
416 typename std::vector<CollisionObject<S>*>::const_iterator& it_beg,
417 typename std::vector<CollisionObject<S>*>::const_iterator& it_end)
418 {
419 /// simple sweep and prune method
420 S delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0];
421 S delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1];
422 S delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2];
423
424 int axis = 0;
425 if(delta_y > delta_x && delta_y > delta_z)
426 axis = 1;
427 else if(delta_z > delta_y && delta_z > delta_x)
428 axis = 2;
429
430 switch(axis)
431 {
432 case 0:
433 it_beg = objs_x.begin();
434 it_end = objs_x.end();
435 break;
436 case 1:
437 it_beg = objs_y.begin();
438 it_end = objs_y.end();
439 break;
440 case 2:
441 it_beg = objs_z.begin();
442 it_end = objs_z.end();
443 break;
444 }
445
446 return axis;
447 }
448
449 //==============================================================================
450 template <typename S>
collide(void * cdata,CollisionCallBack<S> callback)451 void SSaPCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
452 {
453 if(size() == 0) return;
454
455 typename std::vector<CollisionObject<S>*>::const_iterator pos, run_pos, pos_end;
456 size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
457 pos, pos_end);
458 size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
459 size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
460
461 run_pos = pos;
462
463 while((run_pos < pos_end) && (pos < pos_end))
464 {
465 CollisionObject<S>* obj = *(pos++);
466
467 while(1)
468 {
469 if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis])
470 {
471 run_pos++;
472 if(run_pos == pos_end) break;
473 continue;
474 }
475 else
476 {
477 run_pos++;
478 break;
479 }
480 }
481
482 if(run_pos < pos_end)
483 {
484 typename std::vector<CollisionObject<S>*>::const_iterator run_pos2 = run_pos;
485
486 while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis])
487 {
488 CollisionObject<S>* obj2 = *run_pos2;
489 run_pos2++;
490
491 if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2]))
492 {
493 if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3]))
494 {
495 if(callback(obj, obj2, cdata))
496 return;
497 }
498 }
499
500 if(run_pos2 == pos_end) break;
501 }
502 }
503 }
504 }
505
506 //==============================================================================
507 template <typename S>
distance(void * cdata,DistanceCallBack<S> callback)508 void SSaPCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
509 {
510 if(size() == 0) return;
511
512 typename std::vector<CollisionObject<S>*>::const_iterator it, it_end;
513 selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
514
515 S min_dist = std::numeric_limits<S>::max();
516 for(; it != it_end; ++it)
517 {
518 if(distance_(*it, cdata, callback, min_dist))
519 return;
520 }
521 }
522
523 //==============================================================================
524 template <typename S>
collide(BroadPhaseCollisionManager<S> * other_manager_,void * cdata,CollisionCallBack<S> callback)525 void SSaPCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
526 {
527 SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
528
529 if((size() == 0) || (other_manager->size() == 0)) return;
530
531 if(this == other_manager)
532 {
533 collide(cdata, callback);
534 return;
535 }
536
537
538 typename std::vector<CollisionObject<S>*>::const_iterator it, end;
539 if(this->size() < other_manager->size())
540 {
541 for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
542 if(other_manager->collide_(*it, cdata, callback)) return;
543 }
544 else
545 {
546 for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
547 if(collide_(*it, cdata, callback)) return;
548 }
549 }
550
551 //==============================================================================
552 template <typename S>
distance(BroadPhaseCollisionManager<S> * other_manager_,void * cdata,DistanceCallBack<S> callback)553 void SSaPCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
554 {
555 SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
556
557 if((size() == 0) || (other_manager->size() == 0)) return;
558
559 if(this == other_manager)
560 {
561 distance(cdata, callback);
562 return;
563 }
564
565 S min_dist = std::numeric_limits<S>::max();
566 typename std::vector<CollisionObject<S>*>::const_iterator it, end;
567 if(this->size() < other_manager->size())
568 {
569 for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
570 if(other_manager->distance_(*it, cdata, callback, min_dist)) return;
571 }
572 else
573 {
574 for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it)
575 if(distance_(*it, cdata, callback, min_dist)) return;
576 }
577 }
578
579 //==============================================================================
580 template <typename S>
empty()581 bool SSaPCollisionManager<S>::empty() const
582 {
583 return objs_x.empty();
584 }
585
586 //==============================================================================
587 template <typename S>
size()588 size_t SSaPCollisionManager<S>::size() const
589 {
590 return objs_x.size();
591 }
592
593 } // namespace fcl
594
595 #endif
596