84 auto pos_start1 = objs_x.begin();
86 std::upper_bound(pos_start1, objs_x.end(), &dummyHigh,
SortByXLow());
88 while (pos_start1 < pos_end1) {
89 if (*pos_start1 == obj) {
90 objs_x.erase(pos_start1);
96 auto pos_start2 = objs_y.begin();
98 std::upper_bound(pos_start2, objs_y.end(), &dummyHigh,
SortByYLow());
100 while (pos_start2 < pos_end2) {
101 if (*pos_start2 == obj) {
102 objs_y.erase(pos_start2);
108 auto pos_start3 = objs_z.begin();
110 std::upper_bound(pos_start3, objs_z.end(), &dummyHigh,
SortByZLow());
112 while (pos_start3 < pos_end3) {
113 if (*pos_start3 == obj) {
114 objs_z.erase(pos_start3);
160 std::vector<CollisionObject*>& objs)
const {
161 objs.resize(
objs_x.size());
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)
173 if ((*pos_start)->getAABB().overlap(obj->
getAABB())) {
174 if ((*callback)(*pos_start, obj))
return true;
184 typename std::vector<CollisionObject*>::const_iterator pos_start,
185 typename std::vector<CollisionObject*>::const_iterator pos_end,
188 while (pos_start < pos_end) {
189 if (*pos_start != obj)
191 if ((*pos_start)->getAABB().distance(obj->
getAABB()) < min_dist) {
192 if ((*callback)(*pos_start, obj, min_dist))
return true;
205 if (
size() == 0)
return;
213 static const unsigned int CUTOFF = 100;
216 bool coll_res =
false;
218 const auto pos_start1 =
objs_x.begin();
219 const auto pos_end1 =
221 long d1 = pos_end1 - pos_start1;
224 const auto pos_start2 =
objs_y.begin();
225 const auto pos_end2 =
227 long d2 = pos_end2 - pos_start2;
230 const auto pos_start3 =
objs_z.begin();
231 const auto pos_end3 =
233 long d3 = pos_end3 - pos_start3;
236 if (d3 <= d2 && d3 <= d1)
237 coll_res =
checkColl(pos_start3, pos_end3, obj, callback);
239 if (d2 <= d3 && d2 <= d1)
240 coll_res =
checkColl(pos_start2, pos_end2, obj, callback);
242 coll_res =
checkColl(pos_start1, pos_end1, obj, callback);
245 coll_res =
checkColl(pos_start3, pos_end3, obj, callback);
247 coll_res =
checkColl(pos_start2, pos_end2, obj, callback);
249 coll_res =
checkColl(pos_start1, pos_end1, obj, callback);
258 if (
size() == 0)
return;
260 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
268 static const unsigned int CUTOFF = 100;
271 if (min_dist < (std::numeric_limits<FCL_REAL>::max)())
272 dummy_vector +=
Vec3f(min_dist, min_dist, min_dist);
274 typename std::vector<CollisionObject*>::const_iterator pos_start1 =
276 typename std::vector<CollisionObject*>::const_iterator pos_start2 =
278 typename std::vector<CollisionObject*>::const_iterator pos_start3 =
280 typename std::vector<CollisionObject*>::const_iterator pos_end1 =
282 typename std::vector<CollisionObject*>::const_iterator pos_end2 =
284 typename std::vector<CollisionObject*>::const_iterator pos_end3 =
291 old_min_distance = min_dist;
296 long d1 = pos_end1 - pos_start1;
298 bool dist_res =
false;
303 long d2 = pos_end2 - pos_start2;
306 pos_end3 = std::upper_bound(pos_start3,
objs_z.end(), &dummyHigh,
308 long d3 = pos_end3 - pos_start3;
311 if (d3 <= d2 && d3 <= d1)
312 dist_res =
checkDis(pos_start3, pos_end3, obj, callback, min_dist);
314 if (d2 <= d3 && d2 <= d1)
316 checkDis(pos_start2, pos_end2, obj, callback, min_dist);
319 checkDis(pos_start1, pos_end1, obj, callback, min_dist);
322 dist_res =
checkDis(pos_start3, pos_end3, obj, callback, min_dist);
324 dist_res =
checkDis(pos_start2, pos_end2, obj, callback, min_dist);
326 dist_res =
checkDis(pos_start1, pos_end1, obj, callback, min_dist);
329 if (dist_res)
return true;
332 if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
337 if (min_dist < old_min_distance) {
343 if (dummy_vector.isApprox(
346 dummy_vector = dummy_vector + delta;
348 dummy_vector = dummy_vector * 2 - obj->
getAABB().
max_;
356 }
else if (status == 0)
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];
379 if (delta_y > delta_x && delta_y > delta_z)
381 else if (delta_z > delta_y && delta_z > delta_x)
386 it_beg = objs_x.begin();
387 it_end = objs_x.end();
390 it_beg = objs_y.begin();
391 it_end = objs_y.end();
394 it_beg = objs_z.begin();
395 it_end = objs_z.end();
405 if (
size() == 0)
return;
407 typename std::vector<CollisionObject*>::const_iterator
pos, run_pos, pos_end;
409 int axis2 = (axis + 1 > 2) ? 0 : (axis + 1);
410 int axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1);
414 while ((run_pos < pos_end) && (pos < pos_end)) {
418 if ((*run_pos)->getAABB().min_[axis] < obj->
getAABB().
min_[axis]) {
420 if (run_pos == pos_end)
break;
428 if (run_pos < pos_end) {
429 typename std::vector<CollisionObject*>::const_iterator run_pos2 = run_pos;
431 while ((*run_pos2)->getAABB().min_[axis] <= obj->
getAABB().
max_[axis]) {
439 if ((*callback)(obj, obj2))
return;
443 if (run_pos2 == pos_end)
break;
452 if (
size() == 0)
return;
454 typename std::vector<CollisionObject*>::const_iterator it, it_end;
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;
470 if ((
size() == 0) || (other_manager->
size() == 0))
return;
472 if (
this == other_manager) {
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;
482 for (it = other_manager->
objs_x.begin(), end = other_manager->
objs_x.end();
484 if (
collide_(*it, callback))
return;
495 if ((
size() == 0) || (other_manager->
size() == 0))
return;
497 if (
this == other_manager) {
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;
508 for (it = other_manager->
objs_x.begin(), end = other_manager->
objs_x.end();
510 if (
distance_(*it, callback, min_dist))
return;
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
Vec3f min_
The min point in the AABB.
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.
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...
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.
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...
Vec3f max_
The max point in the AABB.
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
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