hpp-fcl 2.4.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
broadphase_spatialhash-inl.h
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
38#ifndef HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
39#define HPP_FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
40
42
43namespace hpp {
44namespace fcl {
45
46//==============================================================================
47template <typename HashTable>
49 FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max,
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))) {
53 hash_table->init(default_table_size);
54}
55
56//==============================================================================
57template <typename HashTable>
59 delete hash_table;
60}
61
62//==============================================================================
63template <typename HashTable>
65 CollisionObject* obj) {
66 objs.push_back(obj);
67
68 const AABB& obj_aabb = obj->getAABB();
69 AABB overlap_aabb;
70
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);
74
75 hash_table->insert(overlap_aabb, obj);
76 } else {
77 objs_outside_scene_limit.push_back(obj);
78 }
79
80 obj_aabb_map[obj] = obj_aabb;
81}
82
83//==============================================================================
84template <typename HashTable>
86 CollisionObject* obj) {
87 objs.remove(obj);
88
89 const AABB& obj_aabb = obj->getAABB();
90 AABB overlap_aabb;
91
92 if (scene_limit.overlap(obj_aabb, overlap_aabb)) {
93 if (!scene_limit.contain(obj_aabb))
94 objs_partially_penetrating_scene_limit.remove(obj);
95
96 hash_table->remove(overlap_aabb, obj);
97 } else {
98 objs_outside_scene_limit.remove(obj);
99 }
100
101 obj_aabb_map.erase(obj);
102}
103
104//==============================================================================
105template <typename HashTable>
107 // Do nothing
108}
109
110//==============================================================================
111template <typename HashTable>
113 hash_table->clear();
114 objs_partially_penetrating_scene_limit.clear();
115 objs_outside_scene_limit.clear();
116
117 for (auto it = objs.cbegin(), end = objs.cend(); it != end; ++it) {
118 CollisionObject* obj = *it;
119 const AABB& obj_aabb = obj->getAABB();
120 AABB overlap_aabb;
121
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);
125
126 hash_table->insert(overlap_aabb, obj);
127 } else {
128 objs_outside_scene_limit.push_back(obj);
129 }
130
131 obj_aabb_map[obj] = obj_aabb;
132 }
133}
134
135//==============================================================================
136template <typename HashTable>
138 CollisionObject* updated_obj) {
139 const AABB& new_aabb = updated_obj->getAABB();
140 const AABB& old_aabb = obj_aabb_map[updated_obj];
141
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);
147
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);
153
154 ObjectStatus old_status;
155 if (is_old_aabb_overlapping) {
156 if (scene_limit.contain(old_aabb))
157 old_status = Inside;
158 else
159 old_status = PartiallyPenetrating;
160 } else {
161 old_status = Outside;
162 }
163
164 if (is_new_aabb_overlapping) {
165 if (scene_limit.contain(new_aabb)) {
166 if (old_status == PartiallyPenetrating) {
167 // Status change: PartiallyPenetrating --> Inside
168 // Required action(s):
169 // - remove object from "objs_partially_penetrating_scene_limit"
170
171 auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
172 objs_partially_penetrating_scene_limit.end(),
173 updated_obj);
174 objs_partially_penetrating_scene_limit.erase(find_it);
175 } else if (old_status == Outside) {
176 // Status change: Outside --> Inside
177 // Required action(s):
178 // - remove object from "objs_outside_scene_limit"
179
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);
183 }
184 } else {
185 if (old_status == Inside) {
186 // Status change: Inside --> PartiallyPenetrating
187 // Required action(s):
188 // - add object to "objs_partially_penetrating_scene_limit"
189
190 objs_partially_penetrating_scene_limit.push_back(updated_obj);
191 } else if (old_status == Outside) {
192 // Status change: Outside --> PartiallyPenetrating
193 // Required action(s):
194 // - remove object from "objs_outside_scene_limit"
195 // - add object to "objs_partially_penetrating_scene_limit"
196
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);
200
201 objs_partially_penetrating_scene_limit.push_back(updated_obj);
202 }
203 }
204 } else {
205 if (old_status == Inside) {
206 // Status change: Inside --> Outside
207 // Required action(s):
208 // - add object to "objs_outside_scene_limit"
209
210 objs_outside_scene_limit.push_back(updated_obj);
211 } else if (old_status == PartiallyPenetrating) {
212 // Status change: PartiallyPenetrating --> Outside
213 // Required action(s):
214 // - remove object from "objs_partially_penetrating_scene_limit"
215 // - add object to "objs_outside_scene_limit"
216
217 auto find_it =
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);
221
222 objs_outside_scene_limit.push_back(updated_obj);
223 }
224 }
225
226 obj_aabb_map[updated_obj] = new_aabb;
227}
228
229//==============================================================================
230template <typename HashTable>
232 const std::vector<CollisionObject*>& updated_objs) {
233 for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
234}
235
236//==============================================================================
237template <typename HashTable>
239 objs.clear();
240 hash_table->clear();
241 objs_outside_scene_limit.clear();
242 obj_aabb_map.clear();
243}
244
245//==============================================================================
246template <typename HashTable>
248 std::vector<CollisionObject*>& objs_) const {
249 objs_.resize(objs.size());
250 std::copy(objs.begin(), objs.end(), objs_.begin());
251}
252
253//==============================================================================
254template <typename HashTable>
256 CollisionObject* obj, CollisionCallBackBase* callback) const {
257 if (size() == 0) return;
258 collide_(obj, callback);
259}
260
261//==============================================================================
262template <typename HashTable>
264 CollisionObject* obj, DistanceCallBackBase* callback) const {
265 if (size() == 0) return;
266 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
267 distance_(obj, callback, min_dist);
268}
269
270//==============================================================================
271template <typename HashTable>
273 CollisionObject* obj, CollisionCallBackBase* callback) const {
274 const auto& obj_aabb = obj->getAABB();
275 AABB overlap_aabb;
276
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;
281
282 if ((*callback)(obj, obj2)) return true;
283 }
284
285 if (!scene_limit.contain(obj_aabb)) {
286 for (const auto& obj2 : objs_outside_scene_limit) {
287 if (obj == obj2) continue;
288
289 if ((*callback)(obj, obj2)) return true;
290 }
291 }
292 } else {
293 for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
294 if (obj == obj2) continue;
295
296 if ((*callback)(obj, obj2)) return true;
297 }
298
299 for (const auto& obj2 : objs_outside_scene_limit) {
300 if (obj == obj2) continue;
301
302 if ((*callback)(obj, obj2)) return true;
303 }
304 }
305
306 return false;
307}
308
309//==============================================================================
310template <typename HashTable>
313 FCL_REAL& min_dist) const {
314 auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
315 auto aabb = obj->getAABB();
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);
319 }
320
321 AABB overlap_aabb;
322
323 auto status = 1;
324 FCL_REAL old_min_distance;
325
326 while (1) {
327 old_min_distance = min_dist;
328
329 if (scene_limit.overlap(aabb, overlap_aabb)) {
330 if (distanceObjectToObjects(obj, hash_table->query(overlap_aabb),
331 callback, min_dist)) {
332 return true;
333 }
334
335 if (!scene_limit.contain(aabb)) {
336 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
337 min_dist)) {
338 return true;
339 }
340 }
341 } else {
342 if (distanceObjectToObjects(obj, objs_partially_penetrating_scene_limit,
343 callback, min_dist)) {
344 return true;
345 }
346
347 if (distanceObjectToObjects(obj, objs_outside_scene_limit, callback,
348 min_dist)) {
349 return true;
350 }
351 }
352
353 if (status == 1) {
354 if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)()) {
355 break;
356 } else {
357 if (min_dist < old_min_distance) {
358 Vec3f min_dist_delta(min_dist, min_dist, min_dist);
359 aabb = AABB(obj->getAABB(), min_dist_delta);
360 status = 0;
361 } else {
362 if (aabb == obj->getAABB())
363 aabb.expand(delta);
364 else
365 aabb.expand(obj->getAABB(), 2.0);
366 }
367 }
368 } else if (status == 0) {
369 break;
370 }
371 }
372
373 return false;
374}
375
376//==============================================================================
377template <typename HashTable>
379 CollisionCallBackBase* callback) const {
380 if (size() == 0) return;
381
382 for (const auto& obj1 : objs) {
383 const auto& obj_aabb = obj1->getAABB();
384 AABB overlap_aabb;
385
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) {
389 if (obj1 < obj2) {
390 if ((*callback)(obj1, obj2)) return;
391 }
392 }
393
394 if (!scene_limit.contain(obj_aabb)) {
395 for (const auto& obj2 : objs_outside_scene_limit) {
396 if (obj1 < obj2) {
397 if ((*callback)(obj1, obj2)) return;
398 }
399 }
400 }
401 } else {
402 for (const auto& obj2 : objs_partially_penetrating_scene_limit) {
403 if (obj1 < obj2) {
404 if ((*callback)(obj1, obj2)) return;
405 }
406 }
407
408 for (const auto& obj2 : objs_outside_scene_limit) {
409 if (obj1 < obj2) {
410 if ((*callback)(obj1, obj2)) return;
411 }
412 }
413 }
414 }
415}
416
417//==============================================================================
418template <typename HashTable>
420 DistanceCallBackBase* callback) const {
421 if (size() == 0) return;
422
423 this->enable_tested_set_ = true;
424 this->tested_set.clear();
425
426 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
427
428 for (const auto& obj : objs) {
429 if (distance_(obj, callback, min_dist)) break;
430 }
431
432 this->enable_tested_set_ = false;
433 this->tested_set.clear();
434}
435
436//==============================================================================
437template <typename HashTable>
439 BroadPhaseCollisionManager* other_manager_,
440 CollisionCallBackBase* callback) const {
441 auto* other_manager =
442 static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
443
444 if ((size() == 0) || (other_manager->size() == 0)) return;
445
446 if (this == other_manager) {
447 collide(callback);
448 return;
449 }
450
451 if (this->size() < other_manager->size()) {
452 for (const auto& obj : objs) {
453 if (other_manager->collide_(obj, callback)) return;
454 }
455 } else {
456 for (const auto& obj : other_manager->objs) {
457 if (collide_(obj, callback)) return;
458 }
459 }
460}
461
462//==============================================================================
463template <typename HashTable>
465 BroadPhaseCollisionManager* other_manager_,
466 DistanceCallBackBase* callback) const {
467 auto* other_manager =
468 static_cast<SpatialHashingCollisionManager<HashTable>*>(other_manager_);
469
470 if ((size() == 0) || (other_manager->size() == 0)) return;
471
472 if (this == other_manager) {
473 distance(callback);
474 return;
475 }
476
477 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
478
479 if (this->size() < other_manager->size()) {
480 for (const auto& obj : objs)
481 if (other_manager->distance_(obj, callback, min_dist)) return;
482 } else {
483 for (const auto& obj : other_manager->objs)
484 if (distance_(obj, callback, min_dist)) return;
485 }
486}
487
488//==============================================================================
489template <typename HashTable>
491 return objs.empty();
492}
493
494//==============================================================================
495template <typename HashTable>
497 return objs.size();
498}
499
500//==============================================================================
501template <typename HashTable>
503 std::vector<CollisionObject*>& objs, Vec3f& l, Vec3f& u) {
504 AABB bound;
505 for (unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB();
506
507 l = bound.min_;
508 u = bound.max_;
509}
510
511//==============================================================================
512template <typename HashTable>
513template <typename Container>
515 CollisionObject* obj, const Container& objs, DistanceCallBackBase* callback,
516 FCL_REAL& min_dist) const {
517 for (auto& obj2 : objs) {
518 if (obj == obj2) continue;
519
520 if (!this->enable_tested_set_) {
521 if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
522 if ((*callback)(obj, obj2, min_dist)) return true;
523 }
524 } else {
525 if (!this->inTestedSet(obj, obj2)) {
526 if (obj->getAABB().distance(obj2->getAABB()) < min_dist) {
527 if ((*callback)(obj, obj2, min_dist)) return true;
528 }
529
530 this->insertTestedSet(obj, obj2);
531 }
532 }
533 }
534
535 return false;
536}
537
538} // namespace fcl
539
540} // namespace hpp
541
542#endif
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:55
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Definition: broadphase_collision_manager.h:88
virtual void update()
update the condition of manager
Definition: broadphase_spatialhash-inl.h:112
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
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_spatialhash-inl.h:106
void unregisterObject(CollisionObject *obj)
remove one object from the manager
Definition: broadphase_spatialhash-inl.h:85
size_t size() const
the number of objects managed by the manager
Definition: broadphase_spatialhash-inl.h:496
bool empty() const
whether the manager is empty
Definition: broadphase_spatialhash-inl.h:490
~SpatialHashingCollisionManager()
Definition: broadphase_spatialhash-inl.h:58
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
void clear()
clear the manager
Definition: broadphase_spatialhash-inl.h:238
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
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
static void computeBound(std::vector< CollisionObject * > &objs, Vec3f &l, Vec3f &u)
compute the bound for the environent
Definition: broadphase_spatialhash-inl.h:502
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 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
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: broadphase_spatialhash-inl.h:64
Vec3f max_
The max point in the AABB.
Definition: AABB.h:59
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.
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: AABB.h:110
Vec3f min_
The min point in the AABB.
Definition: AABB.h:57
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
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,...
double FCL_REAL
Definition: data_types.h:65
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
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73