broadphase_SaP.cpp
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 
39 
40 namespace hpp {
41 namespace fcl {
42 
43 //==============================================================================
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 //==============================================================================
86  elist[0] = nullptr;
87  elist[1] = nullptr;
88  elist[2] = nullptr;
89 
90  optimal_axis = 0;
91 }
92 
93 //==============================================================================
95 
96 //==============================================================================
98  const std::vector<CollisionObject*>& other_objs) {
99  if (other_objs.empty()) return;
100 
101  if (size() > 0)
103  else {
104  std::vector<EndPoint*> endpoints(2 * other_objs.size());
105 
106  for (size_t i = 0; i < other_objs.size(); ++i) {
107  SaPAABB* sapaabb = new SaPAABB();
108  sapaabb->obj = other_objs[i];
109  sapaabb->lo = new EndPoint();
110  sapaabb->hi = new EndPoint();
111  sapaabb->cached = other_objs[i]->getAABB();
112  endpoints[2 * i] = sapaabb->lo;
113  endpoints[2 * i + 1] = sapaabb->hi;
114  sapaabb->lo->minmax = 0;
115  sapaabb->hi->minmax = 1;
116  sapaabb->lo->aabb = sapaabb;
117  sapaabb->hi->aabb = sapaabb;
118  AABB_arr.push_back(sapaabb);
119  obj_aabb_map[other_objs[i]] = sapaabb;
120  }
121 
122  FCL_REAL scale[3];
123  for (int coord = 0; coord < 3; ++coord) {
124  std::sort(
125  endpoints.begin(), endpoints.end(),
126  std::bind(std::less<FCL_REAL>(),
127  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
129  std::placeholders::_1, coord),
130  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
132  std::placeholders::_2, coord)));
133 
134  endpoints[0]->prev[coord] = nullptr;
135  endpoints[0]->next[coord] = endpoints[1];
136  for (size_t i = 1; i < endpoints.size() - 1; ++i) {
137  endpoints[i]->prev[coord] = endpoints[i - 1];
138  endpoints[i]->next[coord] = endpoints[i + 1];
139  }
140  endpoints[endpoints.size() - 1]->prev[coord] =
141  endpoints[endpoints.size() - 2];
142  endpoints[endpoints.size() - 1]->next[coord] = nullptr;
143 
144  elist[coord] = endpoints[0];
145 
146  scale[coord] = endpoints.back()->aabb->cached.max_[coord] -
147  endpoints[0]->aabb->cached.min_[coord];
148  }
149 
150  int axis = 0;
151  if (scale[axis] < scale[1]) axis = 1;
152  if (scale[axis] < scale[2]) axis = 2;
153 
154  EndPoint* pos = elist[axis];
155 
156  while (pos != nullptr) {
157  EndPoint* pos_next = nullptr;
158  SaPAABB* aabb = pos->aabb;
159  EndPoint* pos_it = pos->next[axis];
160 
161  while (pos_it != nullptr) {
162  if (pos_it->aabb == aabb) {
163  if (pos_next == nullptr) pos_next = pos_it;
164  break;
165  }
166 
167  if (pos_it->minmax == 0) {
168  if (pos_next == nullptr) pos_next = pos_it;
169  if (pos_it->aabb->cached.overlap(aabb->cached))
170  overlap_pairs.emplace_back(pos_it->aabb->obj, aabb->obj);
171  }
172  pos_it = pos_it->next[axis];
173  }
174 
175  pos = pos_next;
176  }
177  }
178 
179  updateVelist();
180 }
181 
182 //==============================================================================
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 //==============================================================================
273  if (size() == 0) return;
274 
275  FCL_REAL scale[3];
276  scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0);
277  scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);
278  ;
279  scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2);
280  int axis = 0;
281  if (scale[axis] < scale[1]) axis = 1;
282  if (scale[axis] < scale[2]) axis = 2;
283  optimal_axis = axis;
284 }
285 
286 //==============================================================================
288  if (updated_aabb->cached == updated_aabb->obj->getAABB()) return;
289 
290  SaPAABB* current = updated_aabb;
291 
292  Vec3f new_min = current->obj->getAABB().min_;
293  Vec3f new_max = current->obj->getAABB().max_;
294 
295  SaPAABB dummy;
296  dummy.cached = current->obj->getAABB();
297 
298  for (int coord = 0; coord < 3; ++coord) {
299  int direction; // -1 reverse, 0 nochange, 1 forward
300  EndPoint* temp;
301 
302  if (current->lo->getVal((size_t)coord) > new_min[coord])
303  direction = -1;
304  else if (current->lo->getVal((size_t)coord) < new_min[coord])
305  direction = 1;
306  else
307  direction = 0;
308 
309  if (direction == -1) {
310  // first update the "lo" endpoint of the interval
311  if (current->lo->prev[coord] != nullptr) {
312  temp = current->lo;
313  while ((temp != nullptr) &&
314  (temp->getVal((size_t)coord) > new_min[coord])) {
315  if (temp->minmax == 1)
316  if (temp->aabb->cached.overlap(dummy.cached))
317  addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
318  temp = temp->prev[coord];
319  }
320 
321  if (temp == nullptr) {
322  current->lo->prev[coord]->next[coord] = current->lo->next[coord];
323  current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
324  current->lo->prev[coord] = nullptr;
325  current->lo->next[coord] = elist[coord];
326  elist[coord]->prev[coord] = current->lo;
327  elist[coord] = current->lo;
328  } else {
329  current->lo->prev[coord]->next[coord] = current->lo->next[coord];
330  current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
331  current->lo->prev[coord] = temp;
332  current->lo->next[coord] = temp->next[coord];
333  temp->next[coord]->prev[coord] = current->lo;
334  temp->next[coord] = current->lo;
335  }
336  }
337 
338  current->lo->getVal((size_t)coord) = new_min[coord];
339 
340  // update hi end point
341  temp = current->hi;
342  while (temp->getVal((size_t)coord) > new_max[coord]) {
343  if ((temp->minmax == 0) &&
344  (temp->aabb->cached.overlap(current->cached)))
345  removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
346  temp = temp->prev[coord];
347  }
348 
349  current->hi->prev[coord]->next[coord] = current->hi->next[coord];
350  if (current->hi->next[coord] != nullptr)
351  current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
352  current->hi->prev[coord] = temp;
353  current->hi->next[coord] = temp->next[coord];
354  if (temp->next[coord] != nullptr)
355  temp->next[coord]->prev[coord] = current->hi;
356  temp->next[coord] = current->hi;
357 
358  current->hi->getVal((size_t)coord) = new_max[coord];
359  } else if (direction == 1) {
360  // here, we first update the "hi" endpoint.
361  if (current->hi->next[coord] != nullptr) {
362  temp = current->hi;
363  while ((temp->next[coord] != nullptr) &&
364  (temp->getVal((size_t)coord) < new_max[coord])) {
365  if (temp->minmax == 0)
366  if (temp->aabb->cached.overlap(dummy.cached))
367  addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
368  temp = temp->next[coord];
369  }
370 
371  if (temp->getVal((size_t)coord) < new_max[coord]) {
372  current->hi->prev[coord]->next[coord] = current->hi->next[coord];
373  current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
374  current->hi->prev[coord] = temp;
375  current->hi->next[coord] = nullptr;
376  temp->next[coord] = current->hi;
377  } else {
378  current->hi->prev[coord]->next[coord] = current->hi->next[coord];
379  current->hi->next[coord]->prev[coord] = current->hi->prev[coord];
380  current->hi->prev[coord] = temp->prev[coord];
381  current->hi->next[coord] = temp;
382  temp->prev[coord]->next[coord] = current->hi;
383  temp->prev[coord] = current->hi;
384  }
385  }
386 
387  current->hi->getVal((size_t)coord) = new_max[coord];
388 
389  // then, update the "lo" endpoint of the interval.
390  temp = current->lo;
391 
392  while (temp->getVal((size_t)coord) < new_min[coord]) {
393  if ((temp->minmax == 1) &&
394  (temp->aabb->cached.overlap(current->cached)))
395  removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj));
396  temp = temp->next[coord];
397  }
398 
399  if (current->lo->prev[coord] != nullptr)
400  current->lo->prev[coord]->next[coord] = current->lo->next[coord];
401  else
402  elist[coord] = current->lo->next[coord];
403  current->lo->next[coord]->prev[coord] = current->lo->prev[coord];
404  current->lo->prev[coord] = temp->prev[coord];
405  current->lo->next[coord] = temp;
406  if (temp->prev[coord] != nullptr)
407  temp->prev[coord]->next[coord] = current->lo;
408  else
409  elist[coord] = current->lo;
410  temp->prev[coord] = current->lo;
411  current->lo->getVal((size_t)coord) = new_min[coord];
412  }
413  }
414 }
415 
416 //==============================================================================
418  for (int coord = 0; coord < 3; ++coord) {
419  velist[coord].resize(size() * 2);
420  EndPoint* current = elist[coord];
421  size_t id = 0;
422  while (current) {
423  velist[coord][id] = current;
424  current = current->next[coord];
425  id++;
426  }
427  }
428 }
429 
430 //==============================================================================
432  update_(obj_aabb_map[updated_obj]);
433 
434  updateVelist();
435 
436  setup();
437 }
438 
439 //==============================================================================
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 //==============================================================================
452  for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
453  update_(*it);
454  }
455 
456  updateVelist();
457 
458  setup();
459 }
460 
461 //==============================================================================
463  for (auto it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) {
464  delete (*it)->hi;
465  delete (*it)->lo;
466  delete *it;
467  *it = nullptr;
468  }
469 
470  AABB_arr.clear();
471  overlap_pairs.clear();
472 
473  elist[0] = nullptr;
474  elist[1] = nullptr;
475  elist[2] = nullptr;
476 
477  velist[0].clear();
478  velist[1].clear();
479  velist[2].clear();
480 
481  obj_aabb_map.clear();
482 }
483 
484 //==============================================================================
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 //==============================================================================
498  int axis = optimal_axis;
499  const AABB& obj_aabb = obj->getAABB();
500 
501  FCL_REAL min_val = obj_aabb.min_[axis];
502  // FCL_REAL max_val = obj_aabb.max_[axis];
503 
504  EndPoint dummy;
505  SaPAABB dummy_aabb;
506  dummy_aabb.cached = obj_aabb;
507  dummy.minmax = 1;
508  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  std::bind(std::less<FCL_REAL>(),
515  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
517  std::placeholders::_1, axis),
518  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
520  std::placeholders::_2, axis)));
521 
522  EndPoint* end_pos = nullptr;
523  if (res_it != velist[axis].end()) end_pos = *res_it;
524 
525  EndPoint* pos = elist[axis];
526 
527  while (pos != end_pos) {
528  if (pos->aabb->obj != obj) {
529  if ((pos->minmax == 0) &&
530  (pos->aabb->hi->getVal((size_t)axis) >= min_val)) {
531  if (pos->aabb->cached.overlap(obj->getAABB()))
532  if ((*callback)(obj, pos->aabb->obj)) return true;
533  }
534  }
535  pos = pos->next[axis];
536  }
537 
538  return false;
539 }
540 
541 //==============================================================================
543  bool repeated = false;
544  for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end;
545  ++it) {
546  if (*it == p) {
547  repeated = true;
548  break;
549  }
550  }
551 
552  if (!repeated) overlap_pairs.push_back(p);
553 }
554 
555 //==============================================================================
557  for (auto it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end;
558  ++it) {
559  if (*it == p) {
560  overlap_pairs.erase(it);
561  break;
562  }
563  }
564 
565  // or overlap_pairs.remove_if(isNotValidPair(p));
566 }
567 
568 //==============================================================================
571  callback->init();
572  if (size() == 0) return;
573 
574  collide_(obj, callback);
575 }
576 
577 //==============================================================================
580  FCL_REAL& min_dist) const {
581  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
582  AABB aabb = obj->getAABB();
583 
584  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
585  Vec3f min_dist_delta(min_dist, min_dist, min_dist);
586  aabb.expand(min_dist_delta);
587  }
588 
589  int axis = optimal_axis;
590 
591  int status = 1;
592  FCL_REAL old_min_distance;
593 
594  EndPoint* start_pos = elist[axis];
595 
596  while (1) {
597  old_min_distance = min_dist;
598  FCL_REAL min_val = aabb.min_[axis];
599  // FCL_REAL max_val = aabb.max_[axis];
600 
601  EndPoint dummy;
602  SaPAABB dummy_aabb;
603  dummy_aabb.cached = aabb;
604  dummy.minmax = 1;
605  dummy.aabb = &dummy_aabb;
606 
607  const auto res_it = std::upper_bound(
608  velist[axis].begin(), velist[axis].end(), &dummy,
609  std::bind(std::less<FCL_REAL>(),
610  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
612  std::placeholders::_1, axis),
613  std::bind(static_cast<FCL_REAL (EndPoint::*)(size_t) const>(
615  std::placeholders::_2, axis)));
616 
617  EndPoint* end_pos = nullptr;
618  if (res_it != velist[axis].end()) end_pos = *res_it;
619 
620  EndPoint* pos = start_pos;
621 
622  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  if ((pos->minmax == 0) &&
626  (pos->aabb->hi->getVal((size_t)axis) >= min_val)) {
627  CollisionObject* curr_obj = pos->aabb->obj;
628  if (curr_obj != obj) {
629  if (!this->enable_tested_set_) {
630  if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) {
631  if ((*callback)(curr_obj, obj, min_dist)) return true;
632  }
633  } else {
634  if (!this->inTestedSet(curr_obj, obj)) {
635  if (pos->aabb->cached.distance(obj->getAABB()) < min_dist) {
636  if ((*callback)(curr_obj, obj, min_dist)) return true;
637  }
638 
639  this->insertTestedSet(curr_obj, obj);
640  }
641  }
642  }
643  }
644 
645  pos = pos->next[axis];
646  }
647 
648  if (status == 1) {
649  if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
650  break;
651  else {
652  if (min_dist < old_min_distance) {
653  Vec3f min_dist_delta(min_dist, min_dist, min_dist);
654  aabb = AABB(obj->getAABB(), min_dist_delta);
655  status = 0;
656  } else {
657  if (aabb == obj->getAABB())
658  aabb.expand(delta);
659  else
660  aabb.expand(obj->getAABB(), 2.0);
661  }
662  }
663  } else if (status == 0)
664  break;
665  }
666 
667  return false;
668 }
669 
670 //==============================================================================
673  callback->init();
674  if (size() == 0) return;
675 
676  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
677 
678  distance_(obj, callback, min_dist);
679 }
680 
681 //==============================================================================
683  callback->init();
684  if (size() == 0) return;
685 
686  for (auto it = overlap_pairs.cbegin(), end = overlap_pairs.cend(); it != end;
687  ++it) {
688  CollisionObject* obj1 = it->obj1;
689  CollisionObject* obj2 = it->obj2;
690 
691  if ((*callback)(obj1, obj2)) return;
692  }
693 }
694 
695 //==============================================================================
697  callback->init();
698  if (size() == 0) return;
699 
700  this->enable_tested_set_ = true;
701  this->tested_set.clear();
702 
703  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
704 
705  for (auto it = AABB_arr.cbegin(), end = AABB_arr.cend(); it != end; ++it) {
706  if (distance_((*it)->obj, callback, min_dist)) break;
707  }
708 
709  this->enable_tested_set_ = false;
710  this->tested_set.clear();
711 }
712 
713 //==============================================================================
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 //==============================================================================
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.size(); }
771 
772 //==============================================================================
773 size_t SaPCollisionManager::size() const { return AABB_arr.size(); }
774 
775 //==============================================================================
777  if (minmax)
778  return aabb->cached.max_;
779  else
780  return aabb->cached.min_;
781 }
782 
783 //==============================================================================
785  if (minmax)
786  return aabb->cached.max_;
787  else
788  return aabb->cached.min_;
789 }
790 
791 //==============================================================================
793  if (minmax)
794  return aabb->cached.max_[(int)i];
795  else
796  return aabb->cached.min_[(int)i];
797 }
798 
799 //==============================================================================
801  if (minmax)
802  return aabb->cached.max_[(int)i];
803  else
804  return aabb->cached.min_[(int)i];
805 }
806 
807 //==============================================================================
809  if (a < b) {
810  obj1 = a;
811  obj2 = b;
812  } else {
813  obj1 = b;
814  obj2 = a;
815  }
816 }
817 
818 //==============================================================================
820  return ((obj1 == other.obj1) && (obj2 == other.obj2));
821 }
822 
823 //==============================================================================
825  : obj(obj_) {}
826 
827 //==============================================================================
829  const SaPPair& pair) const {
830  return (pair.obj1 == obj) || (pair.obj2 == obj);
831 }
832 
833 //==============================================================================
835  CollisionObject* obj2_)
836  : obj1(obj1_), obj2(obj2_) {
837  // Do nothing
838 }
839 
840 //==============================================================================
842  return (pair.obj1 == obj1) && (pair.obj2 == obj2);
843 }
844 
845 } // namespace fcl
846 
847 } // namespace hpp
virtual void init()
Initialization of the callback before running the collision broadphase manager.
void update_(SaPAABB *updated_aabb)
axis
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: BV/AABB.h:110
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
void unregisterObject(CollisionObject *obj)
add one object to the manager
pos
Definition: octree.py:8
Main namespace.
virtual void registerObjects(const std::vector< CollisionObject *> &other_objs)
add objects to the manager
void registerObject(CollisionObject *obj)
remove one object from the manager
Base callback class for collision queries. This class can be supersed by child classes to provide des...
std::list< SaPPair > overlap_pairs
The pair of objects that should further check for collision.
SAP interval for one object.
const Vec3f & getVal() const
get the value of the end point
std::map< CollisionObject *, SaPAABB * > obj_aabb_map
SaPPair(CollisionObject *a, CollisionObject *b)
bool collide_(CollisionObject *obj, CollisionCallBackBase *callback) const
virtual void init()
Initialization of the callback before running the collision broadphase manager.
void clear()
clear the manager
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager ...
bool empty() const
whether the manager is empty
bool operator==(const SaPPair &other) const
EndPoint * next[3]
the next end point in the end point list
virtual void update()
update the condition of manager
void setup()
initialize the manager, related with the specific type of manager
double FCL_REAL
Definition: data_types.h:65
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager ...
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL &minv, FCL_REAL &maxv)
Find the smaller and larger one of two values.
Definition: kDOP.cpp:48
void addToOverlapPairs(const SaPPair &p)
Rigorous SAP collision manager.
EndPoint * elist[3]
End point list for x, y, z coordinates.
size_t size() const
the number of objects managed by the manager
std::vector< EndPoint * > velist[3]
vector version of elist, for acceleration
void insertTestedSet(CollisionObject *a, CollisionObject *b) const
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
EndPoint * hi
higher bound end point of the interval
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
std::list< SaPAABB * > AABB_arr
SAP interval list.
bool operator()(const SaPPair &pair) const
void removeFromOverlapPairs(const SaPPair &p)
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
EndPoint * prev[3]
the previous end point in the end point list
const AABB & getAABB() const
get the AABB in world space
bool distance_(CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
Functor to help unregister one object.
AABB & expand(const Vec3f &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: BV/AABB.h:201
std::set< std::pair< CollisionObject *, CollisionObject * > > tested_set
tools help to avoid repeating collision or distance callback for the pairs of objects tested before...
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
EndPoint * lo
lower bound end point of the interval
the object for collision or distance computation, contains the geometry and the transform information...
A pair of objects that are not culling away and should further check collision.
char minmax
tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi ...
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
SaPAABB * aabb
back pointer to SAP interval
bool inTestedSet(CollisionObject *a, CollisionObject *b) const
void registerObjects(const std::vector< CollisionObject *> &other_objs)
add objects to the manager
isNotValidPair(CollisionObject *obj1_, CollisionObject *obj2_)


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00