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_SAP_INL_H
39 #define FCL_BROAD_PHASE_SAP_INL_H
40
41 #include "fcl/broadphase/broadphase_SaP.h"
42
43 namespace fcl
44 {
45
46 //==============================================================================
47 extern template
48 class FCL_EXPORT SaPCollisionManager<double>;
49
50 //==============================================================================
51 template <typename S>
unregisterObject(CollisionObject<S> * obj)52 void SaPCollisionManager<S>::unregisterObject(CollisionObject<S>* obj)
53 {
54 auto it = AABB_arr.begin();
55 for(auto end = AABB_arr.end(); it != end; ++it)
56 {
57 if((*it)->obj == obj)
58 break;
59 }
60
61 AABB_arr.erase(it);
62 obj_aabb_map.erase(obj);
63
64 if(it == AABB_arr.end())
65 return;
66
67 SaPAABB* curr = *it;
68 *it = nullptr;
69
70 for(int coord = 0; coord < 3; ++coord)
71 {
72 //first delete the lo endpoint of the interval.
73 if(curr->lo->prev[coord] == nullptr)
74 elist[coord] = curr->lo->next[coord];
75 else
76 curr->lo->prev[coord]->next[coord] = curr->lo->next[coord];
77
78 curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord];
79
80 //then, delete the "hi" endpoint.
81 if(curr->hi->prev[coord] == nullptr)
82 elist[coord] = curr->hi->next[coord];
83 else
84 curr->hi->prev[coord]->next[coord] = curr->hi->next[coord];
85
86 if(curr->hi->next[coord] != nullptr)
87 curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord];
88 }
89
90 delete curr->lo;
91 delete curr->hi;
92 delete curr;
93
94 overlap_pairs.remove_if(isUnregistered(obj));
95 }
96 \
97 //==============================================================================
98 template <typename S>
SaPCollisionManager()99 SaPCollisionManager<S>::SaPCollisionManager()
100 {
101 elist[0] = nullptr;
102 elist[1] = nullptr;
103 elist[2] = nullptr;
104
105 optimal_axis = 0;
106 }
107
108 //==============================================================================
109 template <typename S>
~SaPCollisionManager()110 SaPCollisionManager<S>::~SaPCollisionManager()
111 {
112 clear();
113 }
114
115 //==============================================================================
116 template <typename S>
registerObjects(const std::vector<CollisionObject<S> * > & other_objs)117 void SaPCollisionManager<S>::registerObjects(const std::vector<CollisionObject<S>*>& other_objs)
118 {
119 if(other_objs.empty()) return;
120
121 if(size() > 0)
122 BroadPhaseCollisionManager<S>::registerObjects(other_objs);
123 else
124 {
125 std::vector<EndPoint*> endpoints(2 * other_objs.size());
126
127 for(size_t i = 0; i < other_objs.size(); ++i)
128 {
129 SaPAABB* sapaabb = new SaPAABB();
130 sapaabb->obj = other_objs[i];
131 sapaabb->lo = new EndPoint();
132 sapaabb->hi = new EndPoint();
133 sapaabb->cached = other_objs[i]->getAABB();
134 endpoints[2 * i] = sapaabb->lo;
135 endpoints[2 * i + 1] = sapaabb->hi;
136 sapaabb->lo->minmax = 0;
137 sapaabb->hi->minmax = 1;
138 sapaabb->lo->aabb = sapaabb;
139 sapaabb->hi->aabb = sapaabb;
140 AABB_arr.push_back(sapaabb);
141 obj_aabb_map[other_objs[i]] = sapaabb;
142 }
143
144
145 S scale[3];
146 for(size_t coord = 0; coord < 3; ++coord)
147 {
148 std::sort(endpoints.begin(), endpoints.end(),
149 std::bind(std::less<S>(),
150 std::bind(static_cast<S (EndPoint::*)(size_t) const >(&EndPoint::getVal), std::placeholders::_1, coord),
151 std::bind(static_cast<S (EndPoint::*)(size_t) const >(&EndPoint::getVal), std::placeholders::_2, coord)));
152
153 endpoints[0]->prev[coord] = nullptr;
154 endpoints[0]->next[coord] = endpoints[1];
155 for(size_t i = 1; i < endpoints.size() - 1; ++i)
156 {
157 endpoints[i]->prev[coord] = endpoints[i-1];
158 endpoints[i]->next[coord] = endpoints[i+1];
159 }
160 endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2];
161 endpoints[endpoints.size() - 1]->next[coord] = nullptr;
162
163 elist[coord] = endpoints[0];
164
165 scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord];
166 }
167
168 int axis = 0;
169 if(scale[axis] < scale[1]) axis = 1;
170 if(scale[axis] < scale[2]) axis = 2;
171
172 EndPoint* pos = elist[axis];
173
174 while(pos != nullptr)
175 {
176 EndPoint* pos_next = nullptr;
177 SaPAABB* aabb = pos->aabb;
178 EndPoint* pos_it = pos->next[axis];
179
180 while(pos_it != nullptr)
181 {
182 if(pos_it->aabb == aabb)
183 {
184 if(pos_next == nullptr) pos_next = pos_it;
185 break;
186 }
187
188 if(pos_it->minmax == 0)
189 {
190 if(pos_next == nullptr) pos_next = pos_it;
191 if(pos_it->aabb->cached.overlap(aabb->cached))
192 overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj);
193 }
194 pos_it = pos_it->next[axis];
195 }
196
197 pos = pos_next;
198 }
199 }
200
201 updateVelist();
202 }
203
204 //==============================================================================
205 template <typename S>
registerObject(CollisionObject<S> * obj)206 void SaPCollisionManager<S>::registerObject(CollisionObject<S>* obj)
207 {
208 SaPAABB* curr = new SaPAABB;
209 curr->cached = obj->getAABB();
210 curr->obj = obj;
211 curr->lo = new EndPoint;
212 curr->lo->minmax = 0;
213 curr->lo->aabb = curr;
214
215 curr->hi = new EndPoint;
216 curr->hi->minmax = 1;
217 curr->hi->aabb = curr;
218
219 for(int coord = 0; coord < 3; ++coord)
220 {
221 EndPoint* current = elist[coord];
222
223 // first insert the lo end point
224 if(current == nullptr) // empty list
225 {
226 elist[coord] = curr->lo;
227 curr->lo->prev[coord] = curr->lo->next[coord] = nullptr;
228 }
229 else // otherwise, find the correct location in the list and insert
230 {
231 EndPoint* curr_lo = curr->lo;
232 S curr_lo_val = curr_lo->getVal()[coord];
233 while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != nullptr))
234 current = current->next[coord];
235
236 if(current->getVal()[coord] >= curr_lo_val)
237 {
238 curr_lo->prev[coord] = current->prev[coord];
239 curr_lo->next[coord] = current;
240 if(current->prev[coord] == nullptr)
241 elist[coord] = curr_lo;
242 else
243 current->prev[coord]->next[coord] = curr_lo;
244
245 current->prev[coord] = curr_lo;
246 }
247 else
248 {
249 curr_lo->prev[coord] = current;
250 curr_lo->next[coord] = nullptr;
251 current->next[coord] = curr_lo;
252 }
253 }
254
255 // now insert hi end point
256 current = curr->lo;
257
258 EndPoint* curr_hi = curr->hi;
259 S curr_hi_val = curr_hi->getVal()[coord];
260
261 if(coord == 0)
262 {
263 while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr))
264 {
265 if(current != curr->lo)
266 if(current->aabb->cached.overlap(curr->cached))
267 overlap_pairs.emplace_back(current->aabb->obj, obj);
268
269 current = current->next[coord];
270 }
271 }
272 else
273 {
274 while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != nullptr))
275 current = current->next[coord];
276 }
277
278 if(current->getVal()[coord] >= curr_hi_val)
279 {
280 curr_hi->prev[coord] = current->prev[coord];
281 curr_hi->next[coord] = current;
282 if(current->prev[coord] == nullptr)
283 elist[coord] = curr_hi;
284 else
285 current->prev[coord]->next[coord] = curr_hi;
286
287 current->prev[coord] = curr_hi;
288 }
289 else
290 {
291 curr_hi->prev[coord] = current;
292 curr_hi->next[coord] = nullptr;
293 current->next[coord] = curr_hi;
294 }
295 }
296
297 AABB_arr.push_back(curr);
298
299 obj_aabb_map[obj] = curr;
300
301 updateVelist();
302 }
303
304 //==============================================================================
305 template <typename S>
setup()306 void SaPCollisionManager<S>::setup()
307 {
308 if(size() == 0) return;
309
310 S scale[3];
311 scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
312 scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);;
313 scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
314 size_t axis = 0;
315 if(scale[axis] < scale[1]) axis = 1;
316 if(scale[axis] < scale[2]) axis = 2;
317 optimal_axis = axis;
318 }
319
320 //==============================================================================
321 template <typename S>
update_(SaPAABB * updated_aabb)322 void SaPCollisionManager<S>::update_(SaPAABB* updated_aabb)
323 {
324 if(updated_aabb->cached.equal(updated_aabb->obj->getAABB()))
325 return;
326
327 SaPAABB* current = updated_aabb;
328
329 Vector3<S> new_min = current->obj->getAABB().min_;
330 Vector3<S> new_max = current->obj->getAABB().max_;
331
332 SaPAABB dummy;
333 dummy.cached = current->obj->getAABB();
334
335 for(int coord = 0; coord < 3; ++coord)
336 {
337 int direction; // -1 reverse, 0 nochange, 1 forward
338 EndPoint* temp;
339
340 if(current->lo->getVal(coord) > new_min[coord])
341 direction = -1;
342 else if(current->lo->getVal(coord) < new_min[coord])
343 direction = 1;
344 else direction = 0;
345
346 if(direction == -1)
347 {
348 //first update the "lo" endpoint of the interval
349 if(current->lo->prev[coord] != nullptr)
350 {
351 temp = current->lo;
352 while((temp != nullptr) && (temp->getVal(coord) > new_min[coord]))
353 {
354 if(temp->minmax == 1)
355 if(temp->aabb->cached.overlap(dummy.cached))
356 addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
357 temp = temp->prev[coord];
358 }
359
360 if(temp == nullptr)
361 {
362 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
363 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
364 current->lo->prev[coord] = nullptr;
365 current->lo->next[coord] = elist[coord];
366 elist[coord]->prev[coord] = current->lo;
367 elist[coord] = current->lo;
368 }
369 else
370 {
371 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
372 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
373 current->lo->prev[coord] = temp;
374 current->lo->next[coord] = temp->next[coord];
375 temp->next[coord]->prev[coord] = current->lo;
376 temp->next[coord] = current->lo;
377 }
378 }
379
380 current->lo->getVal(coord) = new_min[coord];
381
382 // update hi end point
383 temp = current->hi;
384 while(temp->getVal(coord) > new_max[coord])
385 {
386 if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached)))
387 removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
388 temp = temp->prev[coord];
389 }
390
391 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
392 if(current->hi->next[coord] != nullptr)
393 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
394 current->hi->prev[coord] = temp;
395 current->hi->next[coord] = temp->next[coord];
396 if(temp->next[coord] != nullptr)
397 temp->next[coord]->prev[coord] = current->hi;
398 temp->next[coord] = current->hi;
399
400 current->hi->getVal(coord) = new_max[coord];
401 }
402 else if(direction == 1)
403 {
404 //here, we first update the "hi" endpoint.
405 if(current->hi->next[coord] != nullptr)
406 {
407 temp = current->hi;
408 while((temp->next[coord] != nullptr) && (temp->getVal(coord) < new_max[coord]))
409 {
410 if(temp->minmax == 0)
411 if(temp->aabb->cached.overlap(dummy.cached))
412 addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
413 temp = temp->next[coord];
414 }
415
416 if(temp->getVal(coord) < new_max[coord])
417 {
418 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
419 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
420 current->hi->prev[coord] = temp;
421 current->hi->next[coord] = nullptr;
422 temp->next[coord] = current->hi;
423 }
424 else
425 {
426 current->hi->prev[coord]->next[coord] = current->hi->next[coord];
427 current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
428 current->hi->prev[coord] = temp->prev[coord];
429 current->hi->next[coord] = temp;
430 temp->prev[coord]->next[coord] = current->hi;
431 temp->prev[coord] = current->hi;
432 }
433 }
434
435 current->hi->getVal(coord) = new_max[coord];
436
437 //then, update the "lo" endpoint of the interval.
438 temp = current->lo;
439
440 while(temp->getVal(coord) < new_min[coord])
441 {
442 if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached)))
443 removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
444 temp = temp->next[coord];
445 }
446
447 if(current->lo->prev[coord] != nullptr)
448 current->lo->prev[coord]->next[coord] = current->lo->next[coord];
449 else
450 elist[coord] = current->lo->next[coord];
451 current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
452 current->lo->prev[coord] = temp->prev[coord];
453 current->lo->next[coord] = temp;
454 if(temp->prev[coord] != nullptr)
455 temp->prev[coord]->next[coord] = current->lo;
456 else
457 elist[coord] = current->lo;
458 temp->prev[coord] = current->lo;
459 current->lo->getVal(coord) = new_min[coord];
460 }
461 }
462 }
463
464 //==============================================================================
465 template <typename S>
updateVelist()466 void SaPCollisionManager<S>::updateVelist()
467 {
468 for(int coord = 0; coord < 3; ++coord)
469 {
470 velist[coord].resize(size() * 2);
471 EndPoint* current = elist[coord];
472 size_t id = 0;
473 while(current)
474 {
475 velist[coord][id] = current;
476 current = current->next[coord];
477 id++;
478 }
479 }
480 }
481
482 //==============================================================================
483 template <typename S>
update(CollisionObject<S> * updated_obj)484 void SaPCollisionManager<S>::update(CollisionObject<S>* updated_obj)
485 {
486 update_(obj_aabb_map[updated_obj]);
487
488 updateVelist();
489
490 setup();
491 }
492
493 //==============================================================================
494 template <typename S>
update(const std::vector<CollisionObject<S> * > & updated_objs)495 void SaPCollisionManager<S>::update(const std::vector<CollisionObject<S>*>& updated_objs)
496 {
497 for(size_t i = 0; i < updated_objs.size(); ++i)
498 update_(obj_aabb_map[updated_objs[i]]);
499
500 updateVelist();
501
502 setup();
503 }
504
505 //==============================================================================
506 template <typename S>
update()507 void SaPCollisionManager<S>::update()
508 {
509 for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
510 {
511 update_(*it);
512 }
513
514 updateVelist();
515
516 setup();
517 }
518
519 //==============================================================================
520 template <typename S>
clear()521 void SaPCollisionManager<S>::clear()
522 {
523 for(auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it)
524 {
525 delete (*it)->hi;
526 delete (*it)->lo;
527 delete *it;
528 *it = nullptr;
529 }
530
531 AABB_arr.clear();
532 overlap_pairs.clear();
533
534 elist[0] = nullptr;
535 elist[1] = nullptr;
536 elist[2] = nullptr;
537
538 velist[0].clear();
539 velist[1].clear();
540 velist[2].clear();
541
542 obj_aabb_map.clear();
543 }
544
545 //==============================================================================
546 template <typename S>
getObjects(std::vector<CollisionObject<S> * > & objs)547 void SaPCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
548 {
549 objs.resize(AABB_arr.size());
550 int i = 0;
551 for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it, ++i)
552 {
553 objs[i] = (*it)->obj;
554 }
555 }
556
557 //==============================================================================
558 template <typename S>
collide_(CollisionObject<S> * obj,void * cdata,CollisionCallBack<S> callback)559 bool SaPCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
560 {
561 size_t axis = optimal_axis;
562 const AABB<S>& obj_aabb = obj->getAABB();
563
564 S min_val = obj_aabb.min_[axis];
565 // S max_val = obj_aabb.max_[axis];
566
567 EndPoint dummy;
568 SaPAABB dummy_aabb;
569 dummy_aabb.cached = obj_aabb;
570 dummy.minmax = 1;
571 dummy.aabb = &dummy_aabb;
572
573 // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
574 const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
575 std::bind(std::less<S>(),
576 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_1, axis),
577 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_2, axis)));
578
579 EndPoint* end_pos = nullptr;
580 if(res_it != velist[axis].end())
581 end_pos = *res_it;
582
583 EndPoint* pos = elist[axis];
584
585 while(pos != end_pos)
586 {
587 if(pos->aabb->obj != obj)
588 {
589 if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
590 {
591 if(pos->aabb->cached.overlap(obj->getAABB()))
592 if(callback(obj, pos->aabb->obj, cdata))
593 return true;
594 }
595 }
596 pos = pos->next[axis];
597 }
598
599 return false;
600 }
601
602 //==============================================================================
603 template <typename S>
addToOverlapPairs(const typename SaPCollisionManager<S>::SaPPair & p)604 void SaPCollisionManager<S>::addToOverlapPairs(
605 const typename SaPCollisionManager<S>::SaPPair& p)
606 {
607 bool repeated = false;
608 for(auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it)
609 {
610 if(*it == p)
611 {
612 repeated = true;
613 break;
614 }
615 }
616
617 if(!repeated)
618 overlap_pairs.push_back(p);
619 }
620
621 //==============================================================================
622 template <typename S>
removeFromOverlapPairs(const typename SaPCollisionManager<S>::SaPPair & p)623 void SaPCollisionManager<S>::removeFromOverlapPairs(
624 const typename SaPCollisionManager<S>::SaPPair& p)
625 {
626 for(auto it = overlap_pairs.begin(), end = overlap_pairs.end();
627 it != end;
628 ++it)
629 {
630 if(*it == p)
631 {
632 overlap_pairs.erase(it);
633 break;
634 }
635 }
636
637 // or overlap_pairs.remove_if(isNotValidPair(p));
638 }
639
640 //==============================================================================
641 template <typename S>
collide(CollisionObject<S> * obj,void * cdata,CollisionCallBack<S> callback)642 void SaPCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
643 {
644 if(size() == 0) return;
645
646 collide_(obj, cdata, callback);
647 }
648
649 //==============================================================================
650 template <typename S>
distance_(CollisionObject<S> * obj,void * cdata,DistanceCallBack<S> callback,S & min_dist)651 bool SaPCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
652 {
653 Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
654 AABB<S> aabb = obj->getAABB();
655
656 if(min_dist < std::numeric_limits<S>::max())
657 {
658 Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
659 aabb.expand(min_dist_delta);
660 }
661
662 size_t axis = optimal_axis;
663
664 int status = 1;
665 S old_min_distance;
666
667 EndPoint* start_pos = elist[axis];
668
669 while(1)
670 {
671 old_min_distance = min_dist;
672 S min_val = aabb.min_[axis];
673 // S max_val = aabb.max_[axis];
674
675 EndPoint dummy;
676 SaPAABB dummy_aabb;
677 dummy_aabb.cached = aabb;
678 dummy.minmax = 1;
679 dummy.aabb = &dummy_aabb;
680
681
682 const auto res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
683 std::bind(std::less<S>(),
684 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_1, axis),
685 std::bind(static_cast<S (EndPoint::*)(size_t) const>(&EndPoint::getVal), std::placeholders::_2, axis)));
686
687 EndPoint* end_pos = nullptr;
688 if(res_it != velist[axis].end())
689 end_pos = *res_it;
690
691 EndPoint* pos = start_pos;
692
693 while(pos != end_pos)
694 {
695 // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos.
696 // but this seems slower.
697 if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val))
698 {
699 CollisionObject<S>* curr_obj = pos->aabb->obj;
700 if(curr_obj != obj)
701 {
702 if(!this->enable_tested_set_)
703 {
704 if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
705 {
706 if(callback(curr_obj, obj, cdata, min_dist))
707 return true;
708 }
709 }
710 else
711 {
712 if(!this->inTestedSet(curr_obj, obj))
713 {
714 if(pos->aabb->cached.distance(obj->getAABB()) < min_dist)
715 {
716 if(callback(curr_obj, obj, cdata, min_dist))
717 return true;
718 }
719
720 this->insertTestedSet(curr_obj, obj);
721 }
722 }
723 }
724 }
725
726 pos = pos->next[axis];
727 }
728
729 if(status == 1)
730 {
731 if(old_min_distance < std::numeric_limits<S>::max())
732 break;
733 else
734 {
735 if(min_dist < old_min_distance)
736 {
737 Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
738 aabb = AABB<S>(obj->getAABB(), min_dist_delta);
739 status = 0;
740 }
741 else
742 {
743 if(aabb.equal(obj->getAABB()))
744 aabb.expand(delta);
745 else
746 aabb.expand(obj->getAABB(), 2.0);
747 }
748 }
749 }
750 else if(status == 0)
751 break;
752 }
753
754 return false;
755 }
756
757 //==============================================================================
758 template <typename S>
distance(CollisionObject<S> * obj,void * cdata,DistanceCallBack<S> callback)759 void SaPCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
760 {
761 if(size() == 0) return;
762
763 S min_dist = std::numeric_limits<S>::max();
764
765 distance_(obj, cdata, callback, min_dist);
766 }
767
768 //==============================================================================
769 template <typename S>
collide(void * cdata,CollisionCallBack<S> callback)770 void SaPCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
771 {
772 if(size() == 0) return;
773
774 for(auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end; ++it)
775 {
776 CollisionObject<S>* obj1 = it->obj1;
777 CollisionObject<S>* obj2 = it->obj2;
778
779 if(callback(obj1, obj2, cdata))
780 return;
781 }
782 }
783
784 //==============================================================================
785 template <typename S>
distance(void * cdata,DistanceCallBack<S> callback)786 void SaPCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
787 {
788 if(size() == 0) return;
789
790 this->enable_tested_set_ = true;
791 this->tested_set.clear();
792
793 S min_dist = std::numeric_limits<S>::max();
794
795 for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
796 {
797 if(distance_((*it)->obj, cdata, callback, min_dist))
798 break;
799 }
800
801 this->enable_tested_set_ = false;
802 this->tested_set.clear();
803 }
804
805 //==============================================================================
806 template <typename S>
collide(BroadPhaseCollisionManager<S> * other_manager_,void * cdata,CollisionCallBack<S> callback)807 void SaPCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
808 {
809 SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
810
811 if((size() == 0) || (other_manager->size() == 0)) return;
812
813 if(this == other_manager)
814 {
815 collide(cdata, callback);
816 return;
817 }
818
819 if(this->size() < other_manager->size())
820 {
821 for(auto it = AABB_arr.cbegin(); it != AABB_arr.cend(); ++it)
822 {
823 if(other_manager->collide_((*it)->obj, cdata, callback))
824 return;
825 }
826 }
827 else
828 {
829 for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it)
830 {
831 if(collide_((*it)->obj, cdata, callback))
832 return;
833 }
834 }
835 }
836
837 //==============================================================================
838 template <typename S>
distance(BroadPhaseCollisionManager<S> * other_manager_,void * cdata,DistanceCallBack<S> callback)839 void SaPCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
840 {
841 SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
842
843 if((size() == 0) || (other_manager->size() == 0)) return;
844
845 if(this == other_manager)
846 {
847 distance(cdata, callback);
848 return;
849 }
850
851 S min_dist = std::numeric_limits<S>::max();
852
853 if(this->size() < other_manager->size())
854 {
855 for(auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it)
856 {
857 if(other_manager->distance_((*it)->obj, cdata, callback, min_dist))
858 return;
859 }
860 }
861 else
862 {
863 for(auto it = other_manager->AABB_arr.cbegin(), end = other_manager->AABB_arr.cend(); it != end; ++it)
864 {
865 if(distance_((*it)->obj, cdata, callback, min_dist))
866 return;
867 }
868 }
869 }
870
871 //==============================================================================
872 template <typename S>
empty()873 bool SaPCollisionManager<S>::empty() const
874 {
875 return AABB_arr.size();
876 }
877
878 //==============================================================================
879 template <typename S>
size()880 size_t SaPCollisionManager<S>::size() const
881 {
882 return AABB_arr.size();
883 }
884
885 //==============================================================================
886 template <typename S>
getVal()887 const Vector3<S>&SaPCollisionManager<S>::EndPoint::getVal() const
888 {
889 if(minmax) return aabb->cached.max_;
890 else return aabb->cached.min_;
891 }
892
893 //==============================================================================
894 template <typename S>
getVal()895 Vector3<S>&SaPCollisionManager<S>::EndPoint::getVal()
896 {
897 if(minmax) return aabb->cached.max_;
898 else return aabb->cached.min_;
899 }
900
901 //==============================================================================
902 template <typename S>
getVal(size_t i)903 S SaPCollisionManager<S>::EndPoint::getVal(size_t i) const
904 {
905 if(minmax)
906 return aabb->cached.max_[i];
907 else
908 return aabb->cached.min_[i];
909 }
910
911 //==============================================================================
912 template <typename S>
getVal(size_t i)913 S& SaPCollisionManager<S>::EndPoint::getVal(size_t i)
914 {
915 if(minmax)
916 return aabb->cached.max_[i];
917 else
918 return aabb->cached.min_[i];
919 }
920
921 //==============================================================================
922 template <typename S>
SaPPair(CollisionObject<S> * a,CollisionObject<S> * b)923 SaPCollisionManager<S>::SaPPair::SaPPair(CollisionObject<S>* a, CollisionObject<S>* b)
924 {
925 if(a < b)
926 {
927 obj1 = a;
928 obj2 = b;
929 }
930 else
931 {
932 obj1 = b;
933 obj2 = a;
934 }
935 }
936
937 //==============================================================================
938 template <typename S>
939 bool SaPCollisionManager<S>::SaPPair::operator ==(const typename SaPCollisionManager<S>::SaPPair& other) const
940 {
941 return ((obj1 == other.obj1) && (obj2 == other.obj2));
942 }
943
944 //==============================================================================
945 template <typename S>
isUnregistered(CollisionObject<S> * obj_)946 SaPCollisionManager<S>::isUnregistered::isUnregistered(CollisionObject<S>* obj_) : obj(obj_)
947 {}
948
949 //==============================================================================
950 template <typename S>
operator()951 bool SaPCollisionManager<S>::isUnregistered::operator()(const SaPPair& pair) const
952 {
953 return (pair.obj1 == obj) || (pair.obj2 == obj);
954 }
955
956 //==============================================================================
957 template <typename S>
isNotValidPair(CollisionObject<S> * obj1_,CollisionObject<S> * obj2_)958 SaPCollisionManager<S>::isNotValidPair::isNotValidPair(CollisionObject<S>* obj1_, CollisionObject<S>* obj2_) : obj1(obj1_),
959 obj2(obj2_)
960 {
961 // Do nothing
962 }
963
964 //==============================================================================
965 template <typename S>
operator()966 bool SaPCollisionManager<S>::isNotValidPair::operator()(const SaPPair& pair)
967 {
968 return (pair.obj1 == obj1) && (pair.obj2 == obj2);
969 }
970
971 } // namespace fcl
972
973 #endif
974