00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00038 #include "fcl/collision.h"
00039 #include "fcl/collision_func_matrix.h"
00040 #include "fcl/narrowphase/narrowphase.h"
00041
00042 #include <iostream>
00043
00044 namespace fcl
00045 {
00046
00047 template<typename GJKSolver>
00048 CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable()
00049 {
00050 static CollisionFunctionMatrix<GJKSolver> table;
00051 return table;
00052 };
00053
00054 template<typename NarrowPhaseSolver>
00055 std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
00056 const NarrowPhaseSolver* nsolver,
00057 const CollisionRequest& request,
00058 CollisionResult& result)
00059 {
00060 return collide(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(),
00061 nsolver, request, result);
00062 }
00063
00064 template<typename NarrowPhaseSolver>
00065 std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
00066 const CollisionGeometry* o2, const Transform3f& tf2,
00067 const NarrowPhaseSolver* nsolver_,
00068 const CollisionRequest& request,
00069 CollisionResult& result)
00070 {
00071 const NarrowPhaseSolver* nsolver = nsolver_;
00072 if(!nsolver_)
00073 nsolver = new NarrowPhaseSolver();
00074
00075 const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>();
00076
00077 std::size_t res;
00078 if(request.num_max_contacts == 0)
00079 {
00080 std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
00081 res = 0;
00082 }
00083 else
00084 {
00085 OBJECT_TYPE object_type1 = o1->getObjectType();
00086 OBJECT_TYPE object_type2 = o2->getObjectType();
00087 NODE_TYPE node_type1 = o1->getNodeType();
00088 NODE_TYPE node_type2 = o2->getNodeType();
00089
00090 if(object_type1 == OT_GEOM & object_type2 == OT_BVH)
00091 {
00092 if(!looktable.collision_matrix[node_type2][node_type1])
00093 {
00094 std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
00095 res = 0;
00096 }
00097 else
00098 res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result);
00099 }
00100 else
00101 {
00102 if(!looktable.collision_matrix[node_type1][node_type2])
00103 {
00104 std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
00105 res = 0;
00106 }
00107 else
00108 res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result);
00109 }
00110 }
00111
00112 if(!nsolver_)
00113 delete nsolver;
00114
00115 return res;
00116 }
00117
00118 template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
00119 template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
00120 template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_libccd* nsolver, const CollisionRequest& request, CollisionResult& result);
00121 template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver_indep* nsolver, const CollisionRequest& request, CollisionResult& result);
00122
00123
00124 std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
00125 const CollisionRequest& request, CollisionResult& result)
00126 {
00127 GJKSolver_libccd solver;
00128 return collide<GJKSolver_libccd>(o1, o2, &solver, request, result);
00129 }
00130
00131 std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
00132 const CollisionGeometry* o2, const Transform3f& tf2,
00133 const CollisionRequest& request, CollisionResult& result)
00134 {
00135 GJKSolver_libccd solver;
00136 return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, request, result);
00137
00138
00139 }
00140
00141 }