collision_func_matrix-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_COLLISION_FUNC_MATRIX_INL_H
39 #define FCL_COLLISION_FUNC_MATRIX_INL_H
40 
42 
43 #include "fcl/config.h"
44 
45 #include "fcl/common/unused.h"
46 
48 
49 #include "fcl/geometry/shape/box.h"
60 
62 
72 
73 #if FCL_HAVE_OCTOMAP
74 
80 
81 #endif // FCL_HAVE_OCTOMAP
82 
83 namespace fcl
84 {
85 
86 namespace detail
87 {
88 
89 #if FCL_HAVE_OCTOMAP
90 
91 //==============================================================================
92 template <typename Shape, typename NarrowPhaseSolver>
93 std::size_t ShapeOcTreeCollide(
94  const CollisionGeometry<typename Shape::S>* o1,
95  const Transform3<typename Shape::S>& tf1,
96  const CollisionGeometry<typename Shape::S>* o2,
97  const Transform3<typename Shape::S>& tf2,
98  const NarrowPhaseSolver* nsolver,
99  const CollisionRequest<typename Shape::S>& request,
100  CollisionResult<typename Shape::S>& result)
101 {
102  using S = typename Shape::S;
103 
104  if(request.isSatisfied(result)) return result.numContacts();
105 
106  ShapeOcTreeCollisionTraversalNode<Shape, NarrowPhaseSolver> node;
107  const Shape* obj1 = static_cast<const Shape*>(o1);
108  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
109  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
110 
111  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
112  collide(&node);
113 
114  return result.numContacts();
115 }
116 
117 //==============================================================================
118 template <typename Shape, typename NarrowPhaseSolver>
119 std::size_t OcTreeShapeCollide(
120  const CollisionGeometry<typename Shape::S>* o1,
121  const Transform3<typename Shape::S>& tf1,
122  const CollisionGeometry<typename Shape::S>* o2,
123  const Transform3<typename Shape::S>& tf2,
124  const NarrowPhaseSolver* nsolver,
125  const CollisionRequest<typename Shape::S>& request,
126  CollisionResult<typename Shape::S>& result)
127 {
128  using S = typename Shape::S;
129 
130  if(request.isSatisfied(result)) return result.numContacts();
131 
132  OcTreeShapeCollisionTraversalNode<Shape, NarrowPhaseSolver> node;
133  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
134  const Shape* obj2 = static_cast<const Shape*>(o2);
135  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
136 
137  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
138  collide(&node);
139 
140  return result.numContacts();
141 }
142 
143 //==============================================================================
144 template <typename NarrowPhaseSolver>
145 std::size_t OcTreeCollide(
146  const CollisionGeometry<typename NarrowPhaseSolver::S>* o1,
147  const Transform3<typename NarrowPhaseSolver::S>& tf1,
148  const CollisionGeometry<typename NarrowPhaseSolver::S>* o2,
149  const Transform3<typename NarrowPhaseSolver::S>& tf2,
150  const NarrowPhaseSolver* nsolver,
151  const CollisionRequest<typename NarrowPhaseSolver::S>& request,
152  CollisionResult<typename NarrowPhaseSolver::S>& result)
153 {
154  using S = typename NarrowPhaseSolver::S;
155 
156  if(request.isSatisfied(result)) return result.numContacts();
157 
158  OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
159  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
160  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
161  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
162 
163  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
164  collide(&node);
165 
166  return result.numContacts();
167 }
168 
169 //==============================================================================
170 template <typename BV, typename NarrowPhaseSolver>
171 std::size_t OcTreeBVHCollide(
172  const CollisionGeometry<typename BV::S>* o1,
173  const Transform3<typename BV::S>& tf1,
174  const CollisionGeometry<typename BV::S>* o2,
175  const Transform3<typename BV::S>& tf2,
176  const NarrowPhaseSolver* nsolver,
177  const CollisionRequest<typename BV::S>& request,
178  CollisionResult<typename BV::S>& result)
179 {
180  using S = typename BV::S;
181 
182  if(request.isSatisfied(result)) return result.numContacts();
183 
184  if(request.enable_cost && request.use_approximate_cost)
185  {
186  CollisionRequest<S> no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
187  no_cost_request.enable_cost = false; // disable cost computation
188 
189  OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver> node;
190  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
191  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
192  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
193 
194  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
195  collide(&node);
196 
197  Box<S> box;
198  Transform3<S> box_tf;
199  constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node
200 
201  box.cost_density = obj2->cost_density;
202  box.threshold_occupied = obj2->threshold_occupied;
203  box.threshold_free = obj2->threshold_free;
204 
205  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts
206  OcTreeShapeCollide<Box<S>, NarrowPhaseSolver>(o1, tf1, &box, box_tf, nsolver, only_cost_request, result);
207  }
208  else
209  {
210  OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver> node;
211  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
212  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
213  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
214 
215  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
216  collide(&node);
217  }
218 
219  return result.numContacts();
220 }
221 
222 //==============================================================================
223 template <typename BV, typename NarrowPhaseSolver>
224 std::size_t BVHOcTreeCollide(
225  const CollisionGeometry<typename BV::S>* o1,
226  const Transform3<typename BV::S>& tf1,
227  const CollisionGeometry<typename BV::S>* o2,
228  const Transform3<typename BV::S>& tf2,
229  const NarrowPhaseSolver* nsolver,
230  const CollisionRequest<typename BV::S>& request,
231  CollisionResult<typename BV::S>& result)
232 {
233  using S = typename BV::S;
234 
235  if(request.isSatisfied(result)) return result.numContacts();
236 
237  if(request.enable_cost && request.use_approximate_cost)
238  {
239  CollisionRequest<S> no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
240  no_cost_request.enable_cost = false; // disable cost computation
241 
242  MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver> node;
243  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
244  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
245  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
246 
247  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
248  collide(&node);
249 
250  Box<S> box;
251  Transform3<S> box_tf;
252  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
253 
254  box.cost_density = obj1->cost_density;
255  box.threshold_occupied = obj1->threshold_occupied;
256  box.threshold_free = obj1->threshold_free;
257 
258  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
259  ShapeOcTreeCollide<Box<S>, NarrowPhaseSolver>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
260  }
261  else
262  {
263  MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver> node;
264  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
265  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
266  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
267 
268  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
269  collide(&node);
270  }
271 
272  return result.numContacts();
273 }
274 
275 #endif
276 
277 //==============================================================================
278 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
279 std::size_t ShapeShapeCollide(
284  const NarrowPhaseSolver* nsolver,
287 {
288  if(request.isSatisfied(result)) return result.numContacts();
289 
291  const Shape1* obj1 = static_cast<const Shape1*>(o1);
292  const Shape2* obj2 = static_cast<const Shape2*>(o2);
293 
294  if(request.enable_cached_gjk_guess)
295  {
296  nsolver->enableCachedGuess(true);
297  nsolver->setCachedGuess(request.cached_gjk_guess);
298  }
299  else
300  {
301  nsolver->enableCachedGuess(true);
302  }
303 
304  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
305  collide(&node);
306 
307  if(request.enable_cached_gjk_guess)
308  result.cached_gjk_guess = nsolver->getCachedGuess();
309 
310  return result.numContacts();
311 }
312 
313 //==============================================================================
314 template <typename BV, typename Shape, typename NarrowPhaseSolver>
316 {
317  using S = typename BV::S;
318 
319  static std::size_t collide(
320  const CollisionGeometry<S>* o1,
321  const Transform3<S>& tf1,
322  const CollisionGeometry<S>* o2,
323  const Transform3<S>& tf2,
324  const NarrowPhaseSolver* nsolver,
325  const CollisionRequest<S>& request,
326  CollisionResult<S>& result)
327  {
328  if(request.isSatisfied(result)) return result.numContacts();
329 
330  if(request.enable_cost && request.use_approximate_cost)
331  {
332  CollisionRequest<S> no_cost_request(request);
333  no_cost_request.enable_cost = false;
334 
336  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
337  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
338  Transform3<S> tf1_tmp = tf1;
339  const Shape* obj2 = static_cast<const Shape*>(o2);
340 
341  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
342  fcl::detail::collide(&node);
343 
344  delete obj1_tmp;
345 
346  Box<S> box;
347  Transform3<S> box_tf;
348  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
349 
350  box.cost_density = obj1->cost_density;
351  box.threshold_occupied = obj1->threshold_occupied;
352  box.threshold_free = obj1->threshold_free;
353 
354  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
355  ShapeShapeCollide<Box<S>, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
356  }
357  else
358  {
360  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
361  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
362  Transform3<S> tf1_tmp = tf1;
363  const Shape* obj2 = static_cast<const Shape*>(o2);
364 
365  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
366  fcl::detail::collide(&node);
367 
368  delete obj1_tmp;
369  }
370 
371  return result.numContacts();
372  }
373 };
374 
375 //==============================================================================
376 template <typename OrientMeshShapeCollisionTraveralNode,
377  typename BV, typename Shape, typename NarrowPhaseSolver>
380  const Transform3<typename BV::S>& tf1,
382  const Transform3<typename BV::S>& tf2,
383  const NarrowPhaseSolver* nsolver,
384  const CollisionRequest<typename BV::S>& request,
386 {
387  using S = typename BV::S;
388 
389  if(request.isSatisfied(result)) return result.numContacts();
390 
391  if(request.enable_cost && request.use_approximate_cost)
392  {
393  CollisionRequest<S> no_cost_request(request);
394  no_cost_request.enable_cost = false;
395 
396  OrientMeshShapeCollisionTraveralNode node;
397  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
398  const Shape* obj2 = static_cast<const Shape*>(o2);
399 
400  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result);
401  fcl::detail::collide(&node);
402 
403  Box<S> box;
404  Transform3<S> box_tf;
405  constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
406 
407  box.cost_density = obj1->cost_density;
408  box.threshold_occupied = obj1->threshold_occupied;
409  box.threshold_free = obj1->threshold_free;
410 
411  CollisionRequest<S> only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
412  ShapeShapeCollide<Box<S>, Shape>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
413  }
414  else
415  {
416  OrientMeshShapeCollisionTraveralNode node;
417  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
418  const Shape* obj2 = static_cast<const Shape*>(o2);
419 
420  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
421  fcl::detail::collide(&node);
422  }
423 
424  return result.numContacts();
425 }
426 
427 //==============================================================================
428 template <typename Shape, typename NarrowPhaseSolver>
430  OBB<typename Shape::S>, Shape, NarrowPhaseSolver>
431 {
432  using S = typename Shape::S;
433 
434  static std::size_t collide(
435  const CollisionGeometry<S>* o1,
436  const Transform3<S>& tf1,
437  const CollisionGeometry<S>* o2,
438  const Transform3<S>& tf2,
439  const NarrowPhaseSolver* nsolver,
440  const CollisionRequest<S>& request,
441  CollisionResult<S>& result)
442  {
445  OBB<S>,
446  Shape,
447  NarrowPhaseSolver>(
448  o1, tf1, o2, tf2, nsolver, request, result);
449  }
450 };
451 
452 //==============================================================================
453 template <typename Shape, typename NarrowPhaseSolver>
454 struct BVHShapeCollider<RSS<typename Shape::S>, Shape, NarrowPhaseSolver>
455 {
456  using S = typename Shape::S;
457 
458  static std::size_t collide(
459  const CollisionGeometry<S>* o1,
460  const Transform3<S>& tf1,
461  const CollisionGeometry<S>* o2,
462  const Transform3<S>& tf2,
463  const NarrowPhaseSolver* nsolver,
464  const CollisionRequest<S>& request,
465  CollisionResult<S>& result)
466  {
469  RSS<S>,
470  Shape,
471  NarrowPhaseSolver>(
472  o1, tf1, o2, tf2, nsolver, request, result);
473  }
474 };
475 
476 //==============================================================================
477 template <typename Shape, typename NarrowPhaseSolver>
478 struct BVHShapeCollider<kIOS<typename Shape::S>, Shape, NarrowPhaseSolver>
479 {
480  using S = typename Shape::S;
481 
482  static std::size_t collide(
483  const CollisionGeometry<S>* o1,
484  const Transform3<S>& tf1,
485  const CollisionGeometry<S>* o2,
486  const Transform3<S>& tf2,
487  const NarrowPhaseSolver* nsolver,
488  const CollisionRequest<S>& request,
489  CollisionResult<S>& result)
490  {
493  kIOS<S>,
494  Shape,
495  NarrowPhaseSolver>(
496  o1, tf1, o2, tf2, nsolver, request, result);
497  }
498 };
499 
500 //==============================================================================
501 template <typename Shape, typename NarrowPhaseSolver>
502 struct BVHShapeCollider<OBBRSS<typename Shape::S>, Shape, NarrowPhaseSolver>
503 {
504  using S = typename Shape::S;
505 
506  static std::size_t collide(
507  const CollisionGeometry<S>* o1,
508  const Transform3<S>& tf1,
509  const CollisionGeometry<S>* o2,
510  const Transform3<S>& tf2,
511  const NarrowPhaseSolver* nsolver,
512  const CollisionRequest<S>& request,
513  CollisionResult<S>& result)
514  {
517  OBBRSS<S>,
518  Shape,
519  NarrowPhaseSolver>(
520  o1, tf1, o2, tf2, nsolver, request, result);
521  }
522 };
523 
524 //==============================================================================
525 template <typename S, typename BV>
527 {
528  static std::size_t run(
529  const CollisionGeometry<S>* o1,
530  const Transform3<S>& tf1,
531  const CollisionGeometry<S>* o2,
532  const Transform3<S>& tf2,
533  const CollisionRequest<S>& request,
534  CollisionResult<S>& result)
535  {
536  if(request.isSatisfied(result)) return result.numContacts();
537 
539  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
540  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
541  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
542  Transform3<S> tf1_tmp = tf1;
543  BVHModel<BV>* obj2_tmp = new BVHModel<BV>(*obj2);
544  Transform3<S> tf2_tmp = tf2;
545 
546  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
547  collide(&node);
548 
549  delete obj1_tmp;
550  delete obj2_tmp;
551 
552  return result.numContacts();
553  }
554 };
555 
556 //==============================================================================
557 template <typename BV>
558 std::size_t BVHCollide(
560  const Transform3<typename BV::S>& tf1,
562  const Transform3<typename BV::S>& tf2,
563  const CollisionRequest<typename BV::S>& request,
565 {
567  o1, tf1, o2, tf2, request, result);
568 }
569 
570 //==============================================================================
571 template <typename OrientedMeshCollisionTraversalNode, typename BV>
574  const Transform3<typename BV::S>& tf1,
576  const Transform3<typename BV::S>& tf2,
577  const CollisionRequest<typename BV::S>& request,
579 {
580  if(request.isSatisfied(result)) return result.numContacts();
581 
582  OrientedMeshCollisionTraversalNode node;
583  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
584  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
585 
586  initialize(node, *obj1, tf1, *obj2, tf2, request, result);
587  collide(&node);
588 
589  return result.numContacts();
590 }
591 
592 //==============================================================================
593 template <typename S>
594 struct BVHCollideImpl<S, OBB<S>>
595 {
596  static std::size_t run(
597  const CollisionGeometry<S>* o1,
598  const Transform3<S>& tf1,
599  const CollisionGeometry<S>* o2,
600  const Transform3<S>& tf2,
601  const CollisionRequest<S>& request,
602  CollisionResult<S>& result)
603  {
606  o1, tf1, o2, tf2, request, result);
607  }
608 };
609 
610 //==============================================================================
611 template <typename S>
612 struct BVHCollideImpl<S, OBBRSS<S>>
613 {
614  static std::size_t run(
615  const CollisionGeometry<S>* o1,
616  const Transform3<S>& tf1,
617  const CollisionGeometry<S>* o2,
618  const Transform3<S>& tf2,
619  const CollisionRequest<S>& request,
620  CollisionResult<S>& result)
621  {
624  o1, tf1, o2, tf2, request, result);
625  }
626 };
627 
628 //==============================================================================
629 template <typename S>
630 struct BVHCollideImpl<S, kIOS<S>>
631 {
632  static std::size_t run(
633  const CollisionGeometry<S>* o1,
634  const Transform3<S>& tf1,
635  const CollisionGeometry<S>* o2,
636  const Transform3<S>& tf2,
637  const CollisionRequest<S>& request,
638  CollisionResult<S>& result)
639  {
642  o1, tf1, o2, tf2, request, result);
643  }
644 };
645 
646 //==============================================================================
647 template <typename BV, typename NarrowPhaseSolver>
648 std::size_t BVHCollide(
650  const Transform3<typename BV::S>& tf1,
652  const Transform3<typename BV::S>& tf2,
653  const NarrowPhaseSolver* nsolver,
654  const CollisionRequest<typename BV::S>& request,
656 {
657  FCL_UNUSED(nsolver);
658 
659  return BVHCollide<BV>(o1, tf1, o2, tf2, request, result);
660 }
661 
662 //==============================================================================
663 template <typename NarrowPhaseSolver>
665 {
666  using S = typename NarrowPhaseSolver::S;
667 
668  for(int i = 0; i < NODE_COUNT; ++i)
669  {
670  for(int j = 0; j < NODE_COUNT; ++j)
671  collision_matrix[i][j] = nullptr;
672  }
673 
674  collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box<S>, Box<S>, NarrowPhaseSolver>;
675  collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box<S>, Sphere<S>, NarrowPhaseSolver>;
676  collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Box<S>, Ellipsoid<S>, NarrowPhaseSolver>;
677  collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box<S>, Capsule<S>, NarrowPhaseSolver>;
678  collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box<S>, Cone<S>, NarrowPhaseSolver>;
679  collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box<S>, Cylinder<S>, NarrowPhaseSolver>;
680  collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box<S>, Convex<S>, NarrowPhaseSolver>;
681  collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box<S>, Plane<S>, NarrowPhaseSolver>;
682  collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide<Box<S>, Halfspace<S>, NarrowPhaseSolver>;
683 
684  collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere<S>, Box<S>, NarrowPhaseSolver>;
685  collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere<S>, Sphere<S>, NarrowPhaseSolver>;
686  collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Sphere<S>, Ellipsoid<S>, NarrowPhaseSolver>;
687  collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere<S>, Capsule<S>, NarrowPhaseSolver>;
688  collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere<S>, Cone<S>, NarrowPhaseSolver>;
689  collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere<S>, Cylinder<S>, NarrowPhaseSolver>;
690  collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere<S>, Convex<S>, NarrowPhaseSolver>;
691  collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere<S>, Plane<S>, NarrowPhaseSolver>;
692  collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide<Sphere<S>, Halfspace<S>, NarrowPhaseSolver>;
693 
694  collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide<Ellipsoid<S>, Box<S>, NarrowPhaseSolver>;
695  collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide<Ellipsoid<S>, Sphere<S>, NarrowPhaseSolver>;
696  collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide<Ellipsoid<S>, Ellipsoid<S>, NarrowPhaseSolver>;
697  collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide<Ellipsoid<S>, Capsule<S>, NarrowPhaseSolver>;
698  collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide<Ellipsoid<S>, Cone<S>, NarrowPhaseSolver>;
699  collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide<Ellipsoid<S>, Cylinder<S>, NarrowPhaseSolver>;
700  collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide<Ellipsoid<S>, Convex<S>, NarrowPhaseSolver>;
701  collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide<Ellipsoid<S>, Plane<S>, NarrowPhaseSolver>;
702  collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide<Ellipsoid<S>, Halfspace<S>, NarrowPhaseSolver>;
703 
704  collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule<S>, Box<S>, NarrowPhaseSolver>;
705  collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule<S>, Sphere<S>, NarrowPhaseSolver>;
706  collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Capsule<S>, Ellipsoid<S>, NarrowPhaseSolver>;
707  collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule<S>, Capsule<S>, NarrowPhaseSolver>;
708  collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule<S>, Cone<S>, NarrowPhaseSolver>;
709  collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule<S>, Cylinder<S>, NarrowPhaseSolver>;
710  collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule<S>, Convex<S>, NarrowPhaseSolver>;
711  collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule<S>, Plane<S>, NarrowPhaseSolver>;
712  collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide<Capsule<S>, Halfspace<S>, NarrowPhaseSolver>;
713 
714  collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone<S>, Box<S>, NarrowPhaseSolver>;
715  collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone<S>, Sphere<S>, NarrowPhaseSolver>;
716  collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cone<S>, Ellipsoid<S>, NarrowPhaseSolver>;
717  collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone<S>, Capsule<S>, NarrowPhaseSolver>;
718  collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone<S>, Cone<S>, NarrowPhaseSolver>;
719  collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone<S>, Cylinder<S>, NarrowPhaseSolver>;
720  collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone<S>, Convex<S>, NarrowPhaseSolver>;
721  collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone<S>, Plane<S>, NarrowPhaseSolver>;
722  collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide<Cone<S>, Halfspace<S>, NarrowPhaseSolver>;
723 
724  collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder<S>, Box<S>, NarrowPhaseSolver>;
725  collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder<S>, Sphere<S>, NarrowPhaseSolver>;
726  collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide<Cylinder<S>, Ellipsoid<S>, NarrowPhaseSolver>;
727  collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder<S>, Capsule<S>, NarrowPhaseSolver>;
728  collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder<S>, Cone<S>, NarrowPhaseSolver>;
729  collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder<S>, Cylinder<S>, NarrowPhaseSolver>;
730  collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder<S>, Convex<S>, NarrowPhaseSolver>;
731  collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder<S>, Plane<S>, NarrowPhaseSolver>;
732  collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide<Cylinder<S>, Halfspace<S>, NarrowPhaseSolver>;
733 
734  collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex<S>, Box<S>, NarrowPhaseSolver>;
735  collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex<S>, Sphere<S>, NarrowPhaseSolver>;
736  collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide<Convex<S>, Ellipsoid<S>, NarrowPhaseSolver>;
737  collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex<S>, Capsule<S>, NarrowPhaseSolver>;
738  collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex<S>, Cone<S>, NarrowPhaseSolver>;
739  collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex<S>, Cylinder<S>, NarrowPhaseSolver>;
740  collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex<S>, Convex<S>, NarrowPhaseSolver>;
741  collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex<S>, Plane<S>, NarrowPhaseSolver>;
742  collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide<Convex<S>, Halfspace<S>, NarrowPhaseSolver>;
743 
744  collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane<S>, Box<S>, NarrowPhaseSolver>;
745  collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane<S>, Sphere<S>, NarrowPhaseSolver>;
746  collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Plane<S>, Ellipsoid<S>, NarrowPhaseSolver>;
747  collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane<S>, Capsule<S>, NarrowPhaseSolver>;
748  collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane<S>, Cone<S>, NarrowPhaseSolver>;
749  collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane<S>, Cylinder<S>, NarrowPhaseSolver>;
750  collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane<S>, Convex<S>, NarrowPhaseSolver>;
751  collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane<S>, Plane<S>, NarrowPhaseSolver>;
752  collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide<Plane<S>, Halfspace<S>, NarrowPhaseSolver>;
753 
754  collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
755  collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
756  collision_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Halfspace<S>, Ellipsoid<S>, NarrowPhaseSolver>;
757  collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
758  collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
759  collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;
760  collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide<Halfspace<S>, Convex<S>, NarrowPhaseSolver>;
761  collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide<Halfspace<S>, Plane<S>, NarrowPhaseSolver>;
762  collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide<Halfspace<S>, Halfspace<S>, NarrowPhaseSolver>;
763 
764  collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB<S>, Box<S>, NarrowPhaseSolver>::collide;
765  collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB<S>, Sphere<S>, NarrowPhaseSolver>::collide;
766  collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider<AABB<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
767  collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB<S>, Capsule<S>, NarrowPhaseSolver>::collide;
768  collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB<S>, Cone<S>, NarrowPhaseSolver>::collide;
769  collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
770  collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB<S>, Convex<S>, NarrowPhaseSolver>::collide;
771  collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB<S>, Plane<S>, NarrowPhaseSolver>::collide;
772  collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider<AABB<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
773 
774  collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB<S>, Box<S>, NarrowPhaseSolver>::collide;
775  collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB<S>, Sphere<S>, NarrowPhaseSolver>::collide;
776  collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider<OBB<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
777  collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB<S>, Capsule<S>, NarrowPhaseSolver>::collide;
778  collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB<S>, Cone<S>, NarrowPhaseSolver>::collide;
779  collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
780  collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB<S>, Convex<S>, NarrowPhaseSolver>::collide;
781  collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB<S>, Plane<S>, NarrowPhaseSolver>::collide;
782  collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider<OBB<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
783 
784  collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS<S>, Box<S>, NarrowPhaseSolver>::collide;
785  collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
786  collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider<RSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
787  collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
788  collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS<S>, Cone<S>, NarrowPhaseSolver>::collide;
789  collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
790  collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS<S>, Convex<S>, NarrowPhaseSolver>::collide;
791  collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS<S>, Plane<S>, NarrowPhaseSolver>::collide;
792  collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider<RSS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
793 
794  collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 16>, Box<S>, NarrowPhaseSolver>::collide;
795  collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 16>, Sphere<S>, NarrowPhaseSolver>::collide;
796  collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 16>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
797  collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 16>, Capsule<S>, NarrowPhaseSolver>::collide;
798  collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 16>, Cone<S>, NarrowPhaseSolver>::collide;
799  collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 16>, Cylinder<S>, NarrowPhaseSolver>::collide;
800  collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 16>, Convex<S>, NarrowPhaseSolver>::collide;
801  collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 16>, Plane<S>, NarrowPhaseSolver>::collide;
802  collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 16>, Halfspace<S>, NarrowPhaseSolver>::collide;
803 
804  collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 18>, Box<S>, NarrowPhaseSolver>::collide;
805  collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 18>, Sphere<S>, NarrowPhaseSolver>::collide;
806  collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 18>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
807  collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 18>, Capsule<S>, NarrowPhaseSolver>::collide;
808  collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 18>, Cone<S>, NarrowPhaseSolver>::collide;
809  collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 18>, Cylinder<S>, NarrowPhaseSolver>::collide;
810  collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 18>, Convex<S>, NarrowPhaseSolver>::collide;
811  collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 18>, Plane<S>, NarrowPhaseSolver>::collide;
812  collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 18>, Halfspace<S>, NarrowPhaseSolver>::collide;
813 
814  collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<S, 24>, Box<S>, NarrowPhaseSolver>::collide;
815  collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<S, 24>, Sphere<S>, NarrowPhaseSolver>::collide;
816  collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider<KDOP<S, 24>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
817  collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<S, 24>, Capsule<S>, NarrowPhaseSolver>::collide;
818  collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<S, 24>, Cone<S>, NarrowPhaseSolver>::collide;
819  collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<S, 24>, Cylinder<S>, NarrowPhaseSolver>::collide;
820  collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<S, 24>, Convex<S>, NarrowPhaseSolver>::collide;
821  collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<S, 24>, Plane<S>, NarrowPhaseSolver>::collide;
822  collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider<KDOP<S, 24>, Halfspace<S>, NarrowPhaseSolver>::collide;
823 
824  collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS<S>, Box<S>, NarrowPhaseSolver>::collide;
825  collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
826  collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider<kIOS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
827  collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
828  collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS<S>, Cone<S>, NarrowPhaseSolver>::collide;
829  collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
830  collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS<S>, Convex<S>, NarrowPhaseSolver>::collide;
831  collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS<S>, Plane<S>, NarrowPhaseSolver>::collide;
832  collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider<kIOS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
833 
834  collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS<S>, Box<S>, NarrowPhaseSolver>::collide;
835  collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS<S>, Sphere<S>, NarrowPhaseSolver>::collide;
836  collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider<OBBRSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::collide;
837  collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS<S>, Capsule<S>, NarrowPhaseSolver>::collide;
838  collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS<S>, Cone<S>, NarrowPhaseSolver>::collide;
839  collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS<S>, Cylinder<S>, NarrowPhaseSolver>::collide;
840  collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS<S>, Convex<S>, NarrowPhaseSolver>::collide;
841  collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS<S>, Plane<S>, NarrowPhaseSolver>::collide;
842  collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider<OBBRSS<S>, Halfspace<S>, NarrowPhaseSolver>::collide;
843 
844  collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB<S>, NarrowPhaseSolver>;
845  collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB<S>, NarrowPhaseSolver>;
846  collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS<S>, NarrowPhaseSolver>;
847  collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<S, 16>, NarrowPhaseSolver>;
848  collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<S, 18>, NarrowPhaseSolver>;
849  collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<S, 24>, NarrowPhaseSolver>;
850  collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS<S>, NarrowPhaseSolver>;
851  collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS<S>, NarrowPhaseSolver>;
852 
853 #if FCL_HAVE_OCTOMAP
854  collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box<S>, NarrowPhaseSolver>;
855  collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere<S>, NarrowPhaseSolver>;
856  collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide<Ellipsoid<S>, NarrowPhaseSolver>;
857  collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule<S>, NarrowPhaseSolver>;
858  collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone<S>, NarrowPhaseSolver>;
859  collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder<S>, NarrowPhaseSolver>;
860  collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex<S>, NarrowPhaseSolver>;
861  collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane<S>, NarrowPhaseSolver>;
862  collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace<S>, NarrowPhaseSolver>;
863 
864  collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box<S>, NarrowPhaseSolver>;
865  collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere<S>, NarrowPhaseSolver>;
866  collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide<Ellipsoid<S>, NarrowPhaseSolver>;
867  collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule<S>, NarrowPhaseSolver>;
868  collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone<S>, NarrowPhaseSolver>;
869  collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder<S>, NarrowPhaseSolver>;
870  collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex<S>, NarrowPhaseSolver>;
871  collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane<S>, NarrowPhaseSolver>;
872  collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace<S>, NarrowPhaseSolver>;
873 
874  collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
875 
876  collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB<S>, NarrowPhaseSolver>;
877  collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB<S>, NarrowPhaseSolver>;
878  collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS<S>, NarrowPhaseSolver>;
879  collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS<S>, NarrowPhaseSolver>;
880  collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS<S>, NarrowPhaseSolver>;
881  collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<S, 16>, NarrowPhaseSolver>;
882  collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<S, 18>, NarrowPhaseSolver>;
883  collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<S, 24>, NarrowPhaseSolver>;
884 
885  collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB<S>, NarrowPhaseSolver>;
886  collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB<S>, NarrowPhaseSolver>;
887  collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS<S>, NarrowPhaseSolver>;
888  collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS<S>, NarrowPhaseSolver>;
889  collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS<S>, NarrowPhaseSolver>;
890  collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 16>, NarrowPhaseSolver>;
891  collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 18>, NarrowPhaseSolver>;
892  collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<S, 24>, NarrowPhaseSolver>;
893 #endif
894 }
895 
896 } // namespace detail
897 } // namespace fcl
898 
899 #endif
fcl::detail::BVHShapeCollider< OBB< typename Shape::S >, Shape, NarrowPhaseSolver >::S
typename Shape::S S
Definition: collision_func_matrix-inl.h:432
fcl::detail::MeshShapeCollisionTraversalNodeOBBRSS
Definition: mesh_shape_collision_traversal_node.h:194
fcl::detail::CollisionFunctionMatrix::CollisionFunctionMatrix
CollisionFunctionMatrix()
Definition: collision_func_matrix-inl.h:664
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
triangle_p.h
collision_func_matrix.h
utility.h
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
collision_object.h
fcl::detail::ShapeShapeCollide
std::size_t ShapeShapeCollide(const CollisionGeometry< typename Shape1::S > *o1, const Transform3< typename Shape1::S > &tf1, const CollisionGeometry< typename Shape1::S > *o2, const Transform3< typename Shape1::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename Shape1::S > &request, CollisionResult< typename Shape1::S > &result)
Definition: collision_func_matrix-inl.h:279
mesh_continuous_collision_traversal_node.h
fcl::GEOM_OCTREE
@ GEOM_OCTREE
Definition: collision_geometry.h:54
shape_mesh_collision_traversal_node.h
sphere.h
fcl::detail::orientedMeshCollide
std::size_t orientedMeshCollide(const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result)
Definition: collision_func_matrix-inl.h:572
fcl::GEOM_HALFSPACE
@ GEOM_HALFSPACE
Definition: collision_geometry.h:54
shape_bvh_collision_traversal_node.h
fcl::detail::ShapeCollisionTraversalNode
Traversal node for collision between two shapes.
Definition: shape_collision_traversal_node.h:53
fcl::detail::BVHShapeCollider< kIOS< typename Shape::S >, Shape, NarrowPhaseSolver >::S
typename Shape::S S
Definition: collision_func_matrix-inl.h:480
fcl::CollisionGeometry< S_ >::threshold_free
S_ threshold_free
threshold for free (<= is free)
Definition: collision_geometry.h:108
fcl::detail::BVHShapeCollider::collide
static std::size_t collide(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:319
fcl::detail::BVHCollideImpl< S, OBB< S > >::run
static std::size_t run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:596
fcl::Convex< S >
bvh_shape_collision_traversal_node.h
fcl::GEOM_CONE
@ GEOM_CONE
Definition: collision_geometry.h:54
fcl::CollisionGeometry< S_ >::cost_density
S_ cost_density
collision cost for unit volume
Definition: collision_geometry.h:102
mesh_collision_traversal_node.h
fcl::detail::MeshCollisionTraversalNode
Traversal node for collision between two meshes.
Definition: mesh_collision_traversal_node.h:58
unused.h
fcl::GEOM_PLANE
@ GEOM_PLANE
Definition: collision_geometry.h:54
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
fcl::BV_KDOP18
@ BV_KDOP18
Definition: collision_geometry.h:53
fcl::CollisionRequest::num_max_cost_sources
size_t num_max_cost_sources
The maximum number of cost sources that can be returned.
Definition: collision_request.h:69
halfspace.h
fcl::detail::BVHShapeCollider< kIOS< typename Shape::S >, Shape, NarrowPhaseSolver >::collide
static std::size_t collide(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:482
fcl::GEOM_ELLIPSOID
@ GEOM_ELLIPSOID
Definition: collision_geometry.h:54
fcl::detail::BVHCollideImpl::run
static std::size_t run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:528
fcl::detail::BVHShapeCollider< OBBRSS< typename Shape::S >, Shape, NarrowPhaseSolver >::collide
static std::size_t collide(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:506
fcl::CollisionResult
collision result
Definition: collision_request.h:48
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::detail::BVHShapeCollider< OBB< typename Shape::S >, Shape, NarrowPhaseSolver >::collide
static std::size_t collide(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:434
plane.h
mesh_octree_collision_traversal_node.h
fcl::detail::BVHShapeCollider< RSS< typename Shape::S >, Shape, NarrowPhaseSolver >::collide
static std::size_t collide(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:458
box.h
fcl::detail::orientedBVHShapeCollide
std::size_t orientedBVHShapeCollide(const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const NarrowPhaseSolver *nsolver, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result)
Definition: collision_func_matrix-inl.h:378
cone.h
fcl::detail::MeshShapeCollisionTraversalNodeOBB
Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB,...
Definition: mesh_shape_collision_traversal_node.h:110
ellipsoid.h
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::OBB
Oriented bounding box class.
Definition: OBB.h:51
fcl::CollisionRequest::enable_cost
bool enable_cost
If true, the cost sources will be computed.
Definition: collision_request.h:72
fcl::detail::CollisionFunctionMatrix::S
typename NarrowPhaseSolver::S S
Definition: collision_func_matrix.h:56
fcl::BV_KDOP24
@ BV_KDOP24
Definition: collision_geometry.h:53
convex.h
bvh_collision_traversal_node.h
fcl::detail::MeshCollisionTraversalNodeOBBRSS
Definition: mesh_collision_traversal_node.h:213
octree_collision_traversal_node.h
fcl::detail::BVHShapeCollider::S
typename BV::S S
Definition: collision_func_matrix-inl.h:317
fcl::BV_OBB
@ BV_OBB
Definition: collision_geometry.h:53
fcl::CollisionRequest
Parameters for performing collision request.
Definition: collision_request.h:52
fcl::CollisionResult::numContacts
size_t numContacts() const
number of contacts found
Definition: collision_result-inl.h:83
fcl::detail::initialize
template bool initialize(MeshCollisionTraversalNodeOBB< double > &node, const BVHModel< OBB< double >> &model1, const Transform3< double > &tf1, const BVHModel< OBB< double >> &model2, const Transform3< double > &tf2, const CollisionRequest< double > &request, CollisionResult< double > &result)
fcl::BV_KDOP16
@ BV_KDOP16
Definition: collision_geometry.h:53
fcl::NODE_COUNT
@ NODE_COUNT
Definition: collision_geometry.h:54
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d....
Definition: geometry/shape/halfspace.h:59
fcl::CollisionRequest::use_approximate_cost
bool use_approximate_cost
If true, the cost computation is approximated (if computed).
Definition: collision_request.h:75
fcl::detail::collide
template void collide(CollisionTraversalNodeBase< double > *node, BVHFrontList *front_list)
fcl::detail::BVHCollide
std::size_t BVHCollide(const CollisionGeometry< typename BV::S > *o1, const Transform3< typename BV::S > &tf1, const CollisionGeometry< typename BV::S > *o2, const Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result)
Definition: collision_func_matrix-inl.h:558
fcl::GEOM_SPHERE
@ GEOM_SPHERE
Definition: collision_geometry.h:54
fcl::GEOM_CYLINDER
@ GEOM_CYLINDER
Definition: collision_geometry.h:54
fcl::detail::BVHCollideImpl< S, OBBRSS< S > >::run
static std::size_t run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:614
fcl::detail::BVHShapeCollider< RSS< typename Shape::S >, Shape, NarrowPhaseSolver >::S
typename Shape::S S
Definition: collision_func_matrix-inl.h:456
fcl::detail::BVHCollideImpl
Definition: collision_func_matrix-inl.h:526
fcl::detail::MeshShapeCollisionTraversalNodekIOS
Definition: mesh_shape_collision_traversal_node.h:166
fcl::GEOM_CONVEX
@ GEOM_CONVEX
Definition: collision_geometry.h:54
octree_mesh_collision_traversal_node.h
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
shape_collision_traversal_node.h
octree_shape_collision_traversal_node.h
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::Cylinder
Center at zero cylinder.
Definition: cylinder.h:50
fcl::detail::BVHShapeCollider< OBBRSS< typename Shape::S >, Shape, NarrowPhaseSolver >::S
typename Shape::S S
Definition: collision_func_matrix-inl.h:504
fcl::constructBox
template void constructBox(const OBB< double > &bv, Box< double > &box, Transform3< double > &tf)
fcl::CollisionRequest::enable_cached_gjk_guess
bool enable_cached_gjk_guess
If true, uses the provided initial guess for the GJK algorithm.
Definition: collision_request.h:83
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
capsule.h
fcl::CollisionRequest::cached_gjk_guess
Vector3< S > cached_gjk_guess
The initial guess to use in the GJK algorithm.
Definition: collision_request.h:86
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::BV_kIOS
@ BV_kIOS
Definition: collision_geometry.h:53
fcl::detail::BVHCollideImpl< S, kIOS< S > >::run
static std::size_t run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const CollisionRequest< S > &request, CollisionResult< S > &result)
Definition: collision_func_matrix-inl.h:632
fcl::detail::MeshCollisionTraversalNodekIOS
Definition: mesh_collision_traversal_node.h:181
mesh_shape_collision_traversal_node.h
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:50
fcl::CollisionRequest::isSatisfied
bool isSatisfied(const CollisionResult< S > &result) const
Definition: collision_request-inl.h:77
fcl::detail::MeshShapeCollisionTraversalNodeRSS
Definition: mesh_shape_collision_traversal_node.h:138
shape_octree_collision_traversal_node.h
fcl::GEOM_BOX
@ GEOM_BOX
Definition: collision_geometry.h:54
fcl::BV_RSS
@ BV_RSS
Definition: collision_geometry.h:53
fcl::detail::MeshShapeCollisionTraversalNode
Traversal node for collision between mesh and shape.
Definition: mesh_shape_collision_traversal_node.h:52
fcl::detail::BVHShapeCollider
Definition: collision_func_matrix-inl.h:315
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:50
collision_node.h
fcl::Cone
Center at zero cone.
Definition: cone.h:50
fcl::CollisionGeometry< S_ >::threshold_occupied
S_ threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_geometry.h:105
collision_traversal_node_base.h
fcl::CollisionResult::cached_gjk_guess
Vector3< S > cached_gjk_guess
Definition: collision_result.h:62
fcl::BV_OBBRSS
@ BV_OBBRSS
Definition: collision_geometry.h:53
fcl::BV_AABB
@ BV_AABB
Definition: collision_geometry.h:53
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::GEOM_CAPSULE
@ GEOM_CAPSULE
Definition: collision_geometry.h:54
fcl::detail::MeshCollisionTraversalNodeOBB
Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB,...
Definition: mesh_collision_traversal_node.h:99
cylinder.h
fcl::Capsule< S >
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:50


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:37:59