All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends

distance_func_matrix.cpp

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "fcl/distance_func_matrix.h"
00038 
00039 #include "fcl/collision_node.h"
00040 #include "fcl/traversal/traversal_node_setup.h"
00041 #include "fcl/narrowphase/narrowphase.h"
00042 
00043 namespace fcl
00044 {
00045 
00046 template<typename T_SH, typename NarrowPhaseSolver>
00047 FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00048                              const DistanceRequest& request, DistanceResult& result)
00049 {
00050   if(request.isSatisfied(result)) return result.min_distance;
00051   ShapeOcTreeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
00052   const T_SH* obj1 = static_cast<const T_SH*>(o1);
00053   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00054   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00055 
00056   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00057   distance(&node);
00058   
00059   return result.min_distance;
00060 }
00061 
00062 template<typename T_SH, typename NarrowPhaseSolver>
00063 FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00064                              const DistanceRequest& request, DistanceResult& result)
00065 {
00066   if(request.isSatisfied(result)) return result.min_distance;
00067   OcTreeShapeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
00068   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00069   const T_SH* obj2 = static_cast<const T_SH*>(o2);
00070   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00071 
00072   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00073   distance(&node);
00074   
00075   return result.min_distance;
00076 }
00077 
00078 template<typename NarrowPhaseSolver>
00079 FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00080                         const DistanceRequest& request, DistanceResult& result)
00081 {
00082   if(request.isSatisfied(result)) return result.min_distance;
00083   OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
00084   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00085   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00086   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00087 
00088   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00089   distance(&node);
00090 
00091   return result.min_distance;
00092 }
00093 
00094 template<typename T_BVH, typename NarrowPhaseSolver>
00095 FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00096                            const DistanceRequest& request, DistanceResult& result)
00097 {
00098   if(request.isSatisfied(result)) return result.min_distance;
00099   MeshOcTreeDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
00100   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
00101   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00102   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00103 
00104   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00105   distance(&node);
00106 
00107   return result.min_distance;
00108 }
00109 
00110 template<typename T_BVH, typename NarrowPhaseSolver>
00111 FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00112                        const DistanceRequest& request, DistanceResult& result)
00113 {
00114   if(request.isSatisfied(result)) return result.min_distance;
00115   OcTreeMeshDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
00116   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00117   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
00118   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00119 
00120   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00121   distance(&node);
00122 
00123   return result.min_distance;
00124 }
00125 
00126 
00127 
00128 template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
00129 FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00130                         const DistanceRequest& request, DistanceResult& result)
00131 {
00132   if(request.isSatisfied(result)) return result.min_distance;
00133   ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
00134   const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
00135   const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
00136 
00137   initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00138   distance(&node);
00139 
00140   return result.min_distance;
00141 }
00142 
00143 template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
00144 struct BVHShapeDistancer
00145 {
00146   static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00147                            const DistanceRequest& request, DistanceResult& result)
00148   {
00149     if(request.isSatisfied(result)) return result.min_distance;
00150     MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
00151     const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
00152     BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
00153     Transform3f tf1_tmp = tf1;
00154     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00155 
00156     initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
00157     fcl::distance(&node);
00158     
00159     delete obj1_tmp;
00160     return result.min_distance;
00161   }
00162 };
00163 
00164 template<typename T_SH, typename NarrowPhaseSolver>
00165 struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
00166 {
00167   static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00168                            const DistanceRequest& request, DistanceResult& result)
00169   {
00170     if(request.isSatisfied(result)) return result.min_distance;
00171     MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
00172     const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
00173     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00174 
00175     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00176     fcl::distance(&node);
00177 
00178     return result.min_distance;
00179   }
00180 };
00181 
00182 
00183 template<typename T_SH, typename NarrowPhaseSolver>
00184 struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
00185 {
00186   static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00187                        const DistanceRequest& request, DistanceResult& result)
00188   {
00189     if(request.isSatisfied(result)) return result.min_distance;
00190     MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
00191     const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
00192     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00193 
00194     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00195     fcl::distance(&node);
00196 
00197     return result.min_distance;
00198   }
00199 };
00200 
00201 template<typename T_SH, typename NarrowPhaseSolver>
00202 struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
00203 {
00204   static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
00205                            const DistanceRequest& request, DistanceResult& result)
00206   {
00207     if(request.isSatisfied(result)) return result.min_distance;
00208     MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
00209     const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
00210     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00211 
00212     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00213     fcl::distance(&node);
00214 
00215     return result.min_distance;
00216   }
00217 };
00218 
00219 
00220 template<typename T_BVH>
00221 FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00222                      const DistanceRequest& request, DistanceResult& result)
00223 {
00224   if(request.isSatisfied(result)) return result.min_distance;
00225   MeshDistanceTraversalNode<T_BVH> node;
00226   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
00227   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
00228   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
00229   Transform3f tf1_tmp = tf1;
00230   BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
00231   Transform3f tf2_tmp = tf2;
00232 
00233   initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
00234   distance(&node);
00235   
00236   return result.min_distance;
00237 }
00238 
00239 template<>
00240 FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00241                           const DistanceRequest& request, DistanceResult& result)
00242 {
00243   if(request.isSatisfied(result)) return result.min_distance;
00244   MeshDistanceTraversalNodeRSS node;
00245   const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
00246   const BVHModel<RSS>* obj2 = static_cast<const BVHModel<RSS>* >(o2);
00247 
00248   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00249   distance(&node);
00250 
00251   return result.min_distance;
00252 }
00253 
00254 template<>
00255 FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00256                            const DistanceRequest& request, DistanceResult& result)
00257 {
00258   if(request.isSatisfied(result)) return result.min_distance;
00259   MeshDistanceTraversalNodekIOS node;
00260   const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
00261   const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
00262 
00263   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00264   distance(&node);
00265 
00266   return result.min_distance;
00267 }
00268 
00269 
00270 template<>
00271 FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00272                              const DistanceRequest& request, DistanceResult& result)
00273 {
00274   if(request.isSatisfied(result)) return result.min_distance;
00275   MeshDistanceTraversalNodeOBBRSS node;
00276   const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
00277   const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
00278 
00279   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00280   distance(&node);
00281 
00282   return result.min_distance;
00283 }
00284 
00285 
00286 template<typename T_BVH, typename NarrowPhaseSolver>
00287 FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00288                      const NarrowPhaseSolver* nsolver,
00289                      const DistanceRequest& request, DistanceResult& result)
00290 {
00291   return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
00292 }
00293 
00294 template<typename NarrowPhaseSolver>
00295 DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
00296 {
00297   for(int i = 0; i < NODE_COUNT; ++i)
00298   {
00299     for(int j = 0; j < NODE_COUNT; ++j)
00300       distance_matrix[i][j] = NULL;
00301   }
00302 
00303   distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box, NarrowPhaseSolver>;
00304   distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere, NarrowPhaseSolver>;
00305   distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule, NarrowPhaseSolver>;
00306   distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone, NarrowPhaseSolver>;
00307   distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
00308   distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, NarrowPhaseSolver>;
00309   distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, NarrowPhaseSolver>;
00310 
00311   distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
00312   distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
00313   distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule, NarrowPhaseSolver>;
00314   distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone, NarrowPhaseSolver>;
00315   distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
00316   distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, NarrowPhaseSolver>;
00317   distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
00318 
00319   distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
00320   distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
00321   distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule, NarrowPhaseSolver>;
00322   distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone, NarrowPhaseSolver>;
00323   distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
00324   distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, NarrowPhaseSolver>;
00325   distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, NarrowPhaseSolver>;
00326 
00327   distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
00328   distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
00329   distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule, NarrowPhaseSolver>;
00330   distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone, NarrowPhaseSolver>;
00331   distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
00332   distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, NarrowPhaseSolver>;
00333   distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, NarrowPhaseSolver>;
00334 
00335   distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
00336   distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
00337   distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule, NarrowPhaseSolver>;
00338   distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone, NarrowPhaseSolver>;
00339   distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
00340   distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, NarrowPhaseSolver>;
00341   distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, NarrowPhaseSolver>;
00342 
00343   distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
00344   distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
00345   distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex, Capsule, NarrowPhaseSolver>;
00346   distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex, Cone, NarrowPhaseSolver>;
00347   distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
00348   distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, NarrowPhaseSolver>;
00349   distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, NarrowPhaseSolver>;
00350 
00351   distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
00352   distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
00353   distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule, NarrowPhaseSolver>;
00354   distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone, NarrowPhaseSolver>;
00355   distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
00356   distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
00357   distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
00358 
00359   /* AABB distance not implemented */
00360   /*
00361   distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, NarrowPhaseSolver>::distance;
00362   distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, NarrowPhaseSolver>::distance;
00363   distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule, NarrowPhaseSolver>::distance;
00364   distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone, NarrowPhaseSolver>::distance;
00365   distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
00366   distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
00367   distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
00368 
00369   distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
00370   distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
00371   distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule, NarrowPhaseSolver>::distance;
00372   distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone, NarrowPhaseSolver>::distance;
00373   distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
00374   distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
00375   distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
00376   */
00377 
00378   distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
00379   distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
00380   distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule, NarrowPhaseSolver>::distance;
00381   distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone, NarrowPhaseSolver>::distance;
00382   distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
00383   distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
00384   distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
00385 
00386   /* KDOP distance not implemented */
00387   /*
00388   distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, NarrowPhaseSolver>::distance;
00389   distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, NarrowPhaseSolver>::distance;
00390   distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule, NarrowPhaseSolver>::distance;
00391   distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone, NarrowPhaseSolver>::distance;
00392   distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
00393   distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, NarrowPhaseSolver>::distance;
00394   distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, NarrowPhaseSolver>::distance;
00395 
00396   distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
00397   distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
00398   distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule, NarrowPhaseSolver>::distance;
00399   distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone, NarrowPhaseSolver>::distance;
00400   distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
00401   distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, NarrowPhaseSolver>::distance;
00402   distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, NarrowPhaseSolver>::distance;
00403 
00404   distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
00405   distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
00406   distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule, NarrowPhaseSolver>::distance;
00407   distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone, NarrowPhaseSolver>::distance;
00408   distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
00409   distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, NarrowPhaseSolver>::distance;
00410   distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, NarrowPhaseSolver>::distance;
00411   */
00412 
00413   distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
00414   distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere, NarrowPhaseSolver>::distance;
00415   distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule, NarrowPhaseSolver>::distance;
00416   distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone, NarrowPhaseSolver>::distance;
00417   distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
00418   distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, NarrowPhaseSolver>::distance;
00419   distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, NarrowPhaseSolver>::distance;
00420 
00421   distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
00422   distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
00423   distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule, NarrowPhaseSolver>::distance;
00424   distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone, NarrowPhaseSolver>::distance;
00425   distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
00426   distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, NarrowPhaseSolver>::distance;
00427   distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, NarrowPhaseSolver>::distance;
00428 
00429   distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
00430   distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
00431   distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
00432   distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
00433 
00434   distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance<Box, NarrowPhaseSolver>;
00435   distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance<Sphere, NarrowPhaseSolver>;
00436   distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance<Capsule, NarrowPhaseSolver>;
00437   distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance<Cone, NarrowPhaseSolver>;
00438   distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance<Cylinder, NarrowPhaseSolver>;
00439   distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance<Convex, NarrowPhaseSolver>;
00440   distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance<Plane, NarrowPhaseSolver>;
00441 
00442   distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance<Box, NarrowPhaseSolver>;
00443   distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance<Sphere, NarrowPhaseSolver>;
00444   distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance<Capsule, NarrowPhaseSolver>;
00445   distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance<Cone, NarrowPhaseSolver>;
00446   distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance<Cylinder, NarrowPhaseSolver>;
00447   distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance<Convex, NarrowPhaseSolver>;
00448   distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance<Plane, NarrowPhaseSolver>;
00449 
00450   distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance<NarrowPhaseSolver>;
00451 
00452   distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance<AABB, NarrowPhaseSolver>;
00453   distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance<OBB, NarrowPhaseSolver>;
00454   distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance<RSS, NarrowPhaseSolver>;
00455   distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance<OBBRSS, NarrowPhaseSolver>;
00456   distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance<kIOS, NarrowPhaseSolver>;
00457   distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance<KDOP<16>, NarrowPhaseSolver>;
00458   distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance<KDOP<18>, NarrowPhaseSolver>;
00459   distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance<KDOP<24>, NarrowPhaseSolver>;
00460 
00461   distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance<AABB, NarrowPhaseSolver>;
00462   distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance<OBB, NarrowPhaseSolver>;
00463   distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance<RSS, NarrowPhaseSolver>;
00464   distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance<OBBRSS, NarrowPhaseSolver>;
00465   distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance<kIOS, NarrowPhaseSolver>;
00466   distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<16>, NarrowPhaseSolver>;
00467   distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<18>, NarrowPhaseSolver>;
00468   distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance<KDOP<24>, NarrowPhaseSolver>;
00469 }
00470 
00471 template struct DistanceFunctionMatrix<GJKSolver_libccd>;
00472 template struct DistanceFunctionMatrix<GJKSolver_indep>;
00473 
00474 
00475 }