distance_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_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 #include "fcl/common/unused.h"
47 
48 #include "fcl/narrowphase/collision_object.h"
49 
50 #include "fcl/narrowphase/detail/traversal/collision_node.h"
51 #include "fcl/narrowphase/detail/gjk_solver_indep.h"
52 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
53 
54 #include "fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h"
55 #include "fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h"
56 #include "fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h"
57 #include "fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h"
58 #include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h"
59 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h"
60 #include "fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h"
61 #include "fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h"
62 #include "fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h"
63 #include "fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h"
64 #include "fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h"
65 #include "fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h"
66 
67 #if FCL_HAVE_OCTOMAP
68 
69 #include "fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h"
70 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h"
71 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h"
72 #include "fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h"
73 #include "fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h"
74 
75 #endif // FCL_HAVE_OCTOMAP
76 
77 namespace fcl
78 {
79 
80 namespace detail
81 {
82 
83 //==============================================================================
84 #if FCL_HAVE_OCTOMAP
85 template <typename Shape, typename NarrowPhaseSolver>
86 typename Shape::S ShapeOcTreeDistance(
87  const CollisionGeometry<typename Shape::S>* o1,
88  const Transform3<typename Shape::S>& tf1,
89  const CollisionGeometry<typename Shape::S>* o2,
90  const Transform3<typename Shape::S>& tf2,
91  const NarrowPhaseSolver* nsolver,
92  const DistanceRequest<typename Shape::S>& request,
93  DistanceResult<typename Shape::S>& result)
94 {
95  using S = typename Shape::S;
96 
97  if(request.isSatisfied(result)) return result.min_distance;
98  ShapeOcTreeDistanceTraversalNode<Shape, NarrowPhaseSolver> node;
99  const Shape* obj1 = static_cast<const Shape*>(o1);
100  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
101  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
102 
103  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
104  distance(&node);
105 
106  return result.min_distance;
107 }
108 
109 template <typename Shape, typename NarrowPhaseSolver>
110 typename Shape::S OcTreeShapeDistance(
111  const CollisionGeometry<typename Shape::S>* o1,
112  const Transform3<typename Shape::S>& tf1,
113  const CollisionGeometry<typename Shape::S>* o2,
114  const Transform3<typename Shape::S>& tf2,
115  const NarrowPhaseSolver* nsolver,
116  const DistanceRequest<typename Shape::S>& request,
117  DistanceResult<typename Shape::S>& result)
118 {
119  using S = typename Shape::S;
120 
121  if(request.isSatisfied(result)) return result.min_distance;
122  OcTreeShapeDistanceTraversalNode<Shape, NarrowPhaseSolver> node;
123  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
124  const Shape* obj2 = static_cast<const Shape*>(o2);
125  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
126 
127  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
128  distance(&node);
129 
130  return result.min_distance;
131 }
132 
133 template <typename NarrowPhaseSolver>
134 typename NarrowPhaseSolver::S OcTreeDistance(
135  const CollisionGeometry<typename NarrowPhaseSolver::S>* o1,
136  const Transform3<typename NarrowPhaseSolver::S>& tf1,
137  const CollisionGeometry<typename NarrowPhaseSolver::S>* o2,
138  const Transform3<typename NarrowPhaseSolver::S>& tf2,
139  const NarrowPhaseSolver* nsolver,
140  const DistanceRequest<typename NarrowPhaseSolver::S>& request,
141  DistanceResult<typename NarrowPhaseSolver::S>& result)
142 {
143  using S = typename NarrowPhaseSolver::S;
144 
145  if(request.isSatisfied(result)) return result.min_distance;
146  OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
147  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
148  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
149  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
150 
151  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
152  distance(&node);
153 
154  return result.min_distance;
155 }
156 
157 template <typename BV, typename NarrowPhaseSolver>
158 typename BV::S BVHOcTreeDistance(
159  const CollisionGeometry<typename BV::S>* o1,
160  const Transform3<typename BV::S>& tf1,
161  const CollisionGeometry<typename BV::S>* o2,
162  const Transform3<typename BV::S>& tf2,
163  const NarrowPhaseSolver* nsolver,
164  const DistanceRequest<typename BV::S>& request,
165  DistanceResult<typename BV::S>& result)
166 {
167  using S = typename BV::S;
168 
169  if(request.isSatisfied(result)) return result.min_distance;
170  MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver> node;
171  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>*>(o1);
172  const OcTree<S>* obj2 = static_cast<const OcTree<S>*>(o2);
173  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
174 
175  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
176  distance(&node);
177 
178  return result.min_distance;
179 }
180 
181 template <typename BV, typename NarrowPhaseSolver>
182 typename BV::S OcTreeBVHDistance(
183  const CollisionGeometry<typename BV::S>* o1,
184  const Transform3<typename BV::S>& tf1,
185  const CollisionGeometry<typename BV::S>* o2,
186  const Transform3<typename BV::S>& tf2,
187  const NarrowPhaseSolver* nsolver,
188  const DistanceRequest<typename BV::S>& request,
189  DistanceResult<typename BV::S>& result)
190 {
191  using S = typename BV::S;
192 
193  if(request.isSatisfied(result)) return result.min_distance;
194  OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver> node;
195  const OcTree<S>* obj1 = static_cast<const OcTree<S>*>(o1);
196  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>*>(o2);
197  OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
198 
199  initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
200  distance(&node);
201 
202  return result.min_distance;
203 }
204 
205 #endif
206 
207 template <typename Shape1, typename Shape2, typename NarrowPhaseSolver>
208 typename Shape1::S ShapeShapeDistance(
209  const CollisionGeometry<typename Shape1::S>* o1,
210  const Transform3<typename Shape1::S>& tf1,
211  const CollisionGeometry<typename Shape1::S>* o2,
212  const Transform3<typename Shape1::S>& tf2,
213  const NarrowPhaseSolver* nsolver,
214  const DistanceRequest<typename Shape1::S>& request,
215  DistanceResult<typename Shape1::S>& result)
216 {
217  if(request.isSatisfied(result)) return result.min_distance;
218  ShapeDistanceTraversalNode<Shape1, Shape2, NarrowPhaseSolver> node;
219  const Shape1* obj1 = static_cast<const Shape1*>(o1);
220  const Shape2* obj2 = static_cast<const Shape2*>(o2);
221 
222  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
223  distance(&node);
224 
225  return result.min_distance;
226 }
227 
228 template <typename BV, typename Shape, typename NarrowPhaseSolver>
229 struct BVHShapeDistancer
230 {
231  using S = typename BV::S;
232 
233  static S distance(
234  const CollisionGeometry<S>* o1,
235  const Transform3<S>& tf1,
236  const CollisionGeometry<S>* o2,
237  const Transform3<S>& tf2,
238  const NarrowPhaseSolver* nsolver,
239  const DistanceRequest<S>& request,
240  DistanceResult<S>& result)
241  {
242  if(request.isSatisfied(result)) return result.min_distance;
243  MeshShapeDistanceTraversalNode<BV, Shape, NarrowPhaseSolver> node;
244  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
245  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
246  Transform3<S> tf1_tmp = tf1;
247  const Shape* obj2 = static_cast<const Shape*>(o2);
248 
249  initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
250  ::fcl::distance(&node);
251 
252  delete obj1_tmp;
253  return result.min_distance;
254  }
255 };
256 
257 template <typename OrientedMeshShapeDistanceTraversalNode,
258  typename BV, typename Shape, typename NarrowPhaseSolver>
259 typename Shape::S orientedBVHShapeDistance(
260  const CollisionGeometry<typename Shape::S>* o1,
261  const Transform3<typename Shape::S>& tf1,
262  const CollisionGeometry<typename Shape::S>* o2,
263  const Transform3<typename Shape::S>& tf2,
264  const NarrowPhaseSolver* nsolver,
265  const DistanceRequest<typename Shape::S>&
266  request, DistanceResult<typename Shape::S>& result)
267 {
268  if(request.isSatisfied(result)) return result.min_distance;
269  OrientedMeshShapeDistanceTraversalNode node;
270  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
271  const Shape* obj2 = static_cast<const Shape*>(o2);
272 
273  initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
274  distance(&node);
275 
276  return result.min_distance;
277 }
278 
279 template <typename Shape, typename NarrowPhaseSolver>
280 struct BVHShapeDistancer<RSS<typename Shape::S>, Shape, NarrowPhaseSolver>
281 {
282  static typename Shape::S distance(
283  const CollisionGeometry<typename Shape::S>* o1,
284  const Transform3<typename Shape::S>& tf1,
285  const CollisionGeometry<typename Shape::S>* o2,
286  const Transform3<typename Shape::S>& tf2,
287  const NarrowPhaseSolver* nsolver,
288  const DistanceRequest<typename Shape::S>& request,
289  DistanceResult<typename Shape::S>& result)
290  {
291  return detail::orientedBVHShapeDistance<
292  MeshShapeDistanceTraversalNodeRSS<Shape, NarrowPhaseSolver>,
293  RSS<typename Shape::S>,
294  Shape,
295  NarrowPhaseSolver>(
296  o1, tf1, o2, tf2, nsolver, request, result);
297  }
298 };
299 
300 template <typename Shape, typename NarrowPhaseSolver>
301 struct BVHShapeDistancer<kIOS<typename Shape::S>, Shape, NarrowPhaseSolver>
302 {
303  static typename Shape::S distance(
304  const CollisionGeometry<typename Shape::S>* o1,
305  const Transform3<typename Shape::S>& tf1,
306  const CollisionGeometry<typename Shape::S>* o2,
307  const Transform3<typename Shape::S>& tf2,
308  const NarrowPhaseSolver* nsolver,
309  const DistanceRequest<typename Shape::S>& request,
310  DistanceResult<typename Shape::S>& result)
311  {
312  return detail::orientedBVHShapeDistance<
313  MeshShapeDistanceTraversalNodekIOS<Shape, NarrowPhaseSolver>,
314  kIOS<typename Shape::S>,
315  Shape,
316  NarrowPhaseSolver>(
317  o1, tf1, o2, tf2, nsolver, request, result);
318  }
319 };
320 
321 template <typename Shape, typename NarrowPhaseSolver>
322 struct BVHShapeDistancer<OBBRSS<typename Shape::S>, Shape, NarrowPhaseSolver>
323 {
324  static typename Shape::S distance(
325  const CollisionGeometry<typename Shape::S>* o1,
326  const Transform3<typename Shape::S>& tf1,
327  const CollisionGeometry<typename Shape::S>* o2,
328  const Transform3<typename Shape::S>& tf2,
329  const NarrowPhaseSolver* nsolver,
330  const DistanceRequest<typename Shape::S>& request,
331  DistanceResult<typename Shape::S>& result)
332  {
333  return detail::orientedBVHShapeDistance<
334  MeshShapeDistanceTraversalNodeOBBRSS<Shape, NarrowPhaseSolver>,
335  OBBRSS<typename Shape::S>,
336  Shape,
337  NarrowPhaseSolver>(
338  o1, tf1, o2, tf2, nsolver, request, result);
339  }
340 };
341 
342 //==============================================================================
343 template <typename S, typename BV>
344 struct BVHDistanceImpl
345 {
346  static S run(
347  const CollisionGeometry<S>* o1,
348  const Transform3<S>& tf1,
349  const CollisionGeometry<S>* o2,
350  const Transform3<S>& tf2,
351  const DistanceRequest<S>& request,
352  DistanceResult<S>& result)
353  {
354  if(request.isSatisfied(result)) return result.min_distance;
355  MeshDistanceTraversalNode<BV> node;
356  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
357  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
358  BVHModel<BV>* obj1_tmp = new BVHModel<BV>(*obj1);
359  Transform3<S> tf1_tmp = tf1;
360  BVHModel<BV>* obj2_tmp = new BVHModel<BV>(*obj2);
361  Transform3<S> tf2_tmp = tf2;
362 
363  initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
364  distance(&node);
365  delete obj1_tmp;
366  delete obj2_tmp;
367 
368  return result.min_distance;
369  }
370 };
371 
372 //==============================================================================
373 template <typename BV>
374 typename BV::S BVHDistance(
375  const CollisionGeometry<typename BV::S>* o1,
376  const Transform3<typename BV::S>& tf1,
377  const CollisionGeometry<typename BV::S>* o2,
378  const Transform3<typename BV::S>& tf2,
379  const DistanceRequest<typename BV::S>& request,
380  DistanceResult<typename BV::S>& result)
381 {
382  return BVHDistanceImpl<typename BV::S, BV>::run(
383  o1, tf1, o2, tf2, request, result);
384 }
385 
386 template <typename OrientedMeshDistanceTraversalNode, typename BV>
387 typename BV::S orientedMeshDistance(
388  const CollisionGeometry<typename BV::S>* o1,
389  const Transform3<typename BV::S>& tf1,
390  const CollisionGeometry<typename BV::S>* o2,
391  const Transform3<typename BV::S>& tf2,
392  const DistanceRequest<typename BV::S>& request,
393  DistanceResult<typename BV::S>& result)
394 {
395  if(request.isSatisfied(result)) return result.min_distance;
396  OrientedMeshDistanceTraversalNode node;
397  const BVHModel<BV>* obj1 = static_cast<const BVHModel<BV>* >(o1);
398  const BVHModel<BV>* obj2 = static_cast<const BVHModel<BV>* >(o2);
399 
400  initialize(node, *obj1, tf1, *obj2, tf2, request, result);
401  distance(&node);
402 
403  return result.min_distance;
404 }
405 
406 //==============================================================================
407 template <typename S>
408 struct BVHDistanceImpl<S, RSS<S>>
409 {
410  static S run(
411  const CollisionGeometry<S>* o1,
412  const Transform3<S>& tf1,
413  const CollisionGeometry<S>* o2,
414  const Transform3<S>& tf2,
415  const DistanceRequest<S>& request,
416  DistanceResult<S>& result)
417  {
418  return detail::orientedMeshDistance<
419  MeshDistanceTraversalNodeRSS<S>, RSS<S>>(
420  o1, tf1, o2, tf2, request, result);
421  }
422 };
423 
424 //==============================================================================
425 template <typename S>
426 struct BVHDistanceImpl<S, kIOS<S>>
427 {
428  static S run(
429  const CollisionGeometry<S>* o1,
430  const Transform3<S>& tf1,
431  const CollisionGeometry<S>* o2,
432  const Transform3<S>& tf2,
433  const DistanceRequest<S>& request,
434  DistanceResult<S>& result)
435  {
436  return detail::orientedMeshDistance<
437  MeshDistanceTraversalNodekIOS<S>, kIOS<S>>(
438  o1, tf1, o2, tf2, request, result);
439  }
440 };
441 
442 //==============================================================================
443 template <typename S>
444 struct BVHDistanceImpl<S, OBBRSS<S>>
445 {
446  static S run(
447  const CollisionGeometry<S>* o1,
448  const Transform3<S>& tf1,
449  const CollisionGeometry<S>* o2,
450  const Transform3<S>& tf2,
451  const DistanceRequest<S>& request,
452  DistanceResult<S>& result)
453  {
454  return detail::orientedMeshDistance<
455  MeshDistanceTraversalNodeOBBRSS<S>, OBBRSS<S>>(
456  o1, tf1, o2, tf2, request, result);
457  }
458 };
459 
460 //==============================================================================
461 template <typename BV, typename NarrowPhaseSolver>
462 typename BV::S BVHDistance(
463  const CollisionGeometry<typename BV::S>* o1,
464  const Transform3<typename BV::S>& tf1,
465  const CollisionGeometry<typename BV::S>* o2,
466  const Transform3<typename BV::S>& tf2,
467  const NarrowPhaseSolver* nsolver,
468  const DistanceRequest<typename BV::S>& request,
469  DistanceResult<typename BV::S>& result)
470 {
471  FCL_UNUSED(nsolver);
472 
473  return BVHDistance<BV>(o1, tf1, o2, tf2, request, result);
474 }
475 
476 template <typename NarrowPhaseSolver>
477 DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
478 {
479  for(int i = 0; i < NODE_COUNT; ++i)
480  {
481  for(int j = 0; j < NODE_COUNT; ++j)
482  distance_matrix[i][j] = nullptr;
483  }
484 
485  distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box<S>, Box<S>, NarrowPhaseSolver>;
486  distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box<S>, Sphere<S>, NarrowPhaseSolver>;
487  distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Box<S>, Ellipsoid<S>, NarrowPhaseSolver>;
488  distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box<S>, Capsule<S>, NarrowPhaseSolver>;
489  distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box<S>, Cone<S>, NarrowPhaseSolver>;
490  distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box<S>, Cylinder<S>, NarrowPhaseSolver>;
491  distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box<S>, Convex<S>, NarrowPhaseSolver>;
492  distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box<S>, Plane<S>, NarrowPhaseSolver>;
493  distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance<Box<S>, Halfspace<S>, NarrowPhaseSolver>;
494 
495  distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere<S>, Box<S>, NarrowPhaseSolver>;
496  distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere<S>, Sphere<S>, NarrowPhaseSolver>;
497  distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Sphere<S>, Ellipsoid<S>, NarrowPhaseSolver>;
498  distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere<S>, Capsule<S>, NarrowPhaseSolver>;
499  distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere<S>, Cone<S>, NarrowPhaseSolver>;
500  distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere<S>, Cylinder<S>, NarrowPhaseSolver>;
501  distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere<S>, Convex<S>, NarrowPhaseSolver>;
502  distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere<S>, Plane<S>, NarrowPhaseSolver>;
503  distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance<Sphere<S>, Halfspace<S>, NarrowPhaseSolver>;
504 
505  distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance<Ellipsoid<S>, Box<S>, NarrowPhaseSolver>;
506  distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance<Ellipsoid<S>, Sphere<S>, NarrowPhaseSolver>;
507  distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance<Ellipsoid<S>, Ellipsoid<S>, NarrowPhaseSolver>;
508  distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance<Ellipsoid<S>, Capsule<S>, NarrowPhaseSolver>;
509  distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance<Ellipsoid<S>, Cone<S>, NarrowPhaseSolver>;
510  distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance<Ellipsoid<S>, Cylinder<S>, NarrowPhaseSolver>;
511  distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance<Ellipsoid<S>, Convex<S>, NarrowPhaseSolver>;
512  distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance<Ellipsoid<S>, Plane<S>, NarrowPhaseSolver>;
513  distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance<Ellipsoid<S>, Halfspace<S>, NarrowPhaseSolver>;
514 
515  distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule<S>, Box<S>, NarrowPhaseSolver>;
516  distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule<S>, Sphere<S>, NarrowPhaseSolver>;
517  distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Capsule<S>, Ellipsoid<S>, NarrowPhaseSolver>;
518  distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule<S>, Capsule<S>, NarrowPhaseSolver>;
519  distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule<S>, Cone<S>, NarrowPhaseSolver>;
520  distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule<S>, Cylinder<S>, NarrowPhaseSolver>;
521  distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule<S>, Convex<S>, NarrowPhaseSolver>;
522  distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule<S>, Plane<S>, NarrowPhaseSolver>;
523  distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance<Capsule<S>, Halfspace<S>, NarrowPhaseSolver>;
524 
525  distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone<S>, Box<S>, NarrowPhaseSolver>;
526  distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone<S>, Sphere<S>, NarrowPhaseSolver>;
527  distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cone<S>, Ellipsoid<S>, NarrowPhaseSolver>;
528  distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone<S>, Capsule<S>, NarrowPhaseSolver>;
529  distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone<S>, Cone<S>, NarrowPhaseSolver>;
530  distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone<S>, Cylinder<S>, NarrowPhaseSolver>;
531  distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone<S>, Convex<S>, NarrowPhaseSolver>;
532  distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone<S>, Plane<S>, NarrowPhaseSolver>;
533  distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance<Cone<S>, Halfspace<S>, NarrowPhaseSolver>;
534 
535  distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder<S>, Box<S>, NarrowPhaseSolver>;
536  distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder<S>, Sphere<S>, NarrowPhaseSolver>;
537  distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance<Cylinder<S>, Ellipsoid<S>, NarrowPhaseSolver>;
538  distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder<S>, Capsule<S>, NarrowPhaseSolver>;
539  distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder<S>, Cone<S>, NarrowPhaseSolver>;
540  distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder<S>, Cylinder<S>, NarrowPhaseSolver>;
541  distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder<S>, Convex<S>, NarrowPhaseSolver>;
542  distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder<S>, Plane<S>, NarrowPhaseSolver>;
543  distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance<Cylinder<S>, Halfspace<S>, NarrowPhaseSolver>;
544 
545  distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex<S>, Box<S>, NarrowPhaseSolver>;
546  distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex<S>, Sphere<S>, NarrowPhaseSolver>;
547  distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance<Convex<S>, Ellipsoid<S>, NarrowPhaseSolver>;
548  distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex<S>, Capsule<S>, NarrowPhaseSolver>;
549  distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex<S>, Cone<S>, NarrowPhaseSolver>;
550  distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex<S>, Cylinder<S>, NarrowPhaseSolver>;
551  distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex<S>, Convex<S>, NarrowPhaseSolver>;
552  distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex<S>, Plane<S>, NarrowPhaseSolver>;
553  distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance<Convex<S>, Halfspace<S>, NarrowPhaseSolver>;
554 
555  distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane<S>, Box<S>, NarrowPhaseSolver>;
556  distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane<S>, Sphere<S>, NarrowPhaseSolver>;
557  distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance<Plane<S>, Ellipsoid<S>, NarrowPhaseSolver>;
558  distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane<S>, Capsule<S>, NarrowPhaseSolver>;
559  distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane<S>, Cone<S>, NarrowPhaseSolver>;
560  distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane<S>, Cylinder<S>, NarrowPhaseSolver>;
561  distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane<S>, Convex<S>, NarrowPhaseSolver>;
562  distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane<S>, Plane<S>, NarrowPhaseSolver>;
563  distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance<Plane<S>, Halfspace<S>, NarrowPhaseSolver>;
564 
565  distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
566  distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
567  distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
568  distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
569  distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;
570  distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance<Halfspace<S>, Convex<S>, NarrowPhaseSolver>;
571  distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance<Halfspace<S>, Plane<S>, NarrowPhaseSolver>;
572  distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance<Halfspace<S>, Halfspace<S>, NarrowPhaseSolver>;
573 
574  /* AABB distance not implemented */
575  /*
576  distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB<S>, Box<S>, NarrowPhaseSolver>::distance;
577  distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB<S>, Sphere<S>, NarrowPhaseSolver>::distance;
578  distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer<AABB<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
579  distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB<S>, Capsule<S>, NarrowPhaseSolver>::distance;
580  distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB<S>, Cone<S>, NarrowPhaseSolver>::distance;
581  distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
582  distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB<S>, Convex<S>, NarrowPhaseSolver>::distance;
583  distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB<S>, Plane<S>, NarrowPhaseSolver>::distance;
584  distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
585 
586  distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB<S>, Box<S>, NarrowPhaseSolver>::distance;
587  distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB<S>, Sphere<S>, NarrowPhaseSolver>::distance;
588  distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBB<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
589  distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB<S>, Capsule<S>, NarrowPhaseSolver>::distance;
590  distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB<S>, Cone<S>, NarrowPhaseSolver>::distance;
591  distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
592  distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB<S>, Convex<S>, NarrowPhaseSolver>::distance;
593  distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB<S>, Plane<S>, NarrowPhaseSolver>::distance;
594  distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
595  */
596 
597  distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS<S>, Box<S>, NarrowPhaseSolver>::distance;
598  distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
599  distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<RSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
600  distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
601  distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS<S>, Cone<S>, NarrowPhaseSolver>::distance;
602  distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
603  distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS<S>, Convex<S>, NarrowPhaseSolver>::distance;
604  distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS<S>, Plane<S>, NarrowPhaseSolver>::distance;
605  distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer<RSS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
606 
607  /* KDOPd distance not implemented */
608  /*
609  distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 16>, Box<S>, NarrowPhaseSolver>::distance;
610  distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 16>, Sphere<S>, NarrowPhaseSolver>::distance;
611  distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 16>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
612  distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 16>, Capsule<S>, NarrowPhaseSolver>::distance;
613  distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 16>, Cone<S>, NarrowPhaseSolver>::distance;
614  distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 16>, Cylinder<S>, NarrowPhaseSolver>::distance;
615  distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 16>, Convex<S>, NarrowPhaseSolver>::distance;
616  distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 16>, Plane<S>, NarrowPhaseSolver>::distance;
617  distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 16>, Halfspace<S>, NarrowPhaseSolver>::distance;
618 
619  distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 18>, Box<S>, NarrowPhaseSolver>::distance;
620  distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 18>, Sphere<S>, NarrowPhaseSolver>::distance;
621  distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 18>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
622  distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 18>, Capsule<S>, NarrowPhaseSolver>::distance;
623  distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 18>, Cone<S>, NarrowPhaseSolver>::distance;
624  distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 18>, Cylinder<S>, NarrowPhaseSolver>::distance;
625  distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 18>, Convex<S>, NarrowPhaseSolver>::distance;
626  distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 18>, Plane<S>, NarrowPhaseSolver>::distance;
627  distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 18>, Halfspace<S>, NarrowPhaseSolver>::distance;
628 
629  distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<S, 24>, Box<S>, NarrowPhaseSolver>::distance;
630  distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<S, 24>, Sphere<S>, NarrowPhaseSolver>::distance;
631  distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer<KDOP<S, 24>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
632  distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<S, 24>, Capsule<S>, NarrowPhaseSolver>::distance;
633  distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<S, 24>, Cone<S>, NarrowPhaseSolver>::distance;
634  distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<S, 24>, Cylinder<S>, NarrowPhaseSolver>::distance;
635  distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<S, 24>, Convex<S>, NarrowPhaseSolver>::distance;
636  distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<S, 24>, Plane<S>, NarrowPhaseSolver>::distance;
637  distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer<KDOP<S, 24>, Halfspace<S>, NarrowPhaseSolver>::distance;
638  */
639 
640  distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS<S>, Box<S>, NarrowPhaseSolver>::distance;
641  distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
642  distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer<kIOS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
643  distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
644  distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS<S>, Cone<S>, NarrowPhaseSolver>::distance;
645  distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
646  distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS<S>, Convex<S>, NarrowPhaseSolver>::distance;
647  distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS<S>, Plane<S>, NarrowPhaseSolver>::distance;
648  distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer<kIOS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
649 
650  distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS<S>, Box<S>, NarrowPhaseSolver>::distance;
651  distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS<S>, Sphere<S>, NarrowPhaseSolver>::distance;
652  distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer<OBBRSS<S>, Ellipsoid<S>, NarrowPhaseSolver>::distance;
653  distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS<S>, Capsule<S>, NarrowPhaseSolver>::distance;
654  distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS<S>, Cone<S>, NarrowPhaseSolver>::distance;
655  distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS<S>, Cylinder<S>, NarrowPhaseSolver>::distance;
656  distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS<S>, Convex<S>, NarrowPhaseSolver>::distance;
657  distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS<S>, Plane<S>, NarrowPhaseSolver>::distance;
658  distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS<S>, Halfspace<S>, NarrowPhaseSolver>::distance;
659 
660  distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB<S>, NarrowPhaseSolver>;
661  distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS<S>, NarrowPhaseSolver>;
662  distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS<S>, NarrowPhaseSolver>;
663  distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS<S>, NarrowPhaseSolver>;
664 
665 #if FCL_HAVE_OCTOMAP
666  distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box<S>, NarrowPhaseSolver>;
667  distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere<S>, NarrowPhaseSolver>;
668  distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance<Ellipsoid<S>, NarrowPhaseSolver>;
669  distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule<S>, NarrowPhaseSolver>;
670  distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone<S>, NarrowPhaseSolver>;
671  distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder<S>, NarrowPhaseSolver>;
672  distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex<S>, NarrowPhaseSolver>;
673  distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane<S>, NarrowPhaseSolver>;
674  distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance<Halfspace<S>, NarrowPhaseSolver>;
675 
676  distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box<S>, NarrowPhaseSolver>;
677  distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere<S>, NarrowPhaseSolver>;
678  distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance<Ellipsoid<S>, NarrowPhaseSolver>;
679  distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule<S>, NarrowPhaseSolver>;
680  distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone<S>, NarrowPhaseSolver>;
681  distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder<S>, NarrowPhaseSolver>;
682  distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex<S>, NarrowPhaseSolver>;
683  distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane<S>, NarrowPhaseSolver>;
684  distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance<Halfspace<S>, NarrowPhaseSolver>;
685 
686  distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
687 
688  distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB<S>, NarrowPhaseSolver>;
689  distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB<S>, NarrowPhaseSolver>;
690  distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS<S>, NarrowPhaseSolver>;
691  distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS<S>, NarrowPhaseSolver>;
692  distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS<S>, NarrowPhaseSolver>;
693  distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<S, 16>, NarrowPhaseSolver>;
694  distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<S, 18>, NarrowPhaseSolver>;
695  distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<S, 24>, NarrowPhaseSolver>;
696 
697  distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB<S>, NarrowPhaseSolver>;
698  distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB<S>, NarrowPhaseSolver>;
699  distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS<S>, NarrowPhaseSolver>;
700  distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS<S>, NarrowPhaseSolver>;
701  distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS<S>, NarrowPhaseSolver>;
702  distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 16>, NarrowPhaseSolver>;
703  distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 18>, NarrowPhaseSolver>;
704  distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<S, 24>, NarrowPhaseSolver>;
705 #endif
706 
707 }
708 
709 } // namespace detail
710 } // namespace fcl
711 
712 #endif
fcl::DistanceResult::min_distance
S min_distance
Minimum distance between two objects.
Definition: distance_result.h:64
fcl::detail::BVHShapeDistancer< kIOS< typename Shape::S >, Shape, NarrowPhaseSolver >::distance
static Shape::S distance(const CollisionGeometry< typename Shape::S > *o1, const Transform3< typename Shape::S > &tf1, const CollisionGeometry< typename Shape::S > *o2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result)
Definition: distance_func_matrix-inl.h:303
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:60
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::ShapeDistanceTraversalNode
Traversal node for distance between two shapes.
Definition: shape_distance_traversal_node.h:52
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::detail::BVHDistanceImpl< S, kIOS< S > >::run
static S run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result)
Definition: distance_func_matrix-inl.h:428
bvh_distance_traversal_node.h
fcl::detail::BVHDistanceImpl< S, RSS< S > >::run
static S run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result)
Definition: distance_func_matrix-inl.h:410
fcl::detail::MeshShapeDistanceTraversalNode
Traversal node for distance between mesh and shape.
Definition: mesh_shape_distance_traversal_node.h:52
mesh_distance_traversal_node.h
shape_octree_distance_traversal_node.h
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
bvh_shape_distance_traversal_node.h
fcl::detail::orientedMeshDistance
BV::S orientedMeshDistance(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 DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result)
Definition: distance_func_matrix-inl.h:387
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
fcl::DistanceRequest::isSatisfied
bool isSatisfied(const DistanceResult< S > &result) const
Definition: distance_request-inl.h:73
fcl::DistanceResult
distance result
Definition: distance_request.h:48
fcl::Ellipsoid
Center at zero point ellipsoid.
Definition: ellipsoid.h:51
shape_conservative_advancement_traversal_node.h
shape_mesh_distance_traversal_node.h
octree_shape_distance_traversal_node.h
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
distance_func_matrix.h
gjk_solver_indep.h
shape_bvh_distance_traversal_node.h
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
mesh_shape_distance_traversal_node.h
fcl::detail::orientedBVHShapeDistance
Shape::S orientedBVHShapeDistance(const CollisionGeometry< typename Shape::S > *o1, const Transform3< typename Shape::S > &tf1, const CollisionGeometry< typename Shape::S > *o2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result)
Definition: distance_func_matrix-inl.h:259
octree_distance_traversal_node.h
octree_mesh_distance_traversal_node.h
shape_mesh_conservative_advancement_traversal_node.h
mesh_conservative_advancement_traversal_node.h
mesh_octree_distance_traversal_node.h
fcl::detail::BVHDistanceImpl< S, OBBRSS< S > >::run
static S run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result)
Definition: distance_func_matrix-inl.h:446
fcl::detail::BVHShapeDistancer< OBBRSS< typename Shape::S >, Shape, NarrowPhaseSolver >::distance
static Shape::S distance(const CollisionGeometry< typename Shape::S > *o1, const Transform3< typename Shape::S > &tf1, const CollisionGeometry< typename Shape::S > *o2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result)
Definition: distance_func_matrix-inl.h:324
fcl::detail::BVHShapeDistancer< RSS< typename Shape::S >, Shape, NarrowPhaseSolver >::distance
static Shape::S distance(const CollisionGeometry< typename Shape::S > *o1, const Transform3< typename Shape::S > &tf1, const CollisionGeometry< typename Shape::S > *o2, const Transform3< typename Shape::S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< typename Shape::S > &request, DistanceResult< typename Shape::S > &result)
Definition: distance_func_matrix-inl.h:282
gjk_solver_libccd.h
shape_distance_traversal_node.h
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::detail::BVHDistance
BV::S BVHDistance(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 DistanceRequest< typename BV::S > &request, DistanceResult< typename BV::S > &result)
Definition: distance_func_matrix-inl.h:374
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:51
fcl::detail::distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
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_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::Plane
Infinite plane.
Definition: geometry/shape/plane.h:51
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:51
fcl::Cone
Center at zero cone.
Definition: cone.h:51
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
mesh_shape_conservative_advancement_traversal_node.h
collision_object.h
collision_node.h
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:96
fcl::Sphere
Center at zero point sphere.
Definition: sphere.h:51
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
distance_traversal_node_base.h
fcl::DistanceRequest
request to the distance computation
Definition: distance_request.h:52
fcl::detail::BVHDistanceImpl::run
static S run(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const DistanceRequest< S > &request, DistanceResult< S > &result)
Definition: distance_func_matrix-inl.h:346
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
fcl::detail::BVHShapeDistancer::distance
static S distance(const CollisionGeometry< S > *o1, const Transform3< S > &tf1, const CollisionGeometry< S > *o2, const Transform3< S > &tf2, const NarrowPhaseSolver *nsolver, const DistanceRequest< S > &request, DistanceResult< S > &result)
Definition: distance_func_matrix-inl.h:233
fcl::detail::MeshDistanceTraversalNode
Traversal node for distance computation between two meshes.
Definition: mesh_distance_traversal_node.h:55
fcl::detail::ShapeShapeDistance
Shape1::S ShapeShapeDistance(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 DistanceRequest< typename Shape1::S > &request, DistanceResult< typename Shape1::S > &result)
Definition: distance_func_matrix-inl.h:208

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:18