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