broadphase_interval_tree.cpp
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 
39 
40 namespace hpp {
41 namespace fcl {
42 
43 //==============================================================================
45  // must sorted before
46  setup();
47 
48  EndPoint p;
49  p.value = obj->getAABB().min_[0];
50  auto start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p);
51  p.value = obj->getAABB().max_[0];
52  auto end1 = std::upper_bound(start1, endpoints[0].end(), p);
53 
54  if (start1 < end1) {
55  size_t start_id = (size_t)(start1 - endpoints[0].begin());
56  size_t end_id = (size_t)(end1 - endpoints[0].begin());
57  size_t cur_id = (size_t)(start_id);
58 
59  for (size_t i = start_id; i < end_id; ++i) {
60  if (endpoints[0][(size_t)i].obj != obj) {
61  if (i == cur_id)
62  cur_id++;
63  else {
64  endpoints[0][(size_t)cur_id] = endpoints[0][(size_t)i];
65  cur_id++;
66  }
67  }
68  }
69  if (cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2);
70  }
71 
72  p.value = obj->getAABB().min_[1];
73  auto start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p);
74  p.value = obj->getAABB().max_[1];
75  auto end2 = std::upper_bound(start2, endpoints[1].end(), p);
76 
77  if (start2 < end2) {
78  size_t start_id = (size_t)(start2 - endpoints[1].begin());
79  size_t end_id = (size_t)(end2 - endpoints[1].begin());
80  size_t cur_id = (size_t)(start_id);
81 
82  for (size_t i = start_id; i < end_id; ++i) {
83  if (endpoints[1][i].obj != obj) {
84  if (i == cur_id)
85  cur_id++;
86  else {
87  endpoints[1][cur_id] = endpoints[1][i];
88  cur_id++;
89  }
90  }
91  }
92  if (cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2);
93  }
94 
95  p.value = obj->getAABB().min_[2];
96  auto start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p);
97  p.value = obj->getAABB().max_[2];
98  auto end3 = std::upper_bound(start3, endpoints[2].end(), p);
99 
100  if (start3 < end3) {
101  size_t start_id = (size_t)(start3 - endpoints[2].begin());
102  size_t end_id = (size_t)(end3 - endpoints[2].begin());
103  size_t cur_id = (size_t)(start_id);
104 
105  for (size_t i = start_id; i < end_id; ++i) {
106  if (endpoints[2][i].obj != obj) {
107  if (i == cur_id)
108  cur_id++;
109  else {
110  endpoints[2][cur_id] = endpoints[2][i];
111  cur_id++;
112  }
113  }
114  }
115  if (cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2);
116  }
117 
118  // update the interval tree
119  if (obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) {
120  SAPInterval* ivl1 = obj_interval_maps[0][obj];
121  SAPInterval* ivl2 = obj_interval_maps[1][obj];
122  SAPInterval* ivl3 = obj_interval_maps[2][obj];
123 
124  interval_trees[0]->deleteNode(ivl1);
125  interval_trees[1]->deleteNode(ivl2);
126  interval_trees[2]->deleteNode(ivl3);
127 
128  delete ivl1;
129  delete ivl2;
130  delete ivl3;
131 
132  obj_interval_maps[0].erase(obj);
133  obj_interval_maps[1].erase(obj);
134  obj_interval_maps[2].erase(obj);
135  }
136 }
137 
138 //==============================================================================
140  for (int i = 0; i < 3; ++i) interval_trees[i] = nullptr;
141 }
142 
143 //==============================================================================
145 
146 //==============================================================================
148  EndPoint p, q;
149 
150  p.obj = obj;
151  q.obj = obj;
152  p.minmax = 0;
153  q.minmax = 1;
154  p.value = obj->getAABB().min_[0];
155  q.value = obj->getAABB().max_[0];
156  endpoints[0].push_back(p);
157  endpoints[0].push_back(q);
158 
159  p.value = obj->getAABB().min_[1];
160  q.value = obj->getAABB().max_[1];
161  endpoints[1].push_back(p);
162  endpoints[1].push_back(q);
163 
164  p.value = obj->getAABB().min_[2];
165  q.value = obj->getAABB().max_[2];
166  endpoints[2].push_back(p);
167  endpoints[2].push_back(q);
168  setup_ = false;
169 }
170 
171 //==============================================================================
173  if (!setup_) {
174  std::sort(endpoints[0].begin(), endpoints[0].end());
175  std::sort(endpoints[1].begin(), endpoints[1].end());
176  std::sort(endpoints[2].begin(), endpoints[2].end());
177 
178  for (int i = 0; i < 3; ++i) delete interval_trees[i];
179 
180  for (int i = 0; i < 3; ++i) interval_trees[i] = new detail::IntervalTree;
181 
182  for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
183  EndPoint p = endpoints[0][i];
184  CollisionObject* obj = p.obj;
185  if (p.minmax == 0) {
186  SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0],
187  obj->getAABB().max_[0], obj);
188  SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1],
189  obj->getAABB().max_[1], obj);
190  SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2],
191  obj->getAABB().max_[2], obj);
192 
193  interval_trees[0]->insert(ivl1);
194  interval_trees[1]->insert(ivl2);
195  interval_trees[2]->insert(ivl3);
196 
197  obj_interval_maps[0][obj] = ivl1;
198  obj_interval_maps[1][obj] = ivl2;
199  obj_interval_maps[2][obj] = ivl3;
200  }
201  }
202 
203  setup_ = true;
204  }
205 }
206 
207 //==============================================================================
209  setup_ = false;
210 
211  for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
212  if (endpoints[0][i].minmax == 0)
213  endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0];
214  else
215  endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0];
216  }
217 
218  for (size_t i = 0, size = endpoints[1].size(); i < size; ++i) {
219  if (endpoints[1][i].minmax == 0)
220  endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1];
221  else
222  endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1];
223  }
224 
225  for (size_t i = 0, size = endpoints[2].size(); i < size; ++i) {
226  if (endpoints[2][i].minmax == 0)
227  endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2];
228  else
229  endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2];
230  }
231 
232  setup();
233 }
234 
235 //==============================================================================
237  AABB old_aabb;
238  const AABB& new_aabb = updated_obj->getAABB();
239  for (int i = 0; i < 3; ++i) {
240  const auto it = obj_interval_maps[i].find(updated_obj);
241  interval_trees[i]->deleteNode(it->second);
242  old_aabb.min_[i] = it->second->low;
243  old_aabb.max_[i] = it->second->high;
244  it->second->low = new_aabb.min_[i];
245  it->second->high = new_aabb.max_[i];
246  interval_trees[i]->insert(it->second);
247  }
248 
249  EndPoint dummy;
250  typename std::vector<EndPoint>::iterator it;
251  for (int i = 0; i < 3; ++i) {
252  dummy.value = old_aabb.min_[i];
253  it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
254  for (; it != endpoints[i].end(); ++it) {
255  if (it->obj == updated_obj && it->minmax == 0) {
256  it->value = new_aabb.min_[i];
257  break;
258  }
259  }
260 
261  dummy.value = old_aabb.max_[i];
262  it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy);
263  for (; it != endpoints[i].end(); ++it) {
264  if (it->obj == updated_obj && it->minmax == 0) {
265  it->value = new_aabb.max_[i];
266  break;
267  }
268  }
269 
270  std::sort(endpoints[i].begin(), endpoints[i].end());
271  }
272 }
273 
274 //==============================================================================
276  const std::vector<CollisionObject*>& updated_objs) {
277  for (size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]);
278 }
279 
280 //==============================================================================
282  endpoints[0].clear();
283  endpoints[1].clear();
284  endpoints[2].clear();
285 
286  delete interval_trees[0];
287  interval_trees[0] = nullptr;
288  delete interval_trees[1];
289  interval_trees[1] = nullptr;
290  delete interval_trees[2];
291  interval_trees[2] = nullptr;
292 
293  for (int i = 0; i < 3; ++i) {
294  for (auto it = obj_interval_maps[i].cbegin(),
295  end = obj_interval_maps[i].cend();
296  it != end; ++it) {
297  delete it->second;
298  }
299  }
300 
301  for (int i = 0; i < 3; ++i) obj_interval_maps[i].clear();
302 
303  setup_ = false;
304 }
305 
306 //==============================================================================
308  std::vector<CollisionObject*>& objs) const {
309  objs.resize(endpoints[0].size() / 2);
310  size_t j = 0;
311  for (size_t i = 0, size = endpoints[0].size(); i < size; ++i) {
312  if (endpoints[0][i].minmax == 0) {
313  objs[j] = endpoints[0][i].obj;
314  j++;
315  }
316  }
317 }
318 
319 //==============================================================================
322  callback->init();
323  if (size() == 0) return;
324  collide_(obj, callback);
325 }
326 
327 //==============================================================================
330  static const unsigned int CUTOFF = 100;
331 
332  std::deque<detail::SimpleInterval*> results0, results1, results2;
333 
334  results0 =
335  interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]);
336  if (results0.size() > CUTOFF) {
337  results1 = interval_trees[1]->query(obj->getAABB().min_[1],
338  obj->getAABB().max_[1]);
339  if (results1.size() > CUTOFF) {
340  results2 = interval_trees[2]->query(obj->getAABB().min_[2],
341  obj->getAABB().max_[2]);
342  if (results2.size() > CUTOFF) {
343  size_t d1 = results0.size();
344  size_t d2 = results1.size();
345  size_t d3 = results2.size();
346 
347  if (d1 >= d2 && d1 >= d3)
348  return checkColl(results0.begin(), results0.end(), obj, callback);
349  else if (d2 >= d1 && d2 >= d3)
350  return checkColl(results1.begin(), results1.end(), obj, callback);
351  else
352  return checkColl(results2.begin(), results2.end(), obj, callback);
353  } else
354  return checkColl(results2.begin(), results2.end(), obj, callback);
355  } else
356  return checkColl(results1.begin(), results1.end(), obj, callback);
357  } else
358  return checkColl(results0.begin(), results0.end(), obj, callback);
359 }
360 
361 //==============================================================================
364  callback->init();
365  if (size() == 0) return;
366  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
367  distance_(obj, callback, min_dist);
368 }
369 
370 //==============================================================================
373  FCL_REAL& min_dist) const {
374  static const unsigned int CUTOFF = 100;
375 
376  Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
377  AABB aabb = obj->getAABB();
378  if (min_dist < (std::numeric_limits<FCL_REAL>::max)()) {
379  Vec3f min_dist_delta(min_dist, min_dist, min_dist);
380  aabb.expand(min_dist_delta);
381  }
382 
383  int status = 1;
384  FCL_REAL old_min_distance;
385 
386  while (1) {
387  bool dist_res = false;
388 
389  old_min_distance = min_dist;
390 
391  std::deque<detail::SimpleInterval*> results0, results1, results2;
392 
393  results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]);
394  if (results0.size() > CUTOFF) {
395  results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]);
396  if (results1.size() > CUTOFF) {
397  results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]);
398  if (results2.size() > CUTOFF) {
399  size_t d1 = results0.size();
400  size_t d2 = results1.size();
401  size_t d3 = results2.size();
402 
403  if (d1 >= d2 && d1 >= d3)
404  dist_res = checkDist(results0.begin(), results0.end(), obj,
405  callback, min_dist);
406  else if (d2 >= d1 && d2 >= d3)
407  dist_res = checkDist(results1.begin(), results1.end(), obj,
408  callback, min_dist);
409  else
410  dist_res = checkDist(results2.begin(), results2.end(), obj,
411  callback, min_dist);
412  } else
413  dist_res = checkDist(results2.begin(), results2.end(), obj, callback,
414  min_dist);
415  } else
416  dist_res = checkDist(results1.begin(), results1.end(), obj, callback,
417  min_dist);
418  } else
419  dist_res =
420  checkDist(results0.begin(), results0.end(), obj, callback, min_dist);
421 
422  if (dist_res) return true;
423 
424  results0.clear();
425  results1.clear();
426  results2.clear();
427 
428  if (status == 1) {
429  if (old_min_distance < (std::numeric_limits<FCL_REAL>::max)())
430  break;
431  else {
432  if (min_dist < old_min_distance) {
433  Vec3f min_dist_delta(min_dist, min_dist, min_dist);
434  aabb = AABB(obj->getAABB(), min_dist_delta);
435  status = 0;
436  } else {
437  if (aabb == obj->getAABB())
438  aabb.expand(delta);
439  else
440  aabb.expand(obj->getAABB(), 2.0);
441  }
442  }
443  } else if (status == 0)
444  break;
445  }
446 
447  return false;
448 }
449 
450 //==============================================================================
453  callback->init();
454  if (size() == 0) return;
455 
456  std::set<CollisionObject*> active;
457  std::set<std::pair<CollisionObject*, CollisionObject*> > overlap;
458  size_t n = endpoints[0].size();
459  FCL_REAL diff_x = endpoints[0][0].value - endpoints[0][n - 1].value;
460  FCL_REAL diff_y = endpoints[1][0].value - endpoints[1][n - 1].value;
461  FCL_REAL diff_z = endpoints[2][0].value - endpoints[2][n - 1].value;
462 
463  int axis = 0;
464  if (diff_y > diff_x && diff_y > diff_z)
465  axis = 1;
466  else if (diff_z > diff_y && diff_z > diff_x)
467  axis = 2;
468 
469  for (unsigned int i = 0; i < n; ++i) {
470  const EndPoint& endpoint = endpoints[axis][i];
471  CollisionObject* index = endpoint.obj;
472  if (endpoint.minmax == 0) {
473  auto iter = active.begin();
474  auto end = active.end();
475  for (; iter != end; ++iter) {
476  CollisionObject* active_index = *iter;
477  const AABB& b0 = active_index->getAABB();
478  const AABB& b1 = index->getAABB();
479 
480  int axis2 = (axis + 1) % 3;
481  int axis3 = (axis + 2) % 3;
482 
483  if (b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) {
484  std::pair<typename std::set<std::pair<CollisionObject*,
485  CollisionObject*> >::iterator,
486  bool>
487  insert_res;
488  if (active_index < index)
489  insert_res = overlap.insert(std::make_pair(active_index, index));
490  else
491  insert_res = overlap.insert(std::make_pair(index, active_index));
492 
493  if (insert_res.second) {
494  if ((*callback)(active_index, index)) return;
495  }
496  }
497  }
498  active.insert(index);
499  } else
500  active.erase(index);
501  }
502 }
503 
504 //==============================================================================
507  callback->init();
508  if (size() == 0) return;
509 
510  this->enable_tested_set_ = true;
511  this->tested_set.clear();
512  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
513 
514  for (size_t i = 0; i < endpoints[0].size(); ++i)
515  if (distance_(endpoints[0][i].obj, callback, min_dist)) break;
516 
517  this->enable_tested_set_ = false;
518  this->tested_set.clear();
519 }
520 
521 //==============================================================================
523  BroadPhaseCollisionManager* other_manager_,
525  callback->init();
526  IntervalTreeCollisionManager* other_manager =
527  static_cast<IntervalTreeCollisionManager*>(other_manager_);
528 
529  if ((size() == 0) || (other_manager->size() == 0)) return;
530 
531  if (this == other_manager) {
532  collide(callback);
533  return;
534  }
535 
536  if (this->size() < other_manager->size()) {
537  for (size_t i = 0, size = endpoints[0].size(); i < size; ++i)
538  if (other_manager->collide_(endpoints[0][i].obj, callback)) return;
539  } else {
540  for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
541  if (collide_(other_manager->endpoints[0][i].obj, callback)) return;
542  }
543 }
544 
545 //==============================================================================
547  BroadPhaseCollisionManager* other_manager_,
549  callback->init();
550  IntervalTreeCollisionManager* other_manager =
551  static_cast<IntervalTreeCollisionManager*>(other_manager_);
552 
553  if ((size() == 0) || (other_manager->size() == 0)) return;
554 
555  if (this == other_manager) {
557  return;
558  }
559 
560  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
561 
562  if (this->size() < other_manager->size()) {
563  for (size_t i = 0, size = endpoints[0].size(); i < size; ++i)
564  if (other_manager->distance_(endpoints[0][i].obj, callback, min_dist))
565  return;
566  } else {
567  for (size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i)
568  if (distance_(other_manager->endpoints[0][i].obj, callback, min_dist))
569  return;
570  }
571 }
572 
573 //==============================================================================
575  return endpoints[0].empty();
576 }
577 
578 //==============================================================================
580  return endpoints[0].size() / 2;
581 }
582 
583 //==============================================================================
585  typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
586  typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
588  while (pos_start < pos_end) {
589  SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
590  if (ivl->obj != obj) {
591  if (ivl->obj->getAABB().overlap(obj->getAABB())) {
592  if ((*callback)(ivl->obj, obj)) return true;
593  }
594  }
595 
596  pos_start++;
597  }
598 
599  return false;
600 }
601 
602 //==============================================================================
604  typename std::deque<detail::SimpleInterval*>::const_iterator pos_start,
605  typename std::deque<detail::SimpleInterval*>::const_iterator pos_end,
607  FCL_REAL& min_dist) const {
608  while (pos_start < pos_end) {
609  SAPInterval* ivl = static_cast<SAPInterval*>(*pos_start);
610  if (ivl->obj != obj) {
611  if (!this->enable_tested_set_) {
612  if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) {
613  if ((*callback)(ivl->obj, obj, min_dist)) return true;
614  }
615  } else {
616  if (!this->inTestedSet(ivl->obj, obj)) {
617  if (ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) {
618  if ((*callback)(ivl->obj, obj, min_dist)) return true;
619  }
620 
621  this->insertTestedSet(ivl->obj, obj);
622  }
623  }
624  }
625 
626  pos_start++;
627  }
628 
629  return false;
630 }
631 
632 //==============================================================================
634  const typename IntervalTreeCollisionManager::EndPoint& p) const {
635  return value < p.value;
636 }
637 
638 //==============================================================================
640  FCL_REAL high_,
641  CollisionObject* obj_)
642  : detail::SimpleInterval() {
643  this->low = low_;
644  this->high = high_;
645  obj = obj_;
646 }
647 
648 } // namespace fcl
649 } // namespace hpp
hpp::fcl::IntervalTreeCollisionManager::distance_
bool distance_(CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
Definition: broadphase_interval_tree.cpp:371
hpp::fcl::AABB::axisOverlap
bool axisOverlap(const AABB &other, int axis_id) const
Check whether two AABB are overlapped along specific axis.
Definition: BV/AABB.h:192
hpp::fcl::IntervalTreeCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_interval_tree.cpp:172
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
hpp::fcl::detail::IntervalTree::query
std::deque< SimpleInterval * > query(FCL_REAL low, FCL_REAL high)
Return result for a given query.
Definition: interval_tree.cpp:419
hpp::fcl::AABB::overlap
bool overlap(const AABB &other) const
Check whether two AABB are overlap.
Definition: BV/AABB.h:110
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
collision_manager.callback
callback
Definition: collision_manager.py:27
hpp::fcl::detail::SimpleInterval::high
FCL_REAL high
Definition: simple_interval.h:57
hpp::fcl::IntervalTreeCollisionManager::collide_
bool collide_(CollisionObject *obj, CollisionCallBackBase *callback) const
Definition: broadphase_interval_tree.cpp:328
hpp::fcl::BroadPhaseCollisionManager::inTestedSet
bool inTestedSet(CollisionObject *a, CollisionObject *b) const
Definition: broadphase_collision_manager.cpp:76
hpp::fcl::IntervalTreeCollisionManager::EndPoint::operator<
bool operator<(const EndPoint &p) const
Definition: broadphase_interval_tree.cpp:633
hpp::fcl::IntervalTreeCollisionManager::interval_trees
detail::IntervalTree * interval_trees[3]
interval tree manages the intervals
Definition: broadphase_interval_tree.h:160
doxygen_xml_parser.index
index
Definition: doxygen_xml_parser.py:886
hpp::fcl::IntervalTreeCollisionManager::obj_interval_maps
std::map< CollisionObject *, SAPInterval * > obj_interval_maps[3]
Definition: broadphase_interval_tree.h:162
hpp::fcl::BroadPhaseCollisionManager::tested_set
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
hpp::fcl::IntervalTreeCollisionManager::setup_
bool setup_
tag for whether the interval tree is maintained suitably
Definition: broadphase_interval_tree.h:165
hpp::fcl::AABB::min_
Vec3f min_
The min point in the AABB.
Definition: BV/AABB.h:57
hpp::fcl::IntervalTreeCollisionManager::size
size_t size() const
the number of objects managed by the manager
Definition: broadphase_interval_tree.cpp:579
hpp::fcl::AABB::max_
Vec3f max_
The max point in the AABB.
Definition: BV/AABB.h:59
hpp::fcl::IntervalTreeCollisionManager::endpoints
std::vector< EndPoint > endpoints[3]
vector stores all the end points
Definition: broadphase_interval_tree.h:157
hpp::fcl::IntervalTreeCollisionManager::collide
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager
Definition: broadphase_interval_tree.cpp:320
hpp::fcl::IntervalTreeCollisionManager::checkColl
bool checkColl(typename std::deque< detail::SimpleInterval * >::const_iterator pos_start, typename std::deque< detail::SimpleInterval * >::const_iterator pos_end, CollisionObject *obj, CollisionCallBackBase *callback) const
Definition: broadphase_interval_tree.cpp:584
hpp::fcl::IntervalTreeCollisionManager::distance
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: broadphase_interval_tree.cpp:362
hpp::fcl::minmax
void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL &minv, FCL_REAL &maxv)
Find the smaller and larger one of two values.
Definition: kDOP.cpp:48
hpp::fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
hpp::fcl::IntervalTreeCollisionManager::update
virtual void update()
update the condition of manager
Definition: broadphase_interval_tree.cpp:208
hpp::fcl::BroadPhaseCollisionManager::enable_tested_set_
bool enable_tested_set_
Definition: broadphase_collision_manager.h:131
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DistanceCallBackBase
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
hpp::fcl::IntervalTreeCollisionManager::EndPoint::value
FCL_REAL value
end point value
Definition: broadphase_interval_tree.h:123
hpp::fcl::IntervalTreeCollisionManager::IntervalTreeCollisionManager
IntervalTreeCollisionManager()
Definition: broadphase_interval_tree.cpp:139
hpp::fcl::IntervalTreeCollisionManager::unregisterObject
void unregisterObject(CollisionObject *obj)
add one object to the manager
Definition: broadphase_interval_tree.cpp:44
hpp::fcl::detail::SimpleInterval::low
FCL_REAL low
interval is defined as [low, high]
Definition: simple_interval.h:57
hpp::fcl::BroadPhaseCollisionManager::getObjects
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Definition: broadphase_collision_manager.h:88
hpp::fcl::IntervalTreeCollisionManager::~IntervalTreeCollisionManager
~IntervalTreeCollisionManager()
Definition: broadphase_interval_tree.cpp:144
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::AABB::distance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
axis
axis
q
q
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
broadphase_interval_tree.h
hpp::fcl::IntervalTreeCollisionManager::registerObject
void registerObject(CollisionObject *obj)
remove one object from the manager
Definition: broadphase_interval_tree.cpp:147
hpp::fcl::IntervalTreeCollisionManager::SAPInterval
Extention interval tree's interval to SAP interval, adding more information.
Definition: broadphase_interval_tree.h:134
hpp::fcl::IntervalTreeCollisionManager::SAPInterval::SAPInterval
SAPInterval(FCL_REAL low_, FCL_REAL high_, CollisionObject *obj_)
Definition: broadphase_interval_tree.cpp:639
hpp::fcl::IntervalTreeCollisionManager::checkDist
bool checkDist(typename std::deque< detail::SimpleInterval * >::const_iterator pos_start, typename std::deque< detail::SimpleInterval * >::const_iterator pos_end, CollisionObject *obj, DistanceCallBackBase *callback, FCL_REAL &min_dist) const
Definition: broadphase_interval_tree.cpp:603
hpp::fcl::IntervalTreeCollisionManager::EndPoint
SAP end point.
Definition: broadphase_interval_tree.h:118
hpp::fcl::IntervalTreeCollisionManager::empty
bool empty() const
whether the manager is empty
Definition: broadphase_interval_tree.cpp:574
hpp::fcl::detail::IntervalTree
Interval tree.
Definition: interval_tree.h:64
hpp::fcl::overlap
HPP_FCL_DLLAPI bool overlap(const Matrix3f &R0, const Vec3f &T0, const AABB &b1, const AABB &b2)
Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
Definition: AABB.cpp:134
hpp::fcl::CollisionObject::getAABB
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
hpp::fcl::IntervalTreeCollisionManager::SAPInterval::obj
CollisionObject * obj
Definition: broadphase_interval_tree.h:135
hpp::fcl::detail::IntervalTree::insert
IntervalTreeNode * insert(SimpleInterval *new_interval)
Insert one node of the interval tree.
Definition: interval_tree.cpp:183
hpp::fcl::CollisionCallBackBase
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
hpp::fcl::IntervalTreeCollisionManager::clear
void clear()
clear the manager
Definition: broadphase_interval_tree.cpp:281
hpp::fcl::IntervalTreeCollisionManager::EndPoint::obj
CollisionObject * obj
object related with the end point
Definition: broadphase_interval_tree.h:120
hpp::fcl::BroadPhaseCollisionManager::insertTestedSet
void insertTestedSet(CollisionObject *a, CollisionObject *b) const
Definition: broadphase_collision_manager.cpp:85
hpp::fcl::detail::IntervalTree::deleteNode
SimpleInterval * deleteNode(IntervalTreeNode *node)
Delete one node of the interval tree.
Definition: interval_tree.cpp:362
hpp::fcl::AABB::expand
AABB & expand(const Vec3f &delta)
expand the half size of the AABB by delta, and keep the center unchanged.
Definition: BV/AABB.h:201
hpp::fcl::IntervalTreeCollisionManager::EndPoint::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_interval_tree.h:127
hpp::fcl::IntervalTreeCollisionManager
Collision manager based on interval tree.
Definition: broadphase_interval_tree.h:51


hpp-fcl
Author(s):
autogenerated on Fri Jan 26 2024 03:46:12