broadphase_SSaP.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 
44 struct SortByXLow {
45  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
46  if (a->getAABB().min_[0] < b->getAABB().min_[0]) return true;
47  return false;
48  }
49 };
50 
52 struct SortByYLow {
53  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
54  if (a->getAABB().min_[1] < b->getAABB().min_[1]) return true;
55  return false;
56  }
57 };
58 
60 struct SortByZLow {
61  bool operator()(const CollisionObject* a, const CollisionObject* b) const {
62  if (a->getAABB().min_[2] < b->getAABB().min_[2]) return true;
63  return false;
64  }
65 };
66 
68 class HPP_FCL_DLLAPI DummyCollisionObject : public CollisionObject {
69  public:
70  DummyCollisionObject(const AABB& aabb_)
71  : CollisionObject(shared_ptr<CollisionGeometry>()) {
72  this->aabb = aabb_;
73  }
74 
75  void computeLocalAABB() {}
76 };
77 
78 //==============================================================================
80  setup();
81 
82  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
83 
84  auto pos_start1 = objs_x.begin();
85  auto pos_end1 =
86  std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
87 
88  while (pos_start1 < pos_end1) {
89  if (*pos_start1 == obj) {
90  objs_x.erase(pos_start1);
91  break;
92  }
93  ++pos_start1;
94  }
95 
96  auto pos_start2 = objs_y.begin();
97  auto pos_end2 =
98  std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
99 
100  while (pos_start2 < pos_end2) {
101  if (*pos_start2 == obj) {
102  objs_y.erase(pos_start2);
103  break;
104  }
105  ++pos_start2;
106  }
107 
108  auto pos_start3 = objs_z.begin();
109  auto pos_end3 =
110  std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
111 
112  while (pos_start3 < pos_end3) {
113  if (*pos_start3 == obj) {
114  objs_z.erase(pos_start3);
115  break;
116  }
117  ++pos_start3;
118  }
119 }
120 
121 //==============================================================================
123  // Do nothing
124 }
125 
126 //==============================================================================
128  objs_x.push_back(obj);
129  objs_y.push_back(obj);
130  objs_z.push_back(obj);
131  setup_ = false;
132 }
133 
134 //==============================================================================
136  if (!setup_) {
137  std::sort(objs_x.begin(), objs_x.end(), SortByXLow());
138  std::sort(objs_y.begin(), objs_y.end(), SortByYLow());
139  std::sort(objs_z.begin(), objs_z.end(), SortByZLow());
140  setup_ = true;
141  }
142 }
143 
144 //==============================================================================
146  setup_ = false;
147  setup();
148 }
149 
150 //==============================================================================
152  objs_x.clear();
153  objs_y.clear();
154  objs_z.clear();
155  setup_ = false;
156 }
157 
158 //==============================================================================
160  std::vector<CollisionObject*>& objs) const {
161  objs.resize(objs_x.size());
162  std::copy(objs_x.begin(), objs_x.end(), objs.begin());
163 }
164 
165 //==============================================================================
167  std::vector<CollisionObject*>::const_iterator pos_start,
168  std::vector<CollisionObject*>::const_iterator pos_end, CollisionObject* obj,
170  while (pos_start < pos_end) {
171  if (*pos_start != obj) // no collision between the same object
172  {
173  if ((*pos_start)->getAABB().overlap(obj->getAABB())) {
174  if ((*callback)(*pos_start, obj)) return true;
175  }
176  }
177  pos_start++;
178  }
179  return false;
180 }
181 
182 //==============================================================================
184  typename std::vector<CollisionObject*>::const_iterator pos_start,
185  typename std::vector<CollisionObject*>::const_iterator pos_end,
187  FCL_REAL& min_dist) const {
188  while (pos_start < pos_end) {
189  if (*pos_start != obj) // no distance between the same object
190  {
191  if ((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) {
192  if ((*callback)(*pos_start, obj, min_dist)) return true;
193  }
194  }
195  pos_start++;
196  }
197 
198  return false;
199 }
200 
201 //==============================================================================
204  callback->init();
205  if (size() == 0) return;
206 
207  collide_(obj, callback);
208 }
209 
210 //==============================================================================
213  static const unsigned int CUTOFF = 100;
214 
215  DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
216  bool coll_res = false;
217 
218  const auto pos_start1 = objs_x.begin();
219  const auto pos_end1 =
220  std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
221  long d1 = pos_end1 - pos_start1;
222 
223  if (d1 > CUTOFF) {
224  const auto pos_start2 = objs_y.begin();
225  const auto pos_end2 =
226  std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
227  long d2 = pos_end2 - pos_start2;
228 
229  if (d2 > CUTOFF) {
230  const auto pos_start3 = objs_z.begin();
231  const auto pos_end3 =
232  std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow());
233  long d3 = pos_end3 - pos_start3;
234 
235  if (d3 > CUTOFF) {
236  if (d3 <= d2 && d3 <= d1)
237  coll_res = checkColl(pos_start3, pos_end3, obj, callback);
238  else {
239  if (d2 <= d3 && d2 <= d1)
240  coll_res = checkColl(pos_start2, pos_end2, obj, callback);
241  else
242  coll_res = checkColl(pos_start1, pos_end1, obj, callback);
243  }
244  } else
245  coll_res = checkColl(pos_start3, pos_end3, obj, callback);
246  } else
247  coll_res = checkColl(pos_start2, pos_end2, obj, callback);
248  } else
249  coll_res = checkColl(pos_start1, pos_end1, obj, callback);
250 
251  return coll_res;
252 }
253 
254 //==============================================================================
257  callback->init();
258  if (size() == 0) return;
259 
260  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
261  distance_(obj, callback, min_dist);
262 }
263 
264 //==============================================================================
267  FCL_REAL& min_dist) const {
268  static const unsigned int CUTOFF = 100;
269  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
270  Vec3f dummy_vector = obj->getAABB().max_;
271  if (min_dist < (std::numeric_limits<FCL_REAL>::max)())
272  dummy_vector += Vec3f(min_dist, min_dist, min_dist);
273 
274  typename std::vector<CollisionObject*>::const_iterator pos_start1 =
275  objs_x.begin();
276  typename std::vector<CollisionObject*>::const_iterator pos_start2 =
277  objs_y.begin();
278  typename std::vector<CollisionObject*>::const_iterator pos_start3 =
279  objs_z.begin();
280  typename std::vector<CollisionObject*>::const_iterator pos_end1 =
281  objs_x.end();
282  typename std::vector<CollisionObject*>::const_iterator pos_end2 =
283  objs_y.end();
284  typename std::vector<CollisionObject*>::const_iterator pos_end3 =
285  objs_z.end();
286 
287  int status = 1;
288  FCL_REAL old_min_distance;
289 
290  while (1) {
291  old_min_distance = min_dist;
292  DummyCollisionObject dummyHigh((AABB(dummy_vector)));
293 
294  pos_end1 =
295  std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow());
296  long d1 = pos_end1 - pos_start1;
297 
298  bool dist_res = false;
299 
300  if (d1 > CUTOFF) {
301  pos_end2 =
302  std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow());
303  long d2 = pos_end2 - pos_start2;
304 
305  if (d2 > CUTOFF) {
306  pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh,
307  SortByZLow());
308  long d3 = pos_end3 - pos_start3;
309 
310  if (d3 > CUTOFF) {
311  if (d3 <= d2 && d3 <= d1)
312  dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
313  else {
314  if (d2 <= d3 && d2 <= d1)
315  dist_res =
316  checkDis(pos_start2, pos_end2, obj, callback, min_dist);
317  else
318  dist_res =
319  checkDis(pos_start1, pos_end1, obj, callback, min_dist);
320  }
321  } else
322  dist_res = checkDis(pos_start3, pos_end3, obj, callback, min_dist);
323  } else
324  dist_res = checkDis(pos_start2, pos_end2, obj, callback, min_dist);
325  } else {
326  dist_res = checkDis(pos_start1, pos_end1, obj, callback, min_dist);
327  }
328 
329  if (dist_res) return true;
330 
331  if (status == 1) {
332  if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
333  break;
334  else {
335  // from infinity to a finite one, only need one additional loop
336  // to check the possible missed ones to the right of the objs array
337  if (min_dist < old_min_distance) {
338  dummy_vector =
339  obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist);
340  status = 0;
341  } else // need more loop
342  {
343  if (dummy_vector.isApprox(
344  obj->getAABB().max_,
346  dummy_vector = dummy_vector + delta;
347  else
348  dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
349  }
350  }
351 
352  // yes, following is wrong, will result in too large distance.
353  // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1;
354  // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2;
355  // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3;
356  } else if (status == 0)
357  break;
358  }
359 
360  return false;
361 }
362 
363 //==============================================================================
365  const std::vector<CollisionObject*>& objs_x,
366  const std::vector<CollisionObject*>& objs_y,
367  const std::vector<CollisionObject*>& objs_z,
368  typename std::vector<CollisionObject*>::const_iterator& it_beg,
369  typename std::vector<CollisionObject*>::const_iterator& it_end) {
371  FCL_REAL delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] -
372  (objs_x[0])->getAABB().min_[0];
373  FCL_REAL delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] -
374  (objs_y[0])->getAABB().min_[1];
375  FCL_REAL delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] -
376  (objs_z[0])->getAABB().min_[2];
377 
378  int axis = 0;
379  if (delta_y > delta_x && delta_y > delta_z)
380  axis = 1;
381  else if (delta_z > delta_y && delta_z > delta_x)
382  axis = 2;
383 
384  switch (axis) {
385  case 0:
386  it_beg = objs_x.begin();
387  it_end = objs_x.end();
388  break;
389  case 1:
390  it_beg = objs_y.begin();
391  it_end = objs_y.end();
392  break;
393  case 2:
394  it_beg = objs_z.begin();
395  it_end = objs_z.end();
396  break;
397  }
398 
399  return axis;
400 }
401 
402 //==============================================================================
404  callback->init();
405  if (size() == 0) return;
406 
407  typename std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
408  int axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end);
409  int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
410  int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
411 
412  run_pos = pos;
413 
414  while ((run_pos < pos_end) && (pos < pos_end)) {
415  CollisionObject* obj = *(pos++);
416 
417  while (1) {
418  if ((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) {
419  run_pos++;
420  if (run_pos == pos_end) break;
421  continue;
422  } else {
423  run_pos++;
424  break;
425  }
426  }
427 
428  if (run_pos < pos_end) {
429  typename std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
430 
431  while ((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) {
432  CollisionObject* obj2 = *run_pos2;
433  run_pos2++;
434 
435  if ((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) &&
436  (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) {
437  if ((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) &&
438  (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) {
439  if ((*callback)(obj, obj2)) return;
440  }
441  }
442 
443  if (run_pos2 == pos_end) break;
444  }
445  }
446  }
447 }
448 
449 //==============================================================================
451  callback->init();
452  if (size() == 0) return;
453 
454  typename std::vector<CollisionObject*>::const_iterator it, it_end;
455  selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
456 
457  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
458  for (; it != it_end; ++it) {
459  if (distance_(*it, callback, min_dist)) return;
460  }
461 }
462 
463 //==============================================================================
466  callback->init();
467  SSaPCollisionManager* other_manager =
468  static_cast<SSaPCollisionManager*>(other_manager_);
469 
470  if ((size() == 0) || (other_manager->size() == 0)) return;
471 
472  if (this == other_manager) {
473  collide(callback);
474  return;
475  }
476 
477  typename std::vector<CollisionObject*>::const_iterator it, end;
478  if (this->size() < other_manager->size()) {
479  for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
480  if (other_manager->collide_(*it, callback)) return;
481  } else {
482  for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
483  it != end; ++it)
484  if (collide_(*it, callback)) return;
485  }
486 }
487 
488 //==============================================================================
491  callback->init();
492  SSaPCollisionManager* other_manager =
493  static_cast<SSaPCollisionManager*>(other_manager_);
494 
495  if ((size() == 0) || (other_manager->size() == 0)) return;
496 
497  if (this == other_manager) {
498  distance(callback);
499  return;
500  }
501 
502  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
503  typename std::vector<CollisionObject*>::const_iterator it, end;
504  if (this->size() < other_manager->size()) {
505  for (it = objs_x.begin(), end = objs_x.end(); it != end; ++it)
506  if (other_manager->distance_(*it, callback, min_dist)) return;
507  } else {
508  for (it = other_manager->objs_x.begin(), end = other_manager->objs_x.end();
509  it != end; ++it)
510  if (distance_(*it, callback, min_dist)) return;
511  }
512 }
513 
514 //==============================================================================
515 bool SSaPCollisionManager::empty() const { return objs_x.empty(); }
516 
517 //==============================================================================
518 size_t SSaPCollisionManager::size() const { return objs_x.size(); }
519 
520 } // namespace fcl
521 } // namespace hpp
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager ...
std::vector< CollisionObject * > objs_x
Objects sorted according to lower x value.
void unregisterObject(CollisionObject *obj)
add one object to the manager
virtual void init()
Initialization of the callback before running the collision broadphase manager.
virtual void update()
update the condition of manager
axis
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager ...
bool setup_
tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Dummy collision object with a point AABB.
pos
Definition: octree.py:8
Main namespace.
bool collide_(CollisionObject *obj, CollisionCallBackBase *callback) const
Simple SAP collision manager.
Base callback class for collision queries. This class can be supersed by child classes to provide des...
static FCL_REAL epsilon
Definition: simple.cpp:12
DummyCollisionObject(const AABB &aabb_)
bool empty() const
whether the manager is empty
bool operator()(const CollisionObject *a, const CollisionObject *b) const
virtual void init()
Initialization of the callback before running the collision broadphase manager.
bool checkColl(typename std::vector< CollisionObject *>::const_iterator pos_start, typename std::vector< CollisionObject *>::const_iterator pos_end, CollisionObject *obj, CollisionCallBackBase *callback) const
check collision between one object and a list of objects, return value is whether stop is possible ...
bool distance_(CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
void clear()
clear the manager
bool checkDis(typename std::vector< CollisionObject *>::const_iterator pos_start, typename std::vector< CollisionObject *>::const_iterator pos_end, CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
check distance between one object and a list of objects, return value is whether stop is possible ...
bool operator()(const CollisionObject *a, const CollisionObject *b) const
std::vector< CollisionObject * > objs_y
Objects sorted according to lower y value.
double FCL_REAL
Definition: data_types.h:65
Functor sorting objects according to the AABB lower x bound.
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Functor sorting objects according to the AABB lower y bound.
void registerObject(CollisionObject *obj)
remove one object from the manager
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
Functor sorting objects according to the AABB lower z bound.
bool operator()(const CollisionObject *a, const CollisionObject *b) const
void setup()
initialize the manager, related with the specific type of manager
const AABB & getAABB() const
get the AABB in world space
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
the object for collision or distance computation, contains the geometry and the transform information...
std::vector< CollisionObject * > objs_z
Objects sorted according to lower z value.
The geometry for the object for collision or distance computation.
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
static int selectOptimalAxis(const std::vector< CollisionObject *> &objs_x, const std::vector< CollisionObject *> &objs_y, const std::vector< CollisionObject *> &objs_z, typename std::vector< CollisionObject *>::const_iterator &it_beg, typename std::vector< CollisionObject *>::const_iterator &it_end)
size_t size() const
the number of objects managed by the manager


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