38 #ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H 39 #define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H 47 template <
typename HashTable>
50 unsigned int default_table_size)
51 : scene_limit(
AABB(scene_min, scene_max)),
52 hash_table(new HashTable(detail::SpatialHash(scene_limit, cell_size))) {
57 template <
typename HashTable>
63 template <
typename HashTable>
71 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
72 if (!scene_limit.contain(obj_aabb))
73 objs_partially_penetrating_scene_limit.push_back(obj);
75 hash_table->insert(overlap_aabb, obj);
77 objs_outside_scene_limit.push_back(obj);
80 obj_aabb_map[obj] = obj_aabb;
84 template <
typename HashTable>
92 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
93 if (!scene_limit.contain(obj_aabb))
94 objs_partially_penetrating_scene_limit.remove(obj);
96 hash_table->remove(overlap_aabb, obj);
98 objs_outside_scene_limit.remove(obj);
101 obj_aabb_map.erase(obj);
105 template <
typename HashTable>
111 template <
typename HashTable>
114 objs_partially_penetrating_scene_limit.clear();
115 objs_outside_scene_limit.clear();
117 for (
auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
122 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
123 if (!scene_limit.contain(obj_aabb))
124 objs_partially_penetrating_scene_limit.push_back(obj);
126 hash_table->insert(overlap_aabb, obj);
128 objs_outside_scene_limit.push_back(obj);
131 obj_aabb_map[obj] = obj_aabb;
136 template <
typename HashTable>
140 const AABB& old_aabb = obj_aabb_map[updated_obj];
142 AABB old_overlap_aabb;
143 const auto is_old_aabb_overlapping =
144 scene_limit.
overlap(old_aabb, old_overlap_aabb);
145 if (is_old_aabb_overlapping)
146 hash_table->remove(old_overlap_aabb, updated_obj);
148 AABB new_overlap_aabb;
149 const auto is_new_aabb_overlapping =
150 scene_limit.
overlap(new_aabb, new_overlap_aabb);
151 if (is_new_aabb_overlapping)
152 hash_table->insert(new_overlap_aabb, updated_obj);
154 ObjectStatus old_status;
155 if (is_old_aabb_overlapping) {
156 if (scene_limit.contain(old_aabb))
159 old_status = PartiallyPenetrating;
161 old_status = Outside;
164 if (is_new_aabb_overlapping) {
165 if (scene_limit.contain(new_aabb)) {
166 if (old_status == PartiallyPenetrating) {
171 auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
172 objs_partially_penetrating_scene_limit.end(),
174 objs_partially_penetrating_scene_limit.erase(find_it);
175 }
else if (old_status == Outside) {
180 auto find_it = std::find(objs_outside_scene_limit.begin(),
181 objs_outside_scene_limit.end(), updated_obj);
182 objs_outside_scene_limit.erase(find_it);
185 if (old_status == Inside) {
190 objs_partially_penetrating_scene_limit.push_back(updated_obj);
191 }
else if (old_status == Outside) {
197 auto find_it = std::find(objs_outside_scene_limit.begin(),
198 objs_outside_scene_limit.end(), updated_obj);
199 objs_outside_scene_limit.erase(find_it);
201 objs_partially_penetrating_scene_limit.push_back(updated_obj);
205 if (old_status == Inside) {
210 objs_outside_scene_limit.push_back(updated_obj);
211 }
else if (old_status == PartiallyPenetrating) {
218 std::find(objs_partially_penetrating_scene_limit.begin(),
219 objs_partially_penetrating_scene_limit.end(), updated_obj);
220 objs_partially_penetrating_scene_limit.erase(find_it);
222 objs_outside_scene_limit.push_back(updated_obj);
226 obj_aabb_map[updated_obj] = new_aabb;
230 template <
typename HashTable>
232 const std::vector<CollisionObject*>& updated_objs) {
233 for (
size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
237 template <
typename HashTable>
241 objs_outside_scene_limit.clear();
242 obj_aabb_map.clear();
246 template <
typename HashTable>
248 std::vector<CollisionObject*>& objs_)
const {
249 objs_.resize(objs.size());
250 std::copy(objs.begin(), objs.end(), objs_.begin());
254 template <
typename HashTable>
257 if (size() == 0)
return;
258 collide_(obj, callback);
262 template <
typename HashTable>
265 if (size() == 0)
return;
266 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
267 distance_(obj, callback, min_dist);
271 template <
typename HashTable>
274 const auto& obj_aabb = obj->
getAABB();
277 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
278 const auto query_result = hash_table->query(overlap_aabb);
279 for (
const auto& obj2 : query_result) {
280 if (obj == obj2)
continue;
282 if ((*callback)(obj, obj2))
return true;
285 if (!scene_limit.contain(obj_aabb)) {
286 for (
const auto& obj2 : objs_outside_scene_limit) {
287 if (obj == obj2)
continue;
289 if ((*callback)(obj, obj2))
return true;
293 for (
const auto& obj2 : objs_partially_penetrating_scene_limit) {
294 if (obj == obj2)
continue;
296 if ((*callback)(obj, obj2))
return true;
299 for (
const auto& obj2 : objs_outside_scene_limit) {
300 if (obj == obj2)
continue;
302 if ((*callback)(obj, obj2))
return true;
310 template <
typename HashTable>
316 if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
317 Vec3f min_dist_delta(min_dist, min_dist, min_dist);
318 aabb.expand(min_dist_delta);
327 old_min_distance = min_dist;
329 if (scene_limit.overlap(aabb, overlap_aabb)) {
330 if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
331 callback, min_dist)) {
335 if (!scene_limit.contain(aabb)) {
336 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
342 if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
343 callback, min_dist)) {
347 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
354 if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
357 if (min_dist < old_min_distance) {
358 Vec3f min_dist_delta(min_dist, min_dist, min_dist);
368 }
else if (status == 0) {
377 template <
typename HashTable>
380 if (size() == 0)
return;
382 for (
const auto& obj1 : objs) {
383 const auto& obj_aabb = obj1->getAABB();
386 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
387 auto query_result = hash_table->query(overlap_aabb);
388 for (
const auto& obj2 : query_result) {
390 if ((*callback)(obj1, obj2))
return;
394 if (!scene_limit.contain(obj_aabb)) {
395 for (
const auto& obj2 : objs_outside_scene_limit) {
397 if ((*callback)(obj1, obj2))
return;
402 for (
const auto& obj2 : objs_partially_penetrating_scene_limit) {
404 if ((*callback)(obj1, obj2))
return;
408 for (
const auto& obj2 : objs_outside_scene_limit) {
410 if ((*callback)(obj1, obj2))
return;
418 template <
typename HashTable>
421 if (size() == 0)
return;
423 this->enable_tested_set_ =
true;
424 this->tested_set.clear();
426 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
428 for (
const auto& obj : objs) {
429 if (distance_(obj, callback, min_dist))
break;
432 this->enable_tested_set_ =
false;
433 this->tested_set.clear();
437 template <
typename HashTable>
441 auto* other_manager =
444 if ((size() == 0) || (other_manager->size() == 0))
return;
446 if (
this == other_manager) {
451 if (this->size() < other_manager->size()) {
452 for (
const auto& obj : objs) {
453 if (other_manager->collide_(obj, callback))
return;
456 for (
const auto& obj : other_manager->objs) {
457 if (collide_(obj, callback))
return;
463 template <
typename HashTable>
467 auto* other_manager =
470 if ((size() == 0) || (other_manager->size() == 0))
return;
472 if (
this == other_manager) {
477 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
479 if (this->size() < other_manager->size()) {
480 for (
const auto& obj : objs)
481 if (other_manager->distance_(obj, callback, min_dist))
return;
483 for (
const auto& obj : other_manager->objs)
484 if (distance_(obj, callback, min_dist))
return;
489 template <
typename HashTable>
495 template <
typename HashTable>
501 template <
typename HashTable>
503 std::vector<CollisionObject*>& objs,
Vec3f& l,
Vec3f& u) {
505 for (
unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
512 template <
typename HashTable>
513 template <
typename Container>
517 for (
auto& obj2 : objs) {
518 if (obj == obj2)
continue;
520 if (!this->enable_tested_set_) {
522 if ((*callback)(obj, obj2, min_dist))
return true;
525 if (!this->inTestedSet(obj, obj2)) {
527 if ((*callback)(obj, obj2, min_dist))
return true;
530 this->insertTestedSet(obj, obj2);
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_spatialhash-inl.h:106
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: AABB.h:110
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
Main namespace.
Definition: broadphase_bruteforce.h:44
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
void clear()
clear the manager
Definition: broadphase_spatialhash-inl.h:238
static void computeBound(std::vector< CollisionObject *> &objs, Vec3f &l, Vec3f &u)
compute the bound for the environent
Definition: broadphase_spatialhash-inl.h:502
SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f &scene_min, const Vec3f &scene_max, unsigned int default_table_size=1000)
Definition: broadphase_spatialhash-inl.h:48
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:55
virtual void update()
update the condition of manager
Definition: broadphase_spatialhash-inl.h:112
FCL_REAL distance(const Matrix3f &R0, const Vec3f &T0, const kIOS &b1, const kIOS &b2, Vec3f *P=NULL, Vec3f *Q=NULL)
Approximate distance between two kIOS bounding volumes.
size_t size() const
the number of objects managed by the manager
Definition: broadphase_spatialhash-inl.h:496
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 ot the manager ...
Definition: broadphase_spatialhash-inl.h:263
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Definition: broadphase_collision_manager.h:88
bool empty() const
whether the manager is empty
Definition: broadphase_spatialhash-inl.h:490
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
bool distance_(CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
perform distance computation between one object and all the objects belonging ot the manager ...
Definition: broadphase_spatialhash-inl.h:311
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
void unregisterObject(CollisionObject *obj)
remove one object from the manager
Definition: broadphase_spatialhash-inl.h:85
std::size_t collide(const CollisionObject *o1, const CollisionObject *o2, const CollisionRequest &request, CollisionResult &result)
Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function performs the collision between them. Return value is the number of contacts generated between the two objects.
HashTable * hash_table
objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table ...
Definition: broadphase_spatialhash.h:153
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
~SpatialHashingCollisionManager()
Definition: broadphase_spatialhash-inl.h:58
AABB & expand(const Vec3f &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: AABB.h:201
bool collide_(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_spatialhash-inl.h:272
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...
Definition: collision_object.h:215
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_spatialhash-inl.h:255
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: broadphase_spatialhash-inl.h:64