FCL  0.6.0
Flexible Collision Library
distance_func_matrix-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_DISTANCE_FUNC_MATRIX_INL_H
39 #define FCL_DISTANCE_FUNC_MATRIX_INL_H
40 
41 #include "fcl/narrowphase/detail/distance_func_matrix.h"
42 
43 #include "fcl/config.h"
44 
45 #include "fcl/common/types.h"
46 
47 #include "fcl/narrowphase/collision_object.h"
48 
49 #include "fcl/narrowphase/detail/traversal/collision_node.h"
50 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
51 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
52 
53 #include "fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h"
54 #include "fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h"
55 #include "fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h"
56 #include "fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h"
57 #include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h"
58 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h"
59 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h"
60 #include "fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h"
61 #include "fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h"
62 #include "fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h"
63 #include "fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h"
64 #include "fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h"
65 
66 #if FCL_HAVE_OCTOMAP
67 
68 #include "fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h"
69 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h"
70 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h"
71 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h"
72 #include "fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h"
73 
74 #endif // FCL_HAVE_OCTOMAP
75 
76 namespace fcl
77 {
78 
79 namespace detail
80 {
81 
82 //==============================================================================
83 #if FCL_HAVE_OCTOMAP
84 template <typename Shape, typename NarrowPhaseSolver>
85 typename Shape::S ShapeOcTreeDistance(
86  const CollisionGeometry<typename Shape::S>* o1,
87  const Transform3<typename Shape::S>& tf1,
88  const CollisionGeometry<typename Shape::S>* o2,
89  const Transform3<typename Shape::S>& tf2,
90  const NarrowPhaseSolver* nsolver,
91  const DistanceRequest<typename Shape::S>& request,
92  DistanceResult<typename Shape::S>& result)
93 {
94  using S = typename Shape::S;
95 
96  if(request.isSatisfied(result)) return result.min_distance;
97  ShapeOcTreeDistanceTraversalNode<Shape, NarrowPhaseSolver> node;
98  const Shape* obj1 = static_cast<const Shape*>(o1);
99  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
100  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
101 
102  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
103  distance(&node);
104 
105  return result.min_distance;
106 }
107 
108 template <typename Shape, typename NarrowPhaseSolver>
109 typename Shape::S OcTreeShapeDistance(
110  const CollisionGeometry<typename Shape::S>* o1,
111  const Transform3<typename Shape::S>& tf1,
112  const CollisionGeometry<typename Shape::S>* o2,
113  const Transform3<typename Shape::S>& tf2,
114  const NarrowPhaseSolver* nsolver,
115  const DistanceRequest<typename Shape::S>& request,
116  DistanceResult<typename Shape::S>& result)
117 {
118  using S = typename Shape::S;
119 
120  if(request.isSatisfied(result)) return result.min_distance;
121  OcTreeShapeDistanceTraversalNode<Shape, NarrowPhaseSolver> node;
122  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
123  const Shape* obj2 = static_cast<const Shape*>(o2);
124  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
125 
126  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
127  distance(&node);
128 
129  return result.min_distance;
130 }
131 
132 template <typename NarrowPhaseSolver>
133 typename NarrowPhaseSolver::S OcTreeDistance(
134  const CollisionGeometry<typename NarrowPhaseSolver::S>* o1,
135  const Transform3<typename NarrowPhaseSolver::S>& tf1,
136  const CollisionGeometry<typename NarrowPhaseSolver::S>* o2,
137  const Transform3<typename NarrowPhaseSolver::S>& tf2,
138  const NarrowPhaseSolver* nsolver,
139  const DistanceRequest<typename NarrowPhaseSolver::S>& request,
140  DistanceResult<typename NarrowPhaseSolver::S>& result)
141 {
142  using S = typename NarrowPhaseSolver::S;
143 
144  if(request.isSatisfied(result)) return result.min_distance;
145  OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
146  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
147  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
148  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
149 
150  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
151  distance(&node);
152 
153  return result.min_distance;
154 }
155 
156 template <typename BV, typename NarrowPhaseSolver>
157 typename BV::S BVHOcTreeDistance(
158  const CollisionGeometry<typename BV::S>* o1,
159  const Transform3<typename BV::S>& tf1,
160  const CollisionGeometry<typename BV::S>* o2,
161  const Transform3<typename BV::S>& tf2,
162  const NarrowPhaseSolver* nsolver,
163  const DistanceRequest<typename BV::S>& request,
164  DistanceResult<typename BV::S>& result)
165 {
166  using S = typename BV::S;
167 
168  if(request.isSatisfied(result)) return result.min_distance;
169  MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver> node;
170  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
171  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
172  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
173 
174  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
175  distance(&node);
176 
177  return result.min_distance;
178 }
179 
180 template <typename BV, typename NarrowPhaseSolver>
181 typename BV::S OcTreeBVHDistance(
182  const CollisionGeometry<typename BV::S>* o1,
183  const Transform3<typename BV::S>& tf1,
184  const CollisionGeometry<typename BV::S>* o2,
185  const Transform3<typename BV::S>& tf2,
186  const NarrowPhaseSolver* nsolver,
187  const DistanceRequest<typename BV::S>& request,
188  DistanceResult<typename BV::S>& result)
189 {
190  using S = typename BV::S;
191 
192  if(request.isSatisfied(result)) return result.min_distance;
193  OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver> node;
194  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
195  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
196  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
197 
198  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
199  distance(&node);
200 
201  return result.min_distance;
202 }
203 
204 #endif
205 
206 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
207 typename Shape1::S ShapeShapeDistance(
208  const CollisionGeometry<typename Shape1::S>* o1,
209  const Transform3<typename Shape1::S>& tf1,
210  const CollisionGeometry<typename Shape1::S>* o2,
211  const Transform3<typename Shape1::S>& tf2,
212  const NarrowPhaseSolver* nsolver,
213  const DistanceRequest<typename Shape1::S>& request,
214  DistanceResult<typename Shape1::S>& result)
215 {
216  if(request.isSatisfied(result)) return result.min_distance;
217  ShapeDistanceTraversalNode<Shape1, Shape2, NarrowPhaseSolver> node;
218  const Shape1* obj1 = static_cast<const Shape1*>(o1);
219  const Shape2* obj2 = static_cast<const Shape2*>(o2);
220 
221  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
222  distance(&node);
223 
224  return result.min_distance;
225 }
226 
227 template <typename BV, typename Shape, typename NarrowPhaseSolver>
228 struct BVHShapeDistancer
229 {
230  using S = typename BV::S;
231 
232  static S distance(
233  const CollisionGeometry<S>* o1,
234  const Transform3<S>& tf1,
235  const CollisionGeometry<S>* o2,
236  const Transform3<S>& tf2,
237  const NarrowPhaseSolver* nsolver,
238  const DistanceRequest<S>& request,
239  DistanceResult<S>& result)
240  {
241  if(request.isSatisfied(result)) return result.min_distance;
242  MeshShapeDistanceTraversalNode<BV, Shape, NarrowPhaseSolver> node;
243  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
244  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
245  Transform3<S> tf1_tmp = tf1;
246  const Shape* obj2 = static_cast<const Shape*>(o2);
247 
248  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
249  ::fcl::distance(&node);
250 
251  delete obj1_tmp;
252  return result.min_distance;
253  }
254 };
255 
256 template <typename OrientedMeshShapeDistanceTraversalNode,
257  typename BV, typename Shape, typename NarrowPhaseSolver>
258 typename Shape::S orientedBVHShapeDistance(
259  const CollisionGeometry<typename Shape::S>* o1,
260  const Transform3<typename Shape::S>& tf1,
261  const CollisionGeometry<typename Shape::S>* o2,
262  const Transform3<typename Shape::S>& tf2,
263  const NarrowPhaseSolver* nsolver,
264  const DistanceRequest<typename Shape::S>&
265  request, DistanceResult<typename Shape::S>& result)
266 {
267  if(request.isSatisfied(result)) return result.min_distance;
268  OrientedMeshShapeDistanceTraversalNode node;
269  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
270  const Shape* obj2 = static_cast<const Shape*>(o2);
271 
272  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
273  distance(&node);
274 
275  return result.min_distance;
276 }
277 
278 template <typename Shape, typename NarrowPhaseSolver>
279 struct BVHShapeDistancer<RSS<typename Shape::S>, Shape, NarrowPhaseSolver>
280 {
281  static typename Shape::S distance(
282  const CollisionGeometry<typename Shape::S>* o1,
283  const Transform3<typename Shape::S>& tf1,
284  const CollisionGeometry<typename Shape::S>* o2,
285  const Transform3<typename Shape::S>& tf2,
286  const NarrowPhaseSolver* nsolver,
287  const DistanceRequest<typename Shape::S>& request,
288  DistanceResult<typename Shape::S>& result)
289  {
290  return detail::orientedBVHShapeDistance<
291  MeshShapeDistanceTraversalNodeRSS<Shape, NarrowPhaseSolver>,
292  RSS<typename Shape::S>,
293  Shape,
294  NarrowPhaseSolver>(
295  o1, tf1, o2, tf2, nsolver, request, result);
296  }
297 };
298 
299 template <typename Shape, typename NarrowPhaseSolver>
300 struct BVHShapeDistancer<kIOS<typename Shape::S>, Shape, NarrowPhaseSolver>
301 {
302  static typename Shape::S distance(
303  const CollisionGeometry<typename Shape::S>* o1,
304  const Transform3<typename Shape::S>& tf1,
305  const CollisionGeometry<typename Shape::S>* o2,
306  const Transform3<typename Shape::S>& tf2,
307  const NarrowPhaseSolver* nsolver,
308  const DistanceRequest<typename Shape::S>& request,
309  DistanceResult<typename Shape::S>& result)
310  {
311  return detail::orientedBVHShapeDistance<
312  MeshShapeDistanceTraversalNodekIOS<Shape, NarrowPhaseSolver>,
313  kIOS<typename Shape::S>,
314  Shape,
315  NarrowPhaseSolver>(
316  o1, tf1, o2, tf2, nsolver, request, result);
317  }
318 };
319 
320 template <typename Shape, typename NarrowPhaseSolver>
321 struct BVHShapeDistancer<OBBRSS<typename Shape::S>, Shape, NarrowPhaseSolver>
322 {
323  static typename Shape::S distance(
324  const CollisionGeometry<typename Shape::S>* o1,
325  const Transform3<typename Shape::S>& tf1,
326  const CollisionGeometry<typename Shape::S>* o2,
327  const Transform3<typename Shape::S>& tf2,
328  const NarrowPhaseSolver* nsolver,
329  const DistanceRequest<typename Shape::S>& request,
330  DistanceResult<typename Shape::S>& result)
331  {
332  return detail::orientedBVHShapeDistance<
333  MeshShapeDistanceTraversalNodeOBBRSS<Shape, NarrowPhaseSolver>,
334  OBBRSS<typename Shape::S>,
335  Shape,
336  NarrowPhaseSolver>(
337  o1, tf1, o2, tf2, nsolver, request, result);
338  }
339 };
340 
341 //==============================================================================
342 template <typename S, typename BV>
343 struct BVHDistanceImpl
344 {
345  static S run(
346  const CollisionGeometry<S>* o1,
347  const Transform3<S>& tf1,
348  const CollisionGeometry<S>* o2,
349  const Transform3<S>& tf2,
350  const DistanceRequest<S>& request,
351  DistanceResult<S>& result)
352  {
353  if(request.isSatisfied(result)) return result.min_distance;
354  MeshDistanceTraversalNode<BV> node;
355  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
356  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
357  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
358  Transform3<S> tf1_tmp = tf1;
359  BVHModel<BV>* obj2_tmp = new BVHModel<BV>(*obj2);
360  Transform3<S> tf2_tmp = tf2;
361 
362  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
363  distance(&node);
364  delete obj1_tmp;
365  delete obj2_tmp;
366 
367  return result.min_distance;
368  }
369 };
370 
371 //==============================================================================
372 template <typename BV>
373 typename BV::S BVHDistance(
374  const CollisionGeometry<typename BV::S>* o1,
375  const Transform3<typename BV::S>& tf1,
376  const CollisionGeometry<typename BV::S>* o2,
377  const Transform3<typename BV::S>& tf2,
378  const DistanceRequest<typename BV::S>& request,
379  DistanceResult<typename BV::S>& result)
380 {
381  return BVHDistanceImpl<typename BV::S, BV>::run(
382  o1, tf1, o2, tf2, request, result);
383 }
384 
385 template <typename OrientedMeshDistanceTraversalNode, typename BV>
386 typename BV::S orientedMeshDistance(
387  const CollisionGeometry<typename BV::S>* o1,
388  const Transform3<typename BV::S>& tf1,
389  const CollisionGeometry<typename BV::S>* o2,
390  const Transform3<typename BV::S>& tf2,
391  const DistanceRequest<typename BV::S>& request,
392  DistanceResult<typename BV::S>& result)
393 {
394  if(request.isSatisfied(result)) return result.min_distance;
395  OrientedMeshDistanceTraversalNode node;
396  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
397  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
398 
399  initialize(node, *obj1, tf1, *obj2, tf2, request, result);
400  distance(&node);
401 
402  return result.min_distance;
403 }
404 
405 //==============================================================================
406 template <typename S>
407 struct BVHDistanceImpl<S, RSS<S>>
408 {
409  static S run(
410  const CollisionGeometry<S>* o1,
411  const Transform3<S>& tf1,
412  const CollisionGeometry<S>* o2,
413  const Transform3<S>& tf2,
414  const DistanceRequest<S>& request,
415  DistanceResult<S>& result)
416  {
417  return detail::orientedMeshDistance<
418  MeshDistanceTraversalNodeRSS<S>, RSS<S>>(
419  o1, tf1, o2, tf2, request, result);
420  }
421 };
422 
423 //==============================================================================
424 template <typename S>
425 struct BVHDistanceImpl<S, kIOS<S>>
426 {
427  static S run(
428  const CollisionGeometry<S>* o1,
429  const Transform3<S>& tf1,
430  const CollisionGeometry<S>* o2,
431  const Transform3<S>& tf2,
432  const DistanceRequest<S>& request,
433  DistanceResult<S>& result)
434  {
435  return detail::orientedMeshDistance<
436  MeshDistanceTraversalNodekIOS<S>, kIOS<S>>(
437  o1, tf1, o2, tf2, request, result);
438  }
439 };
440 
441 //==============================================================================
442 template <typename S>
443 struct BVHDistanceImpl<S, OBBRSS<S>>
444 {
445  static S run(
446  const CollisionGeometry<S>* o1,
447  const Transform3<S>& tf1,
448  const CollisionGeometry<S>* o2,
449  const Transform3<S>& tf2,
450  const DistanceRequest<S>& request,
451  DistanceResult<S>& result)
452  {
453  return detail::orientedMeshDistance<
454  MeshDistanceTraversalNodeOBBRSS<S>, OBBRSS<S>>(
455  o1, tf1, o2, tf2, request, result);
456  }
457 };
458 
459 //==============================================================================
460 template <typename BV, typename NarrowPhaseSolver>
461 typename BV::S BVHDistance(
462  const CollisionGeometry<typename BV::S>* o1,
463  const Transform3<typename BV::S>& tf1,
464  const CollisionGeometry<typename BV::S>* o2,
465  const Transform3<typename BV::S>& tf2,
466  const NarrowPhaseSolver* nsolver,
467  const DistanceRequest<typename BV::S>& request,
468  DistanceResult<typename BV::S>& result)
469 {
470  return BVHDistance<BV>(o1, tf1, o2, tf2, request, result);
471 }
472 
473 template <typename NarrowPhaseSolver>
474 DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
475 {
476  for(int i = 0; i < NODE_COUNT; ++i)
477  {
478  for(int j = 0; j < NODE_COUNT; ++j)
479  distance_matrix[i][j] = nullptr;
480  }
481 
482  distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box<S>, Box<S>, NarrowPhaseSolver>;
483  distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box<S>, Sphere<S>, NarrowPhaseSolver>;
484  distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Box<S>, Ellipsoid<S>, NarrowPhaseSolver>;
485  distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box<S>, Capsule<S>, NarrowPhaseSolver>;
486  distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box<S>, Cone<S>, NarrowPhaseSolver>;
487  distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box<S>, Cylinder<S>, NarrowPhaseSolver>;
488  distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box<S>, Convex<S>, NarrowPhaseSolver>;
489  distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box<S>, Plane<S>, NarrowPhaseSolver>;
490  distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box<S>, Halfspace<S>, NarrowPhaseSolver>;
491 
492  distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere<S>, Box<S>, NarrowPhaseSolver>;
493  distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere<S>, Sphere<S>, NarrowPhaseSolver>;
494  distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Sphere<S>, Ellipsoid<S>, NarrowPhaseSolver>;
495  distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere<S>, Capsule<S>, NarrowPhaseSolver>;
496  distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere<S>, Cone<S>, NarrowPhaseSolver>;
497  distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere<S>, Cylinder<S>, NarrowPhaseSolver>;
498  distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere<S>, Convex<S>, NarrowPhaseSolver>;
499  distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere<S>, Plane<S>, NarrowPhaseSolver>;
500  distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere<S>, Halfspace<S>, NarrowPhaseSolver>;
501 
502  distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance<Ellipsoid<S>, Box<S>, NarrowPhaseSolver>;
503  distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance<Ellipsoid<S>, Sphere<S>, NarrowPhaseSolver>;
504  distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance<Ellipsoid<S>, Ellipsoid<S>, NarrowPhaseSolver>;
505  distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance<Ellipsoid<S>, Capsule<S>, NarrowPhaseSolver>;
506  distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance<Ellipsoid<S>, Cone<S>, NarrowPhaseSolver>;
507  distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance<Ellipsoid<S>, Cylinder<S>, NarrowPhaseSolver>;
508  distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance<Ellipsoid<S>, Convex<S>, NarrowPhaseSolver>;
509  distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance<Ellipsoid<S>, Plane<S>, NarrowPhaseSolver>;
510  distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance<Ellipsoid<S>, Halfspace<S>, NarrowPhaseSolver>;
511 
512  distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule<S>, Box<S>, NarrowPhaseSolver>;
513  distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule<S>, Sphere<S>, NarrowPhaseSolver>;
514  distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Capsule<S>, Ellipsoid<S>, NarrowPhaseSolver>;
515  distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule<S>, Capsule<S>, NarrowPhaseSolver>;
516  distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule<S>, Cone<S>, NarrowPhaseSolver>;
517  distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule<S>, Cylinder<S>, NarrowPhaseSolver>;
518  distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule<S>, Convex<S>, NarrowPhaseSolver>;
519  distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule<S>, Plane<S>, NarrowPhaseSolver>;
520  distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule<S>, Halfspace<S>, NarrowPhaseSolver>;
521 
522  distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone<S>, Box<S>, NarrowPhaseSolver>;
523  distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone<S>, Sphere<S>, NarrowPhaseSolver>;
524  distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cone<S>, Ellipsoid<S>, NarrowPhaseSolver>;
525  distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone<S>, Capsule<S>, NarrowPhaseSolver>;
526  distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone<S>, Cone<S>, NarrowPhaseSolver>;
527  distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone<S>, Cylinder<S>, NarrowPhaseSolver>;
528  distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone<S>, Convex<S>, NarrowPhaseSolver>;
529  distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone<S>, Plane<S>, NarrowPhaseSolver>;
530  distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone<S>, Halfspace<S>, NarrowPhaseSolver>;
531 
532  distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder<S>, Box<S>, NarrowPhaseSolver>;
533  distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder<S>, Sphere<S>, NarrowPhaseSolver>;
534  distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cylinder<S>, Ellipsoid<S>, NarrowPhaseSolver>;
535  distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder<S>, Capsule<S>, NarrowPhaseSolver>;
536  distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder<S>, Cone<S>, NarrowPhaseSolver>;
537  distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder<S>, Cylinder<S>, NarrowPhaseSolver>;
538  distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder<S>, Convex<S>, NarrowPhaseSolver>;
539  distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder<S>, Plane<S>, NarrowPhaseSolver>;
540  distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder<S>, Halfspace<S>, NarrowPhaseSolver>;
541 
542  distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex<S>, Box<S>, NarrowPhaseSolver>;
543  distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex<S>, Sphere<S>, NarrowPhaseSolver>;
544  distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Convex<S>, Ellipsoid<S>, NarrowPhaseSolver>;
545  distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex<S>, Capsule<S>, NarrowPhaseSolver>;
546  distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex<S>, Cone<S>, NarrowPhaseSolver>;
547  distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex<S>, Cylinder<S>, NarrowPhaseSolver>;
548  distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex<S>, Convex<S>, NarrowPhaseSolver>;
549  distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex<S>, Plane<S>, NarrowPhaseSolver>;
550  distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex<S>, Halfspace<S>, NarrowPhaseSolver>;
551 
552  distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane<S>, Box<S>, NarrowPhaseSolver>;
553  distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane<S>, Sphere<S>, NarrowPhaseSolver>;
554  distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Plane<S>, Ellipsoid<S>, NarrowPhaseSolver>;
555  distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane<S>, Capsule<S>, NarrowPhaseSolver>;
556  distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane<S>, Cone<S>, NarrowPhaseSolver>;
557  distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane<S>, Cylinder<S>, NarrowPhaseSolver>;
558  distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane<S>, Convex<S>, NarrowPhaseSolver>;
559  distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane<S>, Plane<S>, NarrowPhaseSolver>;
560  distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane<S>, Halfspace<S>, NarrowPhaseSolver>;
561 
562  distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
563  distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
564  distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
565  distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
566  distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;
567  distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace<S>, Convex<S>, NarrowPhaseSolver>;
568  distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace<S>, Plane<S>, NarrowPhaseSolver>;
569  distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace<S>, Halfspace<S>, NarrowPhaseSolver>;
570 
571  /* AABB distance not implemented */
572  /*
573  distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB<S>, Box<S>, NarrowPhaseSolver>::distance;
574  distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB<S>, Sphere<S>, NarrowPhaseSolver>::distance;
575  distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer<AABB<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
576  distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB<S>, Capsule<S>, NarrowPhaseSolver>::distance;
577  distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB<S>, Cone<S>, NarrowPhaseSolver>::distance;
578  distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
579  distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB<S>, Convex<S>, NarrowPhaseSolver>::distance;
580  distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB<S>, Plane<S>, NarrowPhaseSolver>::distance;
581  distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
582 
583  distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB<S>, Box<S>, NarrowPhaseSolver>::distance;
584  distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB<S>, Sphere<S>, NarrowPhaseSolver>::distance;
585  distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBB<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
586  distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB<S>, Capsule<S>, NarrowPhaseSolver>::distance;
587  distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB<S>, Cone<S>, NarrowPhaseSolver>::distance;
588  distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
589  distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB<S>, Convex<S>, NarrowPhaseSolver>::distance;
590  distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB<S>, Plane<S>, NarrowPhaseSolver>::distance;
591  distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
592  */
593 
594  distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS<S>, Box<S>, NarrowPhaseSolver>::distance;
595  distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
596  distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<RSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
597  distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
598  distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS<S>, Cone<S>, NarrowPhaseSolver>::distance;
599  distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
600  distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS<S>, Convex<S>, NarrowPhaseSolver>::distance;
601  distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS<S>, Plane<S>, NarrowPhaseSolver>::distance;
602  distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
603 
604  /* KDOPd distance not implemented */
605  /*
606  distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 16>, Box<S>, NarrowPhaseSolver>::distance;
607  distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 16>, Sphere<S>, NarrowPhaseSolver>::distance;
608  distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 16>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
609  distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 16>, Capsule<S>, NarrowPhaseSolver>::distance;
610  distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 16>, Cone<S>, NarrowPhaseSolver>::distance;
611  distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 16>, Cylinder<S>, NarrowPhaseSolver>::distance;
612  distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 16>, Convex<S>, NarrowPhaseSolver>::distance;
613  distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 16>, Plane<S>, NarrowPhaseSolver>::distance;
614  distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 16>, Halfspace<S>, NarrowPhaseSolver>::distance;
615 
616  distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 18>, Box<S>, NarrowPhaseSolver>::distance;
617  distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 18>, Sphere<S>, NarrowPhaseSolver>::distance;
618  distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 18>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
619  distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 18>, Capsule<S>, NarrowPhaseSolver>::distance;
620  distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 18>, Cone<S>, NarrowPhaseSolver>::distance;
621  distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 18>, Cylinder<S>, NarrowPhaseSolver>::distance;
622  distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 18>, Convex<S>, NarrowPhaseSolver>::distance;
623  distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 18>, Plane<S>, NarrowPhaseSolver>::distance;
624  distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 18>, Halfspace<S>, NarrowPhaseSolver>::distance;
625 
626  distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 24>, Box<S>, NarrowPhaseSolver>::distance;
627  distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 24>, Sphere<S>, NarrowPhaseSolver>::distance;
628  distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 24>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
629  distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 24>, Capsule<S>, NarrowPhaseSolver>::distance;
630  distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 24>, Cone<S>, NarrowPhaseSolver>::distance;
631  distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 24>, Cylinder<S>, NarrowPhaseSolver>::distance;
632  distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 24>, Convex<S>, NarrowPhaseSolver>::distance;
633  distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 24>, Plane<S>, NarrowPhaseSolver>::distance;
634  distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 24>, Halfspace<S>, NarrowPhaseSolver>::distance;
635  */
636 
637  distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS<S>, Box<S>, NarrowPhaseSolver>::distance;
638  distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
639  distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer<kIOS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
640  distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
641  distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS<S>, Cone<S>, NarrowPhaseSolver>::distance;
642  distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
643  distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS<S>, Convex<S>, NarrowPhaseSolver>::distance;
644  distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS<S>, Plane<S>, NarrowPhaseSolver>::distance;
645  distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
646 
647  distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS<S>, Box<S>, NarrowPhaseSolver>::distance;
648  distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
649  distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBBRSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
650  distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
651  distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS<S>, Cone<S>, NarrowPhaseSolver>::distance;
652  distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
653  distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS<S>, Convex<S>, NarrowPhaseSolver>::distance;
654  distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS<S>, Plane<S>, NarrowPhaseSolver>::distance;
655  distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
656 
657  distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB<S>, NarrowPhaseSolver>;
658  distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS<S>, NarrowPhaseSolver>;
659  distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS<S>, NarrowPhaseSolver>;
660  distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS<S>, NarrowPhaseSolver>;
661 
662 #if FCL_HAVE_OCTOMAP
663  distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box<S>, NarrowPhaseSolver>;
664  distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere<S>, NarrowPhaseSolver>;
665  distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance<Ellipsoid<S>, NarrowPhaseSolver>;
666  distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule<S>, NarrowPhaseSolver>;
667  distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone<S>, NarrowPhaseSolver>;
668  distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder<S>, NarrowPhaseSolver>;
669  distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex<S>, NarrowPhaseSolver>;
670  distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane<S>, NarrowPhaseSolver>;
671  distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace<S>, NarrowPhaseSolver>;
672 
673  distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box<S>, NarrowPhaseSolver>;
674  distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere<S>, NarrowPhaseSolver>;
675  distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance<Ellipsoid<S>, NarrowPhaseSolver>;
676  distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule<S>, NarrowPhaseSolver>;
677  distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone<S>, NarrowPhaseSolver>;
678  distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder<S>, NarrowPhaseSolver>;
679  distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex<S>, NarrowPhaseSolver>;
680  distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane<S>, NarrowPhaseSolver>;
681  distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace<S>, NarrowPhaseSolver>;
682 
683  distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
684 
685  distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB<S>, NarrowPhaseSolver>;
686  distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB<S>, NarrowPhaseSolver>;
687  distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS<S>, NarrowPhaseSolver>;
688  distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS<S>, NarrowPhaseSolver>;
689  distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS<S>, NarrowPhaseSolver>;
690  distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<S, 16>, NarrowPhaseSolver>;
691  distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<S, 18>, NarrowPhaseSolver>;
692  distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<S, 24>, NarrowPhaseSolver>;
693 
694  distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB<S>, NarrowPhaseSolver>;
695  distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB<S>, NarrowPhaseSolver>;
696  distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS<S>, NarrowPhaseSolver>;
697  distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS<S>, NarrowPhaseSolver>;
698  distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS<S>, NarrowPhaseSolver>;
699  distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 16>, NarrowPhaseSolver>;
700  distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 18>, NarrowPhaseSolver>;
701  distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 24>, NarrowPhaseSolver>;
702 #endif
703 
704 }
705 
706 } // namespace detail
707 } // namespace fcl
708 
709 #endif
fcl::DistanceResult::min_distance
S min_distance
minimum distance between two objects. if two objects are in collision, min_distance <= 0...
Definition: distance_result.h:56
fcl::Halfspace
Half Space: this is equivalent to the Planed in ODE. The separation plane is defined as n * x = d; Po...
Definition: halfspace.h:55
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::DistanceFunctionMatrix
distance matrix stores the functions for distance between different types of objects and provides a u...
Definition: distance_func_matrix.h:54
fcl::RSS
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
fcl::detail::MeshShapeDistanceTraversalNode
Traversal node for distance between mesh and shape.
Definition: mesh_shape_distance_traversal_node.h:52
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:48
fcl::detail::MeshDistanceTraversalNodekIOS
Definition: mesh_distance_traversal_node.h:135
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:59
fcl::detail::BVHShapeDistancer
Definition: distance_func_matrix-inl.h:228
fcl::detail::BVHDistanceImpl
Definition: distance_func_matrix-inl.h:343
fcl::Capsule
Center at zero point capsule.
Definition: capsule.h:48
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:48
fcl::detail::MeshShapeDistanceTraversalNodeRSS
Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS...
Definition: mesh_shape_distance_traversal_node.h:121
fcl::detail::MeshShapeDistanceTraversalNodekIOS
Definition: mesh_shape_distance_traversal_node.h:151
fcl::detail::MeshShapeDistanceTraversalNodeOBBRSS
Definition: mesh_shape_distance_traversal_node.h:180
fcl::Plane
Infinite plane.
Definition: plane.h:48
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:48
fcl::Cone
Center at zero cone.
Definition: cone.h:48
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::detail::MeshDistanceTraversalNodeRSS
Traversal node for distance computation between two meshes if their underlying BVH node is oriented n...
Definition: mesh_distance_traversal_node.h:95
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:48
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
fcl::detail::MeshDistanceTraversalNode
Traversal node for distance computation between two meshes.
Definition: mesh_distance_traversal_node.h:55
fcl::detail::MeshDistanceTraversalNodeOBBRSS
Definition: mesh_distance_traversal_node.h:175