broadphase_SaP-inl.h
Go to the documentation of this file.
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 
38 #ifndef FCL_BROAD_PHASE_SAP_INL_H
39 #define FCL_BROAD_PHASE_SAP_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT SaPCollisionManager<double>;
49 
50 //==============================================================================
51 template <typename S>
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>
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>
111 {
112  clear();
113 }
114 
115 //==============================================================================
116 template <typename S>
117 void SaPCollisionManager<S>::registerObjects(const std::vector<CollisionObject<S>*>& other_objs)
118 {
119  if(other_objs.empty()) return;
120 
121  if(size() > 0)
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
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>
874 {
875  return AABB_arr.size();
876 }
877 
878 //==============================================================================
879 template <typename S>
880 size_t SaPCollisionManager<S>::size() const
881 {
882  return AABB_arr.size();
883 }
884 
885 //==============================================================================
886 template <typename S>
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>
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>
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>
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>
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>
946 SaPCollisionManager<S>::isUnregistered::isUnregistered(CollisionObject<S>* obj_) : obj(obj_)
947 {}
948 
949 //==============================================================================
950 template <typename S>
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>
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>
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
fcl::SaPCollisionManager::optimal_axis
size_t optimal_axis
Definition: broadphase_SaP.h:143
fcl::SaPCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
fcl::SaPCollisionManager::size
size_t size() const
the number of objects managed by the manager
fcl::SaPCollisionManager::removeFromOverlapPairs
void removeFromOverlapPairs(const SaPPair &p)
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
fcl::SaPCollisionManager::distance_
bool distance_(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback, S &min_dist) const
fcl::SaPCollisionManager::update
void update()
update the condition of manager
fcl::minmax
template void minmax(double a, double b, double &minv, double &maxv)
fcl::SaPCollisionManager::AABB_arr
std::list< SaPAABB * > AABB_arr
SAP interval list.
Definition: broadphase_SaP.h:138
fcl::BroadPhaseCollisionManager::tested_set
std::set< std::pair< CollisionObject< S > *, CollisionObject< S > * > > tested_set
tools help to avoid repeating collision or distance callback for the pairs of objects tested before....
Definition: broadphase_collision_manager.h:127
fcl::SaPCollisionManager::updateVelist
void updateVelist()
testing::internal::operator==
bool operator==(T *ptr, const linked_ptr< T > &x)
Definition: gtest-linked_ptr.h:213
fcl::BroadPhaseCollisionManager::registerObjects
virtual void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
Definition: broadphase_collision_manager-inl.h:68
fcl::BroadPhaseCollisionManager::inTestedSet
bool inTestedSet(CollisionObject< S > *a, CollisionObject< S > *b) const
Definition: broadphase_collision_manager-inl.h:96
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
fcl::SaPCollisionManager< double >
template class FCL_EXPORT SaPCollisionManager< double >
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::SaPCollisionManager::getObjects
void getObjects(std::vector< CollisionObject< S > * > &objs) const
return the objects managed by the manager
fcl::SaPCollisionManager::overlap_pairs
std::list< SaPPair > overlap_pairs
The pair of objects that should further check for collision.
Definition: broadphase_SaP.h:141
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::SaPCollisionManager::unregisterObject
void unregisterObject(CollisionObject< S > *obj)
add one object to the manager
fcl::SaPCollisionManager::collide
void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager
fcl::SaPCollisionManager::SaPCollisionManager
SaPCollisionManager()
fcl::SaPCollisionManager::empty
bool empty() const
whether the manager is empty
fcl::SaPCollisionManager::distance
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager
fcl::SaPCollisionManager::clear
void clear()
clear the manager
fcl::SaPCollisionManager::isUnregistered
class FCL_EXPORT isUnregistered
Functor to help unregister one object.
Definition: broadphase_SaP.h:122
fcl::BroadPhaseCollisionManager::insertTestedSet
void insertTestedSet(CollisionObject< S > *a, CollisionObject< S > *b) const
Definition: broadphase_collision_manager-inl.h:105
broadphase_SaP.h
fcl::SaPCollisionManager::obj_aabb_map
std::map< CollisionObject< S > *, SaPAABB * > obj_aabb_map
Definition: broadphase_SaP.h:145
fcl::SaPCollisionManager::collide_
bool collide_(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
fcl::SaPCollisionManager::addToOverlapPairs
void addToOverlapPairs(const SaPPair &p)
fcl::SaPCollisionManager::elist
EndPoint * elist[3]
End point list for x, y, z coordinates.
Definition: broadphase_SaP.h:132
aabb
SaPCollisionManager< S >::SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:184
fcl::SaPCollisionManager::~SaPCollisionManager
~SaPCollisionManager()
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::SaPCollisionManager::registerObjects
void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
fcl::BroadPhaseCollisionManager::enable_tested_set_
bool enable_tested_set_
Definition: broadphase_collision_manager.h:128
fcl::SaPCollisionManager::registerObject
void registerObject(CollisionObject< S > *obj)
remove one object from the manager
fcl::SaPCollisionManager::update_
void update_(SaPAABB *updated_aabb)
fcl::SaPCollisionManager::velist
std::vector< EndPoint * > velist[3]
vector version of elist, for acceleration
Definition: broadphase_SaP.h:135


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:49