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>
84 template <
typename HashTable>
105 template <
typename HashTable>
111 template <
typename HashTable>
117 for (
auto it =
objs.cbegin(), end =
objs.cend(); it != end; ++it) {
136 template <
typename HashTable>
142 AABB old_overlap_aabb;
143 const auto is_old_aabb_overlapping =
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 =
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) {
159 old_status = PartiallyPenetrating;
161 old_status = Outside;
164 if (is_new_aabb_overlapping) {
166 if (old_status == PartiallyPenetrating) {
175 }
else if (old_status == Outside) {
185 if (old_status == Inside) {
191 }
else if (old_status == Outside) {
205 if (old_status == Inside) {
211 }
else if (old_status == PartiallyPenetrating) {
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>
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;
262 template <
typename HashTable>
265 if (
size() == 0)
return;
266 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
271 template <
typename HashTable>
274 const auto& obj_aabb = obj->
getAABB();
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;
287 if (obj == obj2)
continue;
289 if ((*callback)(obj, obj2))
return true;
294 if (obj == obj2)
continue;
296 if ((*callback)(obj, obj2))
return true;
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;
330 if (distanceObjectToObjects(obj,
hash_table->query(overlap_aabb),
331 callback, min_dist)) {
343 callback, min_dist)) {
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();
387 auto query_result =
hash_table->query(overlap_aabb);
388 for (
const auto& obj2 : query_result) {
390 if ((*callback)(obj1, obj2))
return;
397 if ((*callback)(obj1, obj2))
return;
404 if ((*callback)(obj1, obj2))
return;
410 if ((*callback)(obj1, obj2))
return;
418 template <
typename HashTable>
421 if (
size() == 0)
return;
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;
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>
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;
522 if ((*callback)(obj, obj2, min_dist))
return true;
527 if ((*callback)(obj, obj2, min_dist))
return true;
bool enable_tested_set_
Definition: broadphase_collision_manager.h:131
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
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
std::list< CollisionObject * > objs_partially_penetrating_scene_limit
objects partially penetrating (not totally inside nor outside) the scene limit are in another list ...
Definition: broadphase_spatialhash.h:139
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
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...
Definition: AABB.h:54
AABB scene_limit
the size of the scene
Definition: broadphase_spatialhash.h:145
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
std::list< CollisionObject * > objs_outside_scene_limit
objects outside the scene limit are in another list
Definition: broadphase_spatialhash.h:142
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
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
std::list< CollisionObject * > objs
all objects in the scene
Definition: broadphase_spatialhash.h:135
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
~SpatialHashingCollisionManager()
Definition: broadphase_spatialhash-inl.h:58
bool contain(const Vec3f &p) const
Check whether the AABB contains a point.
Definition: AABB.h:101
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
std::set< std::pair< CollisionObject *, CollisionObject * > > tested_set
tools help to avoid repeating collision or distance callback for the pairs of objects tested before...
Definition: broadphase_collision_manager.h:130
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
bool inTestedSet(CollisionObject *a, CollisionObject *b) const
std::map< CollisionObject *, AABB > obj_aabb_map
store the map between objects and their aabbs. will make update more convenient
Definition: broadphase_spatialhash.h:149
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: broadphase_spatialhash-inl.h:64