broadphase_interval_tree-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 FCL_BROAD_PHASE_INTERVAL_TREE_INL_H
39 #define FCL_BROAD_PHASE_INTERVAL_TREE_INL_H
40 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
49 
50 //==============================================================================
51 template <typename S>
53 {
54  // must sorted before
55  setup();
56 
57  EndPoint p;
58  p.value = obj->getAABB().min_[0];
59  auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p);
60  p.value = obj->getAABB().max_[0];
61  auto end1 = std::upper_bound(start1, endpoints[0].end(), p);
62 
63  if(start1 < end1)
64  {
65  unsigned int start_id = start1 - endpoints[0].begin();
66  unsigned int end_id = end1 - endpoints[0].begin();
67  unsigned int cur_id = start_id;
68  for(unsigned int i = start_id; i < end_id; ++i)
69  {
70  if(endpoints[0][i].obj != obj)
71  {
72  if(i == cur_id) cur_id++;
73  else
74  {
75  endpoints[0][cur_id] = endpoints[0][i];
76  cur_id++;
77  }
78  }
79  }
80  if(cur_id < end_id)
81  endpoints[0].resize(endpoints[0].size() - 2);
82  }
83 
84  p.value = obj->getAABB().min_[1];
85  auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p);
86  p.value = obj->getAABB().max_[1];
87  auto end2 = std::upper_bound(start2, endpoints[1].end(), p);
88 
89  if(start2 < end2)
90  {
91  unsigned int start_id = start2 - endpoints[1].begin();
92  unsigned int end_id = end2 - endpoints[1].begin();
93  unsigned int cur_id = start_id;
94  for(unsigned int i = start_id; i < end_id; ++i)
95  {
96  if(endpoints[1][i].obj != obj)
97  {
98  if(i == cur_id) cur_id++;
99  else
100  {
101  endpoints[1][cur_id] = endpoints[1][i];
102  cur_id++;
103  }
104  }
105  }
106  if(cur_id < end_id)
107  endpoints[1].resize(endpoints[1].size() - 2);
108  }
109 
110 
111  p.value = obj->getAABB().min_[2];
112  auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p);
113  p.value = obj->getAABB().max_[2];
114  auto end3 = std::upper_bound(start3, endpoints[2].end(), p);
115 
116  if(start3 < end3)
117  {
118  unsigned int start_id = start3 - endpoints[2].begin();
119  unsigned int end_id = end3 - endpoints[2].begin();
120  unsigned int cur_id = start_id;
121  for(unsigned int i = start_id; i < end_id; ++i)
122  {
123  if(endpoints[2][i].obj != obj)
124  {
125  if(i == cur_id) cur_id++;
126  else
127  {
128  endpoints[2][cur_id] = endpoints[2][i];
129  cur_id++;
130  }
131  }
132  }
133  if(cur_id < end_id)
134  endpoints[2].resize(endpoints[2].size() - 2);
135  }
136 
137  // update the interval tree
138  if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end())
139  {
140  SAPInterval* ivl1 = obj_interval_maps[0][obj];
141  SAPInterval* ivl2 = obj_interval_maps[1][obj];
142  SAPInterval* ivl3 = obj_interval_maps[2][obj];
143 
144  interval_trees[0]->deleteNode(ivl1);
145  interval_trees[1]->deleteNode(ivl2);
146  interval_trees[2]->deleteNode(ivl3);
147 
148  delete ivl1;
149  delete ivl2;
150  delete ivl3;
151 
152  obj_interval_maps[0].erase(obj);
153  obj_interval_maps[1].erase(obj);
154  obj_interval_maps[2].erase(obj);
155  }
156 }
157 
158 //==============================================================================
159 template <typename S>
161 {
162  for(int i = 0; i < 3; ++i)
163  interval_trees[i] = nullptr;
164 }
165 
166 //==============================================================================
167 template <typename S>
168 IntervalTreeCollisionManager<S>::~IntervalTreeCollisionManager()
169 {
170  clear();
171 }
172 
173 //==============================================================================
174 template <typename S>
175 void IntervalTreeCollisionManager<S>::registerObject(CollisionObject<S>* obj)
176 {
177  EndPoint p, q;
178 
179  p.obj = obj;
180  q.obj = obj;
181  p.minmax = 0;
182  q.minmax = 1;
183  p.value = obj->getAABB().min_[0];
184  q.value = obj->getAABB().max_[0];
185  endpoints[0].push_back(p);
186  endpoints[0].push_back(q);
187 
188  p.value = obj->getAABB().min_[1];
189  q.value = obj->getAABB().max_[1];
190  endpoints[1].push_back(p);
191  endpoints[1].push_back(q);
192 
193  p.value = obj->getAABB().min_[2];
194  q.value = obj->getAABB().max_[2];
195  endpoints[2].push_back(p);
196  endpoints[2].push_back(q);
197  setup_ = false;
198 }
199 
200 //==============================================================================
201 template <typename S>
202 void IntervalTreeCollisionManager<S>::setup()
203 {
204  if(!setup_)
205  {
206  std::sort(endpoints[0].begin(), endpoints[0].end());
207  std::sort(endpoints[1].begin(), endpoints[1].end());
208  std::sort(endpoints[2].begin(), endpoints[2].end());
209 
210  for(int i = 0; i < 3; ++i)
211  delete interval_trees[i];
212 
213  for(int i = 0; i < 3; ++i)
214  interval_trees[i] = new detail::IntervalTree<S>;
215 
216  for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
217  {
218  EndPoint p = endpoints[0][i];
219  CollisionObject<S>* obj = p.obj;
220  if(p.minmax == 0)
221  {
222  SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj);
223  SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj);
224  SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj);
225 
226  interval_trees[0]->insert(ivl1);
227  interval_trees[1]->insert(ivl2);
228  interval_trees[2]->insert(ivl3);
229 
230  obj_interval_maps[0][obj] = ivl1;
231  obj_interval_maps[1][obj] = ivl2;
232  obj_interval_maps[2][obj] = ivl3;
233  }
234  }
235 
236  setup_ = true;
237  }
238 }
239 
240 //==============================================================================
241 template <typename S>
242 void IntervalTreeCollisionManager<S>::update()
243 {
244  setup_ = false;
245 
246  for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
247  {
248  if(endpoints[0][i].minmax == 0)
249  endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
250  else
251  endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
252  }
253 
254  for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i)
255  {
256  if(endpoints[1][i].minmax == 0)
257  endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
258  else
259  endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
260  }
261 
262  for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i)
263  {
264  if(endpoints[2][i].minmax == 0)
265  endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
266  else
267  endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
268  }
269 
270  setup();
271 
272 }
273 
274 //==============================================================================
275 template <typename S>
276 void IntervalTreeCollisionManager<S>::update(CollisionObject<S>* updated_obj)
277 {
278  AABB<S> old_aabb;
279  const AABB<S>& new_aabb = updated_obj->getAABB();
280  for(int i = 0; i < 3; ++i)
281  {
282  const auto it = obj_interval_maps[i].find(updated_obj);
283  interval_trees[i]->deleteNode(it->second);
284  old_aabb.min_[i] = it->second->low;
285  old_aabb.max_[i] = it->second->high;
286  it->second->low = new_aabb.min_[i];
287  it->second->high = new_aabb.max_[i];
288  interval_trees[i]->insert(it->second);
289  }
290 
291  EndPoint dummy;
292  typename std::vector<EndPoint>::iterator it;
293  for(int i = 0; i < 3; ++i)
294  {
295  dummy.value = old_aabb.min_[i];
296  it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
297  for(; it != endpoints[i].end(); ++it)
298  {
299  if(it->obj == updated_obj && it->minmax == 0)
300  {
301  it->value = new_aabb.min_[i];
302  break;
303  }
304  }
305 
306  dummy.value = old_aabb.max_[i];
307  it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
308  for(; it != endpoints[i].end(); ++it)
309  {
310  if(it->obj == updated_obj && it->minmax == 0)
311  {
312  it->value = new_aabb.max_[i];
313  break;
314  }
315  }
316 
317  std::sort(endpoints[i].begin(), endpoints[i].end());
318  }
319 }
320 
321 //==============================================================================
322 template <typename S>
323 void IntervalTreeCollisionManager<S>::update(const std::vector<CollisionObject<S>*>& updated_objs)
324 {
325  for(size_t i = 0; i < updated_objs.size(); ++i)
326  update(updated_objs[i]);
327 }
328 
329 //==============================================================================
330 template <typename S>
331 void IntervalTreeCollisionManager<S>::clear()
332 {
333  endpoints[0].clear();
334  endpoints[1].clear();
335  endpoints[2].clear();
336 
337  delete interval_trees[0]; interval_trees[0] = nullptr;
338  delete interval_trees[1]; interval_trees[1] = nullptr;
339  delete interval_trees[2]; interval_trees[2] = nullptr;
340 
341  for(int i = 0; i < 3; ++i)
342  {
343  for(auto it = obj_interval_maps[i].cbegin(), end = obj_interval_maps[i].cend();
344  it != end; ++it)
345  {
346  delete it->second;
347  }
348  }
349 
350  for(int i = 0; i < 3; ++i)
351  obj_interval_maps[i].clear();
352 
353  setup_ = false;
354 }
355 
356 //==============================================================================
357 template <typename S>
358 void IntervalTreeCollisionManager<S>::getObjects(std::vector<CollisionObject<S>*>& objs) const
359 {
360  objs.resize(endpoints[0].size() / 2);
361  unsigned int j = 0;
362  for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i)
363  {
364  if(endpoints[0][i].minmax == 0)
365  {
366  objs[j] = endpoints[0][i].obj; j++;
367  }
368  }
369 }
370 
371 //==============================================================================
372 template <typename S>
373 void IntervalTreeCollisionManager<S>::collide(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
374 {
375  if(size() == 0) return;
376  collide_(obj, cdata, callback);
377 }
378 
379 //==============================================================================
380 template <typename S>
381 bool IntervalTreeCollisionManager<S>::collide_(CollisionObject<S>* obj, void* cdata, CollisionCallBack<S> callback) const
382 {
383  static const unsigned int CUTOFF = 100;
384 
385  std::deque<detail::SimpleInterval<S>*> results0, results1, results2;
386 
387  results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
388  if(results0.size() > CUTOFF)
389  {
390  results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]);
391  if(results1.size() > CUTOFF)
392  {
393  results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]);
394  if(results2.size() > CUTOFF)
395  {
396  int d1 = results0.size();
397  int d2 = results1.size();
398  int d3 = results2.size();
399 
400  if(d1 >= d2 && d1 >= d3)
401  return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
402  else if(d2 >= d1 && d2 >= d3)
403  return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
404  else
405  return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
406  }
407  else
408  return checkColl(results2.begin(), results2.end(), obj, cdata, callback);
409  }
410  else
411  return checkColl(results1.begin(), results1.end(), obj, cdata, callback);
412  }
413  else
414  return checkColl(results0.begin(), results0.end(), obj, cdata, callback);
415 }
416 
417 //==============================================================================
418 template <typename S>
419 void IntervalTreeCollisionManager<S>::distance(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback) const
420 {
421  if(size() == 0) return;
422  S min_dist = std::numeric_limits<S>::max();
423  distance_(obj, cdata, callback, min_dist);
424 }
425 
426 //==============================================================================
427 template <typename S>
428 bool IntervalTreeCollisionManager<S>::distance_(CollisionObject<S>* obj, void* cdata, DistanceCallBack<S> callback, S& min_dist) const
429 {
430  static const unsigned int CUTOFF = 100;
431 
432  Vector3<S> delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
433  AABB<S> aabb = obj->getAABB();
434  if(min_dist < std::numeric_limits<S>::max())
435  {
436  Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
437  aabb.expand(min_dist_delta);
438  }
439 
440  int status = 1;
441  S old_min_distance;
442 
443  while(1)
444  {
445  bool dist_res = false;
446 
447  old_min_distance = min_dist;
448 
449  std::deque<detail::SimpleInterval<S>*> results0, results1, results2;
450 
451  results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
452  if(results0.size() > CUTOFF)
453  {
454  results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
455  if(results1.size() > CUTOFF)
456  {
457  results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
458  if(results2.size() > CUTOFF)
459  {
460  int d1 = results0.size();
461  int d2 = results1.size();
462  int d3 = results2.size();
463 
464  if(d1 >= d2 && d1 >= d3)
465  dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
466  else if(d2 >= d1 && d2 >= d3)
467  dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
468  else
469  dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
470  }
471  else
472  dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist);
473  }
474  else
475  dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist);
476  }
477  else
478  dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist);
479 
480  if(dist_res) return true;
481 
482  results0.clear();
483  results1.clear();
484  results2.clear();
485 
486  if(status == 1)
487  {
488  if(old_min_distance < std::numeric_limits<S>::max())
489  break;
490  else
491  {
492  if(min_dist < old_min_distance)
493  {
494  Vector3<S> min_dist_delta(min_dist, min_dist, min_dist);
495  aabb = AABB<S>(obj->getAABB(), min_dist_delta);
496  status = 0;
497  }
498  else
499  {
500  if(aabb.equal(obj->getAABB()))
501  aabb.expand(delta);
502  else
503  aabb.expand(obj->getAABB(), 2.0);
504  }
505  }
506  }
507  else if(status == 0)
508  break;
509  }
510 
511  return false;
512 }
513 
514 //==============================================================================
515 template <typename S>
516 void IntervalTreeCollisionManager<S>::collide(void* cdata, CollisionCallBack<S> callback) const
517 {
518  if(size() == 0) return;
519 
520  std::set<CollisionObject<S>*> active;
521  std::set<std::pair<CollisionObject<S>*, CollisionObject<S>*> > overlap;
522  unsigned int n = endpoints[0].size();
523  S diff_x = endpoints[0][0].value - endpoints[0][n-1].value;
524  S diff_y = endpoints[1][0].value - endpoints[1][n-1].value;
525  S diff_z = endpoints[2][0].value - endpoints[2][n-1].value;
526 
527  int axis = 0;
528  if(diff_y > diff_x && diff_y > diff_z)
529  axis = 1;
530  else if(diff_z > diff_y && diff_z > diff_x)
531  axis = 2;
532 
533  for(unsigned int i = 0; i < n; ++i)
534  {
535  const EndPoint& endpoint = endpoints[axis][i];
536  CollisionObject<S>* index = endpoint.obj;
537  if(endpoint.minmax == 0)
538  {
539  auto iter = active.begin();
540  auto end = active.end();
541  for(; iter != end; ++iter)
542  {
543  CollisionObject<S>* active_index = *iter;
544  const AABB<S>& b0 = active_index->getAABB();
545  const AABB<S>& b1 = index->getAABB();
546 
547  int axis2 = (axis + 1) % 3;
548  int axis3 = (axis + 2) % 3;
549 
550  if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3))
551  {
552  std::pair<typename std::set<std::pair<CollisionObject<S>*, CollisionObject<S>*> >::iterator, bool> insert_res;
553  if(active_index < index)
554  insert_res = overlap.insert(std::make_pair(active_index, index));
555  else
556  insert_res = overlap.insert(std::make_pair(index, active_index));
557 
558  if(insert_res.second)
559  {
560  if(callback(active_index, index, cdata))
561  return;
562  }
563  }
564  }
565  active.insert(index);
566  }
567  else
568  active.erase(index);
569  }
570 
571 }
572 
573 //==============================================================================
574 template <typename S>
575 void IntervalTreeCollisionManager<S>::distance(void* cdata, DistanceCallBack<S> callback) const
576 {
577  if(size() == 0) return;
578 
579  this->enable_tested_set_ = true;
580  this->tested_set.clear();
581  S min_dist = std::numeric_limits<S>::max();
582 
583  for(size_t i = 0; i < endpoints[0].size(); ++i)
584  if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break;
585 
586  this->enable_tested_set_ = false;
587  this->tested_set.clear();
588 }
589 
590 //==============================================================================
591 template <typename S>
592 void IntervalTreeCollisionManager<S>::collide(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, CollisionCallBack<S> callback) const
593 {
594  IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
595 
596  if((size() == 0) || (other_manager->size() == 0)) return;
597 
598  if(this == other_manager)
599  {
600  collide(cdata, callback);
601  return;
602  }
603 
604  if(this->size() < other_manager->size())
605  {
606  for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
607  if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return;
608  }
609  else
610  {
611  for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
612  if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return;
613  }
614 }
615 
616 //==============================================================================
617 template <typename S>
618 void IntervalTreeCollisionManager<S>::distance(BroadPhaseCollisionManager<S>* other_manager_, void* cdata, DistanceCallBack<S> callback) const
619 {
620  IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
621 
622  if((size() == 0) || (other_manager->size() == 0)) return;
623 
624  if(this == other_manager)
625  {
626  distance(cdata, callback);
627  return;
628  }
629 
630  S min_dist = std::numeric_limits<S>::max();
631 
632  if(this->size() < other_manager->size())
633  {
634  for(size_t i = 0, size = endpoints[0].size(); i < size; ++i)
635  if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return;
636  }
637  else
638  {
639  for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
640  if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return;
641  }
642 }
643 
644 //==============================================================================
645 template <typename S>
646 bool IntervalTreeCollisionManager<S>::empty() const
647 {
648  return endpoints[0].empty();
649 }
650 
651 //==============================================================================
652 template <typename S>
653 size_t IntervalTreeCollisionManager<S>::size() const
654 {
655  return endpoints[0].size() / 2;
656 }
657 
658 //==============================================================================
659 template <typename S>
660 bool IntervalTreeCollisionManager<S>::checkColl(
661  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
662  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
663  CollisionObject<S>* obj,
664  void* cdata,
665  CollisionCallBack<S> callback) const
666 {
667  while(pos_start < pos_end)
668  {
669  SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
670  if(ivl->obj != obj)
671  {
672  if(ivl->obj->getAABB().overlap(obj->getAABB()))
673  {
674  if(callback(ivl->obj, obj, cdata))
675  return true;
676  }
677  }
678 
679  pos_start++;
680  }
681 
682  return false;
683 }
684 
685 //==============================================================================
686 template <typename S>
687 bool IntervalTreeCollisionManager<S>::checkDist(
688  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_start,
689  typename std::deque<detail::SimpleInterval<S>*>::const_iterator pos_end,
690  CollisionObject<S>* obj,
691  void* cdata,
692  DistanceCallBack<S> callback,
693  S& min_dist) const
694 {
695  while(pos_start < pos_end)
696  {
697  SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
698  if(ivl->obj != obj)
699  {
700  if(!this->enable_tested_set_)
701  {
702  if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
703  {
704  if(callback(ivl->obj, obj, cdata, min_dist))
705  return true;
706  }
707  }
708  else
709  {
710  if(!this->inTestedSet(ivl->obj, obj))
711  {
712  if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist)
713  {
714  if(callback(ivl->obj, obj, cdata, min_dist))
715  return true;
716  }
717 
718  this->insertTestedSet(ivl->obj, obj);
719  }
720  }
721  }
722 
723  pos_start++;
724  }
725 
726  return false;
727 }
728 
729 //==============================================================================
730 template <typename S>
731 bool IntervalTreeCollisionManager<S>::EndPoint::operator<(
732  const typename IntervalTreeCollisionManager<S>::EndPoint& p) const
733 {
734  return value < p.value;
735 }
736 
737 //==============================================================================
738 template <typename S>
739 IntervalTreeCollisionManager<S>::SAPInterval::SAPInterval(
740  S low_, S high_, CollisionObject<S>* obj_)
741  : detail::SimpleInterval<S>()
742 {
743  this->low = low_;
744  this->high = high_;
745  obj = obj_;
746 }
747 
748 } // namespace fcl
749 
750 #endif
fcl::IntervalTreeCollisionManager::obj_interval_maps
std::map< CollisionObject< S > *, SAPInterval * > obj_interval_maps[3]
Definition: broadphase_interval_tree.h:139
fcl::distance
S distance(const Eigen::MatrixBase< DerivedA > &R0, const Eigen::MatrixBase< DerivedB > &T0, const kIOS< S > &b1, const kIOS< S > &b2, Vector3< S > *P, Vector3< S > *Q)
Approximate distance between two kIOS bounding volumes.
Definition: kIOS-inl.h:266
fcl::IntervalTreeCollisionManager::unregisterObject
void unregisterObject(CollisionObject< S > *obj)
add one object to the manager
fcl::IntervalTreeCollisionManager::endpoints
std::vector< EndPoint > endpoints[3]
vector stores all the end points
Definition: broadphase_interval_tree.h:134
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::IntervalTreeCollisionManager::IntervalTreeCollisionManager
IntervalTreeCollisionManager()
fcl::detail::overlap
bool overlap(S a1, S a2, S b1, S b2)
returns 1 if the intervals overlap, and 0 otherwise
Definition: interval_tree-inl.h:498
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::IntervalTreeCollisionManager::interval_trees
detail::IntervalTree< S > * interval_trees[3]
interval tree manages the intervals
Definition: broadphase_interval_tree.h:137
fcl::collide
template FCL_EXPORT std::size_t collide(const CollisionObject< double > *o1, const CollisionObject< double > *o2, const CollisionRequest< double > &request, CollisionResult< double > &result)
broadphase_interval_tree.h
minmax
char minmax
tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi
Definition: broadphase_SaP.h:181
fcl::IntervalTreeCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
aabb
SaPCollisionManager< S >::SaPAABB * aabb
back pointer to SAP interval
Definition: broadphase_SaP.h:184
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::IntervalTreeCollisionManager< double >
template class FCL_EXPORT IntervalTreeCollisionManager< double >
fcl::IntervalTreeCollisionManager::size
size_t size() const
the number of objects managed by the manager


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:49