46 for (
auto end =
AABB_arr.end(); it != end; ++it) {
47 if ((*it)->obj == obj)
break;
58 for (
int coord = 0; coord < 3; ++coord) {
60 if (curr->
lo->
prev[coord] ==
nullptr)
68 if (curr->
hi->
prev[coord] ==
nullptr)
73 if (curr->
hi->
next[coord] !=
nullptr)
98 const std::vector<CollisionObject*>& other_objs) {
99 if (other_objs.empty())
return;
104 std::vector<EndPoint*> endpoints(2 * other_objs.size());
106 for (
size_t i = 0; i < other_objs.size(); ++i) {
108 sapaabb->
obj = other_objs[i];
111 sapaabb->
cached = other_objs[i]->getAABB();
112 endpoints[2 * i] = sapaabb->
lo;
113 endpoints[2 * i + 1] = sapaabb->
hi;
116 sapaabb->
lo->
aabb = sapaabb;
117 sapaabb->
hi->
aabb = sapaabb;
123 for (
int coord = 0; coord < 3; ++coord) {
125 endpoints.begin(), endpoints.end(),
126 std::bind(std::less<FCL_REAL>(),
129 std::placeholders::_1, coord),
132 std::placeholders::_2, coord)));
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];
140 endpoints[endpoints.size() - 1]->prev[coord] =
141 endpoints[endpoints.size() - 2];
142 endpoints[endpoints.size() - 1]->next[coord] =
nullptr;
144 elist[coord] = endpoints[0];
147 endpoints[0]->aabb->cached.min_[coord];
151 if (scale[axis] < scale[1]) axis = 1;
152 if (scale[axis] < scale[2]) axis = 2;
156 while (pos !=
nullptr) {
161 while (pos_it !=
nullptr) {
162 if (pos_it->
aabb == aabb) {
163 if (pos_next ==
nullptr) pos_next = pos_it;
167 if (pos_it->
minmax == 0) {
168 if (pos_next ==
nullptr) pos_next = pos_it;
172 pos_it = pos_it->
next[axis];
195 for (
int coord = 0; coord < 3; ++coord) {
199 if (current ==
nullptr)
202 curr->
lo->
prev[coord] = curr->
lo->
next[coord] =
nullptr;
207 while ((current->
getVal()[coord] < curr_lo_val) &&
208 (current->
next[coord] !=
nullptr))
209 current = current->
next[coord];
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;
217 current->
prev[coord]->
next[coord] = curr_lo;
219 current->
prev[coord] = curr_lo;
221 curr_lo->
prev[coord] = current;
222 curr_lo->
next[coord] =
nullptr;
223 current->
next[coord] = curr_lo;
234 while ((current->
getVal()[coord] < curr_hi_val) &&
235 (current->
next[coord] !=
nullptr)) {
236 if (current != curr->
lo)
240 current = current->
next[coord];
243 while ((current->
getVal()[coord] < curr_hi_val) &&
244 (current->
next[coord] !=
nullptr))
245 current = current->
next[coord];
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;
254 current->
prev[coord]->
next[coord] = curr_hi;
256 current->
prev[coord] = curr_hi;
258 curr_hi->
prev[coord] = current;
259 curr_hi->
next[coord] =
nullptr;
260 current->
next[coord] = curr_hi;
273 if (
size() == 0)
return;
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);
279 scale[2] = (
velist[2].back())->getVal(2) -
velist[2][0]->getVal(2);
281 if (scale[axis] < scale[1]) axis = 1;
282 if (scale[axis] < scale[2]) axis = 2;
290 SaPAABB* current = updated_aabb;
298 for (
int coord = 0; coord < 3; ++coord) {
302 if (current->
lo->
getVal((
size_t)coord) > new_min[coord])
304 else if (current->
lo->
getVal((
size_t)coord) < new_min[coord])
309 if (direction == -1) {
311 if (current->
lo->
prev[coord] !=
nullptr) {
313 while ((temp !=
nullptr) &&
314 (temp->
getVal((
size_t)coord) > new_min[coord])) {
318 temp = temp->
prev[coord];
321 if (temp ==
nullptr) {
324 current->
lo->
prev[coord] =
nullptr;
331 current->
lo->
prev[coord] = temp;
334 temp->
next[coord] = current->
lo;
338 current->
lo->
getVal((
size_t)coord) = new_min[coord];
342 while (temp->
getVal((
size_t)coord) > new_max[coord]) {
343 if ((temp->
minmax == 0) &&
346 temp = temp->
prev[coord];
350 if (current->
hi->
next[coord] !=
nullptr)
352 current->
hi->
prev[coord] = temp;
354 if (temp->
next[coord] !=
nullptr)
356 temp->
next[coord] = current->
hi;
358 current->
hi->
getVal((
size_t)coord) = new_max[coord];
359 }
else if (direction == 1) {
361 if (current->
hi->
next[coord] !=
nullptr) {
363 while ((temp->
next[coord] !=
nullptr) &&
364 (temp->
getVal((
size_t)coord) < new_max[coord])) {
368 temp = temp->
next[coord];
371 if (temp->
getVal((
size_t)coord) < new_max[coord]) {
374 current->
hi->
prev[coord] = temp;
375 current->
hi->
next[coord] =
nullptr;
376 temp->
next[coord] = current->
hi;
381 current->
hi->
next[coord] = temp;
383 temp->
prev[coord] = current->
hi;
387 current->
hi->
getVal((
size_t)coord) = new_max[coord];
392 while (temp->
getVal((
size_t)coord) < new_min[coord]) {
393 if ((temp->
minmax == 1) &&
396 temp = temp->
next[coord];
399 if (current->
lo->
prev[coord] !=
nullptr)
405 current->
lo->
next[coord] = temp;
406 if (temp->
prev[coord] !=
nullptr)
410 temp->
prev[coord] = current->
lo;
411 current->
lo->
getVal((
size_t)coord) = new_min[coord];
418 for (
int coord = 0; coord < 3; ++coord) {
423 velist[coord][id] = current;
424 current = current->
next[coord];
441 const std::vector<CollisionObject*>& updated_objs) {
442 for (
size_t i = 0; i < updated_objs.size(); ++i)
486 std::vector<CollisionObject*>& objs)
const {
491 objs[i] = (*it)->obj;
506 dummy_aabb.
cached = obj_aabb;
508 dummy.
aabb = &dummy_aabb;
512 const auto res_it = std::upper_bound(
514 std::bind(std::less<FCL_REAL>(),
517 std::placeholders::_1, axis),
520 std::placeholders::_2, axis)));
523 if (res_it !=
velist[axis].end()) end_pos = *res_it;
527 while (pos != end_pos) {
532 if ((*callback)(obj, pos->
aabb->
obj))
return true;
535 pos = pos->
next[axis];
543 bool repeated =
false;
572 if (
size() == 0)
return;
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);
597 old_min_distance = min_dist;
605 dummy.
aabb = &dummy_aabb;
607 const auto res_it = std::upper_bound(
609 std::bind(std::less<FCL_REAL>(),
612 std::placeholders::_1, axis),
615 std::placeholders::_2, axis)));
618 if (res_it !=
velist[axis].end()) end_pos = *res_it;
622 while (pos != end_pos) {
628 if (curr_obj != obj) {
631 if ((*callback)(curr_obj, obj, min_dist))
return true;
636 if ((*callback)(curr_obj, obj, min_dist))
return true;
645 pos = pos->
next[axis];
649 if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
652 if (min_dist < old_min_distance) {
653 Vec3f min_dist_delta(min_dist, min_dist, min_dist);
663 }
else if (status == 0)
674 if (
size() == 0)
return;
676 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
684 if (
size() == 0)
return;
691 if ((*callback)(obj1, obj2))
return;
698 if (
size() == 0)
return;
703 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
706 if (
distance_((*it)->obj, callback, min_dist))
break;
720 if ((
size() == 0) || (other_manager->
size() == 0))
return;
722 if (
this == other_manager) {
727 if (this->
size() < other_manager->
size()) {
729 if (other_manager->
collide_((*it)->obj, callback))
return;
732 for (
auto it = other_manager->
AABB_arr.cbegin(),
733 end = other_manager->
AABB_arr.cend();
735 if (
collide_((*it)->obj, callback))
return;
747 if ((
size() == 0) || (other_manager->
size() == 0))
return;
749 if (
this == other_manager) {
754 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
756 if (this->
size() < other_manager->
size()) {
758 if (other_manager->
distance_((*it)->obj, callback, min_dist))
return;
761 for (
auto it = other_manager->
AABB_arr.cbegin(),
762 end = other_manager->
AABB_arr.cend();
764 if (
distance_((*it)->obj, callback, min_dist))
return;
778 return aabb->cached.max_;
780 return aabb->cached.min_;
786 return aabb->cached.max_;
788 return aabb->cached.min_;
794 return aabb->cached.max_[(int)i];
796 return aabb->cached.min_[(int)i];
802 return aabb->cached.max_[(int)i];
804 return aabb->cached.min_[(int)i];
820 return ((obj1 == other.
obj1) && (obj2 == other.
obj2));
836 : obj1(obj1_), obj2(obj2_) {
virtual void init()
Initialization of the callback before running the collision broadphase manager.
isUnregistered(CollisionObject *obj_)
void update_(SaPAABB *updated_aabb)
Vec3f min_
The min point in the AABB.
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
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
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...
End point for an interval.
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
AABB cached
cached AABB value
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
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.
void addToOverlapPairs(const SaPPair &p)
CollisionObject * obj
object
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...
EndPoint * hi
higher bound end point of the interval
Vec3f max_
The max point in the AABB.
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.
EndPoint * prev[3]
the previous end point in the end point list
bool operator()(const SaPPair &pair)
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.
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
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_)