fcl » src
collision_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 
00038 #include "fcl/collision_func_matrix.h"
00039 
00040 #include "fcl/traversal/traversal_node_setup.h"
00041 #include "fcl/collision_node.h"
00042 #include "fcl/narrowphase/narrowphase.h"
00043 
00044 
00045 namespace fcl
00046 {
00047 
00048 template<typename T_SH, typename NarrowPhaseSolver>
00049 std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00050                                const NarrowPhaseSolver* nsolver,
00051                                const CollisionRequest& request, CollisionResult& result)
00052 {
00053   if(request.isSatisfied(result)) return result.numContacts();
00054 
00055   ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
00056   const T_SH* obj1 = static_cast<const T_SH*>(o1);
00057   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00058   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00059 
00060   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00061   collide(&node);
00062 
00063   return result.numContacts();
00064 }
00065 
00066 template<typename T_SH, typename NarrowPhaseSolver>
00067 std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00068                                const NarrowPhaseSolver* nsolver,
00069                                const CollisionRequest& request, CollisionResult& result)
00070 {
00071   if(request.isSatisfied(result)) return result.numContacts();
00072 
00073   OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
00074   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00075   const T_SH* obj2 = static_cast<const T_SH*>(o2);
00076   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00077 
00078   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00079   collide(&node);
00080 
00081   return result.numContacts();
00082 }
00083 
00084 template<typename NarrowPhaseSolver>
00085 std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00086                           const NarrowPhaseSolver* nsolver,
00087                           const CollisionRequest& request, CollisionResult& result)
00088 {
00089   if(request.isSatisfied(result)) return result.numContacts();
00090 
00091   OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
00092   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00093   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00094   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00095 
00096   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00097   collide(&node);
00098 
00099   return result.numContacts();
00100 }
00101 
00102 template<typename T_BVH, typename NarrowPhaseSolver>
00103 std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00104                              const NarrowPhaseSolver* nsolver,
00105                              const CollisionRequest& request, CollisionResult& result)
00106 {
00107   if(request.isSatisfied(result)) return result.numContacts();
00108 
00109   OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
00110   const OcTree* obj1 = static_cast<const OcTree*>(o1);
00111   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
00112   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00113 
00114   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00115   collide(&node);
00116   
00117   return result.numContacts();
00118 }
00119 
00120 template<typename T_BVH, typename NarrowPhaseSolver>
00121 std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
00122                              const NarrowPhaseSolver* nsolver,
00123                              const CollisionRequest& request, CollisionResult& result)
00124 {
00125   if(request.isSatisfied(result)) return result.numContacts();
00126 
00127   MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
00128   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
00129   const OcTree* obj2 = static_cast<const OcTree*>(o2);
00130   OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
00131 
00132   initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
00133   collide(&node);
00134   
00135   return result.numContacts();
00136 }
00137 
00138 
00139 template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
00140 std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00141                               const NarrowPhaseSolver* nsolver,
00142                               const CollisionRequest& request, CollisionResult& result)
00143 {
00144   if(request.isSatisfied(result)) return result.numContacts();
00145 
00146   ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
00147   const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
00148   const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
00149 
00150   initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00151   collide(&node);
00152 
00153   return result.numContacts();
00154 }
00155 
00156 template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
00157 struct BVHShapeCollider
00158 {
00159   static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00160                              const NarrowPhaseSolver* nsolver,
00161                              const CollisionRequest& request, CollisionResult& result)
00162   {
00163     if(request.isSatisfied(result)) return result.numContacts();
00164 
00165     MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
00166     const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
00167     BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
00168     Transform3f tf1_tmp = tf1;
00169     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00170 
00171     initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
00172     fcl::collide(&node);
00173 
00174     delete obj1_tmp;
00175     return result.numContacts();
00176   }
00177 };
00178 
00179 
00180 template<typename T_SH, typename NarrowPhaseSolver>
00181 struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
00182 {
00183   static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00184                              const NarrowPhaseSolver* nsolver,
00185                              const CollisionRequest& request, CollisionResult& result)
00186   {
00187     if(request.isSatisfied(result)) return result.numContacts();
00188 
00189     MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
00190     const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
00191     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00192 
00193     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00194     fcl::collide(&node);
00195 
00196     return result.numContacts();
00197   } 
00198 };
00199 
00200 
00201 template<typename T_SH, typename NarrowPhaseSolver>
00202 struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
00203 {
00204   static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00205                              const NarrowPhaseSolver* nsolver,
00206                              const CollisionRequest& request, CollisionResult& result)
00207   {
00208     if(request.isSatisfied(result)) return result.numContacts();
00209 
00210     MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
00211     const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
00212     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00213 
00214     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00215     fcl::collide(&node);
00216 
00217     return result.numContacts();
00218   } 
00219 };
00220 
00221 
00222 template<typename T_SH, typename NarrowPhaseSolver>
00223 struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
00224 {
00225   static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00226                              const NarrowPhaseSolver* nsolver,
00227                              const CollisionRequest& request, CollisionResult& result)
00228   {
00229     if(request.isSatisfied(result)) return result.numContacts();
00230 
00231     MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
00232     const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
00233     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00234 
00235     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00236     fcl::collide(&node);
00237 
00238     return result.numContacts();
00239   } 
00240 };
00241 
00242 
00243 template<typename T_SH, typename NarrowPhaseSolver>
00244 struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
00245 {
00246   static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00247                              const NarrowPhaseSolver* nsolver,
00248                              const CollisionRequest& request, CollisionResult& result)
00249   {
00250     if(request.isSatisfied(result)) return result.numContacts();
00251 
00252     MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
00253     const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
00254     const T_SH* obj2 = static_cast<const T_SH*>(o2);
00255 
00256     initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
00257     fcl::collide(&node);
00258 
00259     return result.numContacts();
00260   } 
00261 };
00262 
00263 
00264 template<typename T_BVH>
00265 std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
00266 {
00267   if(request.isSatisfied(result)) return result.numContacts();
00268   
00269   MeshCollisionTraversalNode<T_BVH> node;
00270   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
00271   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
00272   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
00273   Transform3f tf1_tmp = tf1;
00274   BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
00275   Transform3f tf2_tmp = tf2;
00276   
00277   initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
00278   collide(&node);
00279 
00280   delete obj1_tmp;
00281   delete obj2_tmp;
00282 
00283   return result.numContacts();
00284 }
00285 
00286 template<>
00287 std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
00288 {
00289   if(request.isSatisfied(result)) return result.numContacts();
00290 
00291   MeshCollisionTraversalNodeOBB node;
00292   const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
00293   const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
00294 
00295   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00296   collide(&node);
00297 
00298   return result.numContacts();
00299 }
00300 
00301 template<>
00302 std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
00303 {
00304   if(request.isSatisfied(result)) return result.numContacts();
00305 
00306   MeshCollisionTraversalNodeOBBRSS node;
00307   const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
00308   const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
00309 
00310   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00311   collide(&node);
00312 
00313   return result.numContacts();
00314 }
00315 
00316 
00317 template<>
00318 std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
00319 {
00320   if(request.isSatisfied(result)) return result.numContacts();
00321 
00322   MeshCollisionTraversalNodekIOS node;
00323   const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
00324   const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
00325 
00326   initialize(node, *obj1, tf1, *obj2, tf2, request, result);
00327   collide(&node);
00328 
00329   return result.numContacts();
00330 }
00331 
00332 
00333 template<typename T_BVH, typename NarrowPhaseSolver>
00334 std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, 
00335                        const NarrowPhaseSolver* nsolver,
00336                        const CollisionRequest& request, CollisionResult& result)
00337 {
00338   return BVHCollide<T_BVH>(o1, tf1, o2, tf2, request, result);
00339 }
00340 
00341 
00342 template<typename NarrowPhaseSolver>
00343 CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
00344 {
00345   for(int i = 0; i < NODE_COUNT; ++i)
00346   {
00347     for(int j = 0; j < NODE_COUNT; ++j)
00348       collision_matrix[i][j] = NULL;
00349   }
00350 
00351   collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box, NarrowPhaseSolver>;
00352   collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere, NarrowPhaseSolver>;
00353   collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule, NarrowPhaseSolver>;
00354   collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone, NarrowPhaseSolver>;
00355   collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
00356   collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, NarrowPhaseSolver>;
00357   collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, NarrowPhaseSolver>;
00358 
00359   collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
00360   collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
00361   collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule, NarrowPhaseSolver>;
00362   collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone, NarrowPhaseSolver>;
00363   collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
00364   collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, NarrowPhaseSolver>;
00365   collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
00366 
00367   collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
00368   collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
00369   collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule, NarrowPhaseSolver>;
00370   collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone, NarrowPhaseSolver>;
00371   collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
00372   collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, NarrowPhaseSolver>;
00373   collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, NarrowPhaseSolver>;
00374 
00375   collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
00376   collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
00377   collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule, NarrowPhaseSolver>;
00378   collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone, NarrowPhaseSolver>;
00379   collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
00380   collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, NarrowPhaseSolver>;
00381   collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, NarrowPhaseSolver>;
00382 
00383   collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
00384   collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
00385   collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule, NarrowPhaseSolver>;
00386   collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone, NarrowPhaseSolver>;
00387   collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
00388   collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, NarrowPhaseSolver>;
00389   collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, NarrowPhaseSolver>;
00390 
00391   collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
00392   collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
00393   collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule, NarrowPhaseSolver>;
00394   collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone, NarrowPhaseSolver>;
00395   collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
00396   collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, NarrowPhaseSolver>;
00397   collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, NarrowPhaseSolver>;
00398 
00399   collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
00400   collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
00401   collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule, NarrowPhaseSolver>;
00402   collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone, NarrowPhaseSolver>;
00403   collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
00404   collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, NarrowPhaseSolver>;
00405   collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, NarrowPhaseSolver>;
00406 
00407   collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
00408   collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
00409   collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule, NarrowPhaseSolver>::collide;
00410   collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone, NarrowPhaseSolver>::collide;
00411   collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
00412   collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, NarrowPhaseSolver>::collide;
00413   collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, NarrowPhaseSolver>::collide;
00414 
00415   collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
00416   collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
00417   collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule, NarrowPhaseSolver>::collide;
00418   collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone, NarrowPhaseSolver>::collide;
00419   collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
00420   collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, NarrowPhaseSolver>::collide;
00421   collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, NarrowPhaseSolver>::collide;
00422 
00423   collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
00424   collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
00425   collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule, NarrowPhaseSolver>::collide;
00426   collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone, NarrowPhaseSolver>::collide;
00427   collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
00428   collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, NarrowPhaseSolver>::collide;
00429   collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, NarrowPhaseSolver>::collide;
00430 
00431   collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
00432   collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
00433   collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule, NarrowPhaseSolver>::collide;
00434   collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone, NarrowPhaseSolver>::collide;
00435   collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
00436   collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, NarrowPhaseSolver>::collide;
00437   collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, NarrowPhaseSolver>::collide;
00438 
00439   collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
00440   collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
00441   collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule, NarrowPhaseSolver>::collide;
00442   collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone, NarrowPhaseSolver>::collide;
00443   collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
00444   collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, NarrowPhaseSolver>::collide;
00445   collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, NarrowPhaseSolver>::collide;
00446 
00447   collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
00448   collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
00449   collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule, NarrowPhaseSolver>::collide;
00450   collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone, NarrowPhaseSolver>::collide;
00451   collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
00452   collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, NarrowPhaseSolver>::collide;
00453   collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, NarrowPhaseSolver>::collide;
00454 
00455   collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
00456   collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
00457   collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule, NarrowPhaseSolver>::collide;
00458   collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone, NarrowPhaseSolver>::collide;
00459   collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
00460   collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, NarrowPhaseSolver>::collide;
00461   collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, NarrowPhaseSolver>::collide;
00462 
00463   collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
00464   collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
00465   collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule, NarrowPhaseSolver>::collide;
00466   collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone, NarrowPhaseSolver>::collide;
00467   collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
00468   collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, NarrowPhaseSolver>::collide;
00469   collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, NarrowPhaseSolver>::collide;
00470 
00471   collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, NarrowPhaseSolver>;
00472   collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, NarrowPhaseSolver>;
00473   collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS, NarrowPhaseSolver>;
00474   collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16>, NarrowPhaseSolver>;
00475   collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18>, NarrowPhaseSolver>;
00476   collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24>, NarrowPhaseSolver>;
00477   collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, NarrowPhaseSolver>;
00478   collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, NarrowPhaseSolver>;
00479 
00480   collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box, NarrowPhaseSolver>;
00481   collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere, NarrowPhaseSolver>;
00482   collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule, NarrowPhaseSolver>;
00483   collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone, NarrowPhaseSolver>;
00484   collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder, NarrowPhaseSolver>;
00485   collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<Convex, NarrowPhaseSolver>;
00486   collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane, NarrowPhaseSolver>;
00487 
00488   collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box, NarrowPhaseSolver>;
00489   collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere, NarrowPhaseSolver>;
00490   collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule, NarrowPhaseSolver>;
00491   collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone, NarrowPhaseSolver>;
00492   collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder, NarrowPhaseSolver>;
00493   collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<Convex, NarrowPhaseSolver>;
00494   collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane, NarrowPhaseSolver>;
00495 
00496   collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide<NarrowPhaseSolver>;
00497 
00498   collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB, NarrowPhaseSolver>;
00499   collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB, NarrowPhaseSolver>;
00500   collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS, NarrowPhaseSolver>;
00501   collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS, NarrowPhaseSolver>;
00502   collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS, NarrowPhaseSolver>;
00503   collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16>, NarrowPhaseSolver>;
00504   collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18>, NarrowPhaseSolver>;
00505   collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24>, NarrowPhaseSolver>;
00506 
00507   collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB, NarrowPhaseSolver>;
00508   collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB, NarrowPhaseSolver>;
00509   collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS, NarrowPhaseSolver>;
00510   collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS, NarrowPhaseSolver>;
00511   collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS, NarrowPhaseSolver>;
00512   collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16>, NarrowPhaseSolver>;
00513   collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18>, NarrowPhaseSolver>;
00514   collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24>, NarrowPhaseSolver>;
00515 
00516 }
00517 
00518 
00519 template struct CollisionFunctionMatrix<GJKSolver_libccd>;
00520 template struct CollisionFunctionMatrix<GJKSolver_indep>;
00521 
00522 
00523 }