FCL  0.6.0
Flexible Collision Library
broadphase_spatialhash-inl.h
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 FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
39 #define FCL_BROADPHASE_BROADPAHSESPATIALHASH_INL_H
40 
41 #include "fcl/broadphase/broadphase_spatialhash.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class SpatialHashingCollisionManager<
49  double,
50  detail::SimpleHashTable<
51  AABB<double>, CollisionObject<double>*, detail::SpatialHash<double>>>;
52 
53 //==============================================================================
54 template<typename S, typename HashTable>
55 SpatialHashingCollisionManager<S, HashTable>::SpatialHashingCollisionManager(
56  S cell_size,
57  const Vector3<S>& scene_min,
58  const Vector3<S>& scene_max,
59  unsigned int default_table_size)
60  : scene_limit(AABB<S>(scene_min, scene_max)),
61  hash_table(new HashTable(detail::SpatialHash<S>(scene_limit, cell_size)))
62 {
63  hash_table->init(default_table_size);
64 }
65 
66 //==============================================================================
67 template<typename S, typename HashTable>
68 SpatialHashingCollisionManager<S, HashTable>::~SpatialHashingCollisionManager()
69 {
70  delete hash_table;
71 }
72 
73 //==============================================================================
74 template<typename S, typename HashTable>
76  CollisionObject<S>* obj)
77 {
78  objs.push_back(obj);
79 
80  const AABB<S>& obj_aabb = obj->getAABB();
81  AABB<S> overlap_aabb;
82 
83  if(scene_limit.overlap(obj_aabb, overlap_aabb))
84  {
85  if(!scene_limit.contain(obj_aabb))
86  objs_partially_penetrating_scene_limit.push_back(obj);
87 
88  hash_table->insert(overlap_aabb, obj);
89  }
90  else
91  {
92  objs_outside_scene_limit.push_back(obj);
93  }
94 
95  obj_aabb_map[obj] = obj_aabb;
96 }
97 
98 //==============================================================================
99 template<typename S, typename HashTable>
101 {
102  objs.remove(obj);
103 
104  const AABB<S>& obj_aabb = obj->getAABB();
105  AABB<S> overlap_aabb;
106 
107  if(scene_limit.overlap(obj_aabb, overlap_aabb))
108  {
109  if(!scene_limit.contain(obj_aabb))
110  objs_partially_penetrating_scene_limit.remove(obj);
111 
112  hash_table->remove(overlap_aabb, obj);
113  }
114  else
115  {
116  objs_outside_scene_limit.remove(obj);
117  }
118 
119  obj_aabb_map.erase(obj);
120 }
121 
122 //==============================================================================
123 template<typename S, typename HashTable>
125 {
126  // Do nothing
127 }
128 
129 //==============================================================================
130 template<typename S, typename HashTable>
132 {
133  hash_table->clear();
134  objs_partially_penetrating_scene_limit.clear();
135  objs_outside_scene_limit.clear();
136 
137  for(auto it = objs.cbegin(), end = objs.cend(); it != end; ++it)
138  {
139  CollisionObject<S>* obj = *it;
140  const AABB<S>& obj_aabb = obj->getAABB();
141  AABB<S> overlap_aabb;
142 
143  if(scene_limit.overlap(obj_aabb, overlap_aabb))
144  {
145  if(!scene_limit.contain(obj_aabb))
146  objs_partially_penetrating_scene_limit.push_back(obj);
147 
148  hash_table->insert(overlap_aabb, obj);
149  }
150  else
151  {
152  objs_outside_scene_limit.push_back(obj);
153  }
154 
155  obj_aabb_map[obj] = obj_aabb;
156  }
157 }
158 
159 //==============================================================================
160 template<typename S, typename HashTable>
162 {
163  const AABB<S>& new_aabb = updated_obj->getAABB();
164  const AABB<S>& old_aabb = obj_aabb_map[updated_obj];
165 
166  AABB<S> old_overlap_aabb;
167  const auto is_old_aabb_overlapping
168  = scene_limit.overlap(old_aabb, old_overlap_aabb);
169  if(is_old_aabb_overlapping)
170  hash_table->remove(old_overlap_aabb, updated_obj);
171 
172  AABB<S> new_overlap_aabb;
173  const auto is_new_aabb_overlapping
174  = scene_limit.overlap(new_aabb, new_overlap_aabb);
175  if(is_new_aabb_overlapping)
176  hash_table->insert(new_overlap_aabb, updated_obj);
177 
178  ObjectStatus old_status;
179  if(is_old_aabb_overlapping)
180  {
181  if(scene_limit.contain(old_aabb))
182  old_status = Inside;
183  else
184  old_status = PartiallyPenetrating;
185  }
186  else
187  {
188  old_status = Outside;
189  }
190 
191  if(is_new_aabb_overlapping)
192  {
193  if(scene_limit.contain(new_aabb))
194  {
195  if (old_status == PartiallyPenetrating)
196  {
197  // Status change: PartiallyPenetrating --> Inside
198  // Required action(s):
199  // - remove object from "objs_partially_penetrating_scene_limit"
200 
201  auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
202  objs_partially_penetrating_scene_limit.end(),
203  updated_obj);
204  objs_partially_penetrating_scene_limit.erase(find_it);
205  }
206  else if (old_status == Outside)
207  {
208  // Status change: Outside --> Inside
209  // Required action(s):
210  // - remove object from "objs_outside_scene_limit"
211 
212  auto find_it = std::find(objs_outside_scene_limit.begin(),
213  objs_outside_scene_limit.end(),
214  updated_obj);
215  objs_outside_scene_limit.erase(find_it);
216  }
217  }
218  else
219  {
220  if (old_status == Inside)
221  {
222  // Status change: Inside --> PartiallyPenetrating
223  // Required action(s):
224  // - add object to "objs_partially_penetrating_scene_limit"
225 
226  objs_partially_penetrating_scene_limit.push_back(updated_obj);
227  }
228  else if (old_status == Outside)
229  {
230  // Status change: Outside --> PartiallyPenetrating
231  // Required action(s):
232  // - remove object from "objs_outside_scene_limit"
233  // - add object to "objs_partially_penetrating_scene_limit"
234 
235  auto find_it = std::find(objs_outside_scene_limit.begin(),
236  objs_outside_scene_limit.end(),
237  updated_obj);
238  objs_outside_scene_limit.erase(find_it);
239 
240  objs_partially_penetrating_scene_limit.push_back(updated_obj);
241  }
242  }
243  }
244  else
245  {
246  if (old_status == Inside)
247  {
248  // Status change: Inside --> Outside
249  // Required action(s):
250  // - add object to "objs_outside_scene_limit"
251 
252  objs_outside_scene_limit.push_back(updated_obj);
253  }
254  else if (old_status == PartiallyPenetrating)
255  {
256  // Status change: PartiallyPenetrating --> Outside
257  // Required action(s):
258  // - remove object from "objs_partially_penetrating_scene_limit"
259  // - add object to "objs_outside_scene_limit"
260 
261  auto find_it = std::find(objs_partially_penetrating_scene_limit.begin(),
262  objs_partially_penetrating_scene_limit.end(),
263  updated_obj);
264  objs_partially_penetrating_scene_limit.erase(find_it);
265 
266  objs_outside_scene_limit.push_back(updated_obj);
267  }
268  }
269 
270  obj_aabb_map[updated_obj] = new_aabb;
271 }
272 
273 //==============================================================================
274 template<typename S, typename HashTable>
276 {
277  for(size_t i = 0; i < updated_objs.size(); ++i)
278  update(updated_objs[i]);
279 }
280 
281 //==============================================================================
282 template<typename S, typename HashTable>
284 {
285  objs.clear();
286  hash_table->clear();
287  objs_outside_scene_limit.clear();
288  obj_aabb_map.clear();
289 }
290 
291 //==============================================================================
292 template<typename S, typename HashTable>
294 {
295  objs_.resize(objs.size());
296  std::copy(objs.begin(), objs.end(), objs_.begin());
297 }
298 
299 //==============================================================================
300 template<typename S, typename HashTable>
302 {
303  if(size() == 0) return;
304  collide_(obj, cdata, callback);
305 }
306 
307 //==============================================================================
308 template<typename S, typename HashTable>
310 {
311  if(size() == 0) return;
312  S min_dist = std::numeric_limits<S>::max();
313  distance_(obj, cdata, callback, min_dist);
314 }
315 
316 //==============================================================================
317 template<typename S, typename HashTable>
319  CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
320 {
321  const auto& obj_aabb = obj->getAABB();
322  AABB<S> overlap_aabb;
323 
324  if(scene_limit.overlap(obj_aabb, overlap_aabb))
325  {
326  const auto query_result = hash_table->query(overlap_aabb);
327  for(const auto& obj2 : query_result)
328  {
329  if(obj == obj2)
330  continue;
331 
332  if(callback(obj, obj2, cdata))
333  return true;
334  }
335 
336  if(!scene_limit.contain(obj_aabb))
337  {
338  for(const auto& obj2 : objs_outside_scene_limit)
339  {
340  if(obj == obj2)
341  continue;
342 
343  if(callback(obj, obj2, cdata))
344  return true;
345  }
346  }
347  }
348  else
349  {
350  for(const auto& obj2 : objs_partially_penetrating_scene_limit)
351  {
352  if(obj == obj2)
353  continue;
354 
355  if(callback(obj, obj2, cdata))
356  return true;
357  }
358 
359  for(const auto& obj2 : objs_outside_scene_limit)
360  {
361  if(obj == obj2)
362  continue;
363 
364  if(callback(obj, obj2, cdata))
365  return true;
366  }
367  }
368 
369  return false;
370 }
371 
372 //==============================================================================
373 template<typename S, typename HashTable>
375  CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
376 {
377  auto delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
378  auto aabb = obj->getAABB();
379  if(min_dist < std::numeric_limits<S>::max())
380  {
381  Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
382  aabb.expand(min_dist_delta);
383  }
384 
385  AABB<S> overlap_aabb;
386 
387  auto status = 1;
388  S old_min_distance;
389 
390  while(1)
391  {
392  old_min_distance = min_dist;
393 
394  if(scene_limit.overlap(aabb, overlap_aabb))
395  {
396  if (distanceObjectToObjects(
397  obj, hash_table->query(overlap_aabb), cdata, callback, min_dist))
398  {
399  return true;
400  }
401 
402  if(!scene_limit.contain(aabb))
403  {
404  if (distanceObjectToObjects(
405  obj, objs_outside_scene_limit, cdata, callback, min_dist))
406  {
407  return true;
408  }
409  }
410  }
411  else
412  {
413  if (distanceObjectToObjects(
414  obj, objs_partially_penetrating_scene_limit, cdata, callback, min_dist))
415  {
416  return true;
417  }
418 
419  if (distanceObjectToObjects(
420  obj, objs_outside_scene_limit, cdata, callback, min_dist))
421  {
422  return true;
423  }
424  }
425 
426  if(status == 1)
427  {
428  if(old_min_distance < std::numeric_limits<S>::max())
429  {
430  break;
431  }
432  else
433  {
434  if(min_dist < old_min_distance)
435  {
436  Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
437  aabb = AABB<S>(obj->getAABB(), min_dist_delta);
438  status = 0;
439  }
440  else
441  {
442  if(aabb.equal(obj->getAABB()))
443  aabb.expand(delta);
444  else
445  aabb.expand(obj->getAABB(), 2.0);
446  }
447  }
448  }
449  else if(status == 0)
450  {
451  break;
452  }
453  }
454 
455  return false;
456 }
457 
458 //==============================================================================
459 template<typename S, typename HashTable>
461  void* cdata, CollisionCallBack<S> callback) const
462 {
463  if(size() == 0)
464  return;
465 
466  for(const auto& obj1 : objs)
467  {
468  const auto& obj_aabb = obj1->getAABB();
469  AABB<S> overlap_aabb;
470 
471  if(scene_limit.overlap(obj_aabb, overlap_aabb))
472  {
473  auto query_result = hash_table->query(overlap_aabb);
474  for(const auto& obj2 : query_result)
475  {
476  if(obj1 < obj2)
477  {
478  if(callback(obj1, obj2, cdata))
479  return;
480  }
481  }
482 
483  if(!scene_limit.contain(obj_aabb))
484  {
485  for(const auto& obj2 : objs_outside_scene_limit)
486  {
487  if(obj1 < obj2)
488  {
489  if(callback(obj1, obj2, cdata))
490  return;
491  }
492  }
493  }
494  }
495  else
496  {
497  for(const auto& obj2 : objs_partially_penetrating_scene_limit)
498  {
499  if(obj1 < obj2)
500  {
501  if(callback(obj1, obj2, cdata))
502  return;
503  }
504  }
505 
506  for(const auto& obj2 : objs_outside_scene_limit)
507  {
508  if(obj1 < obj2)
509  {
510  if(callback(obj1, obj2, cdata))
511  return;
512  }
513  }
514  }
515  }
516 }
517 
518 //==============================================================================
519 template<typename S, typename HashTable>
521  void* cdata, DistanceCallBack<S> callback) const
522 {
523  if(size() == 0)
524  return;
525 
526  this->enable_tested_set_ = true;
527  this->tested_set.clear();
528 
529  S min_dist = std::numeric_limits<S>::max();
530 
531  for(const auto& obj : objs)
532  {
533  if(distance_(obj, cdata, callback, min_dist))
534  break;
535  }
536 
537  this->enable_tested_set_ = false;
538  this->tested_set.clear();
539 }
540 
541 //==============================================================================
542 template<typename S, typename HashTable>
544 {
545  auto* other_manager = static_cast<SpatialHashingCollisionManager<S, HashTable>* >(other_manager_);
546 
547  if((size() == 0) || (other_manager->size() == 0))
548  return;
549 
550  if(this == other_manager)
551  {
552  collide(cdata, callback);
553  return;
554  }
555 
556  if(this->size() < other_manager->size())
557  {
558  for(const auto& obj : objs)
559  {
560  if(other_manager->collide_(obj, cdata, callback))
561  return;
562  }
563  }
564  else
565  {
566  for(const auto& obj : other_manager->objs)
567  {
568  if(collide_(obj, cdata, callback))
569  return;
570  }
571  }
572 }
573 
574 //==============================================================================
575 template<typename S, typename HashTable>
577 {
578  auto* other_manager = static_cast<SpatialHashingCollisionManager<S, HashTable>* >(other_manager_);
579 
580  if((size() == 0) || (other_manager->size() == 0))
581  return;
582 
583  if(this == other_manager)
584  {
585  distance(cdata, callback);
586  return;
587  }
588 
589  S min_dist = std::numeric_limits<S>::max();
590 
591  if(this->size() < other_manager->size())
592  {
593  for(const auto& obj : objs)
594  if(other_manager->distance_(obj, cdata, callback, min_dist)) return;
595  }
596  else
597  {
598  for(const auto& obj : other_manager->objs)
599  if(distance_(obj, cdata, callback, min_dist)) return;
600  }
601 }
602 
603 //==============================================================================
604 template<typename S, typename HashTable>
606 {
607  return objs.empty();
608 }
609 
610 //==============================================================================
611 template<typename S, typename HashTable>
613 {
614  return objs.size();
615 }
616 
617 //==============================================================================
618 template<typename S, typename HashTable>
620  std::vector<CollisionObject<S>*>& objs, Vector3<S>& l, Vector3<S>& u)
621 {
622  AABB<S> bound;
623  for(unsigned int i = 0; i < objs.size(); ++i)
624  bound += objs[i]->getAABB();
625 
626  l = bound.min_;
627  u = bound.max_;
628 }
629 
630 //==============================================================================
631 template<typename S, typename HashTable>
632 template<typename Container>
634  CollisionObject<S>* obj,
635  const Container& objs,
636  void* cdata,
637  DistanceCallBack<S> callback,
638  S& min_dist) const
639 {
640  for(auto& obj2 : objs)
641  {
642  if(obj == obj2)
643  continue;
644 
645  if(!this->enable_tested_set_)
646  {
647  if(obj->getAABB().distance(obj2->getAABB()) < min_dist)
648  {
649  if(callback(obj, obj2, cdata, min_dist))
650  return true;
651  }
652  }
653  else
654  {
655  if(!this->inTestedSet(obj, obj2))
656  {
657  if(obj->getAABB().distance(obj2->getAABB()) < min_dist)
658  {
659  if(callback(obj, obj2, cdata, min_dist))
660  return true;
661  }
662 
663  this->insertTestedSet(obj, obj2);
664  }
665  }
666  }
667 
668  return false;
669 }
670 
671 } // namespace fcl
672 
673 #endif
void getObjects(std::vector< CollisionObject< S > * > &objs) const
return the objects managed by the manager
Definition: broadphase_spatialhash-inl.h:293
void clear()
clear the manager
Definition: broadphase_spatialhash-inl.h:283
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager ...
Definition: broadphase_SaP-inl.h:759
bool empty() const
whether the manager is empty
Definition: broadphase_spatialhash-inl.h:605
void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_spatialhash-inl.h:301
size_t size() const
the number of objects managed by the manager
Definition: broadphase_SaP-inl.h:880
static void computeBound(std::vector< CollisionObject< S > * > &objs, Vector3< S > &l, Vector3< S > &u)
compute the bound for the environent
Definition: broadphase_spatialhash-inl.h:619
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging ot the manager ...
Definition: broadphase_spatialhash-inl.h:309
spatial hashing collision mananger
Definition: broadphase_spatialhash.h:56
bool overlap(const AABB< S > &other) const
Check whether two AABB are overlap.
Definition: AABB-inl.h:98
Vector3< S > max_
The max point in the AABB.
Definition: AABB.h:59
std::set< std::pair< CollisionObject< S > *, CollisionObject< S > * > > tested_set
tools help to avoid repeating collision or distance callback for the pairs of objects tested before...
Definition: broadphase_collision_manager.h:127
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata) CollisionCallBack
Callback for collision between two objects. Return value is whether can stop now. ...
Definition: broadphase_collision_manager.h:53
void update()
update the condition of manager
Definition: broadphase_SaP-inl.h:507
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata, S &dist) DistanceCallBack
Callback for distance between two objects, Return value is whether can stop now, also return the mini...
Definition: broadphase_collision_manager.h:60
void registerObject(CollisionObject< S > *obj)
add one object to the manager
Definition: broadphase_spatialhash-inl.h:75
Vector3< S > min_
The min point in the AABB.
Definition: AABB.h:56
bool collide_(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_spatialhash-inl.h:318
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_spatialhash-inl.h:124
const AABB< S > & getAABB() const
get the AABB in world space
Definition: collision_object-inl.h:111
void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager ...
Definition: broadphase_SaP-inl.h:642
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:51
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66
bool distance_(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback, S &min_dist) const
perform distance computation between one object and all the objects belonging ot the manager ...
Definition: broadphase_spatialhash-inl.h:374
AABB< S > & expand(const Vector3< S > &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: AABB-inl.h:327
S distance(const AABB< S > &other, Vector3< S > *P, Vector3< S > *Q) const
Distance between two AABBs; P and Q, should not be nullptr, return the nearest points.
Definition: AABB-inl.h:237
size_t size() const
the number of objects managed by the manager
Definition: broadphase_spatialhash-inl.h:612
void unregisterObject(CollisionObject< S > *obj)
remove one object from the manager
Definition: broadphase_spatialhash-inl.h:100
void update()
update the condition of manager
Definition: broadphase_spatialhash-inl.h:131