From 4f75ae6af65314dcff38982fc3a239c2d223563a Mon Sep 17 00:00:00 2001
From: Lucile Remigy <lucile.remigy@epitech.eu>
Date: Mon, 21 Oct 2019 14:49:57 +0200
Subject: [PATCH] traits collision_func_matrix.cpp
---
src/collision_func_matrix.cpp | 233 ++++++++++++----------------------
src/traits_traversal.h | 23 +++-
2 files changed, 100 insertions(+), 156 deletions(-)
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index 78e9719e..73d6a931 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -49,92 +49,24 @@ namespace fcl
{
#ifdef HPP_FCL_HAVE_OCTOMAP
-template<typename T_SH>
-std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver* nsolver,
- const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
- ShapeOcTreeCollisionTraversalNode<T_SH> node (request);
- const T_SH* obj1 = static_cast<const T_SH*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
- collide(&node, request, result);
-
- return result.numContacts();
-}
-
-template<typename T_SH>
-std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
+template<typename TypeA, typename TypeB>
+std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
- OcTreeShapeCollisionTraversalNode<T_SH> node (request);
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
+ typename TraversalTraits<TypeA, TypeB>::CollisionTraversal_t node (request);
+ const TypeA* obj1 = static_cast<const TypeA*>(o1);
+ const TypeB* obj2 = static_cast<const TypeB*>(o2);
OcTreeSolver otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
collide(&node, request, result);
return result.numContacts();
-}
-
-std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver* nsolver,
- const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
-
- OcTreeCollisionTraversalNode node (request);
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
- collide(&node, request, result);
-
- return result.numContacts();
-}
-
-template<typename T_BVH>
-std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver* nsolver,
- const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
-
- OcTreeMeshCollisionTraversalNode<T_BVH> node (request);
- const OcTree* obj1 = static_cast<const OcTree*>(o1);
- const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
- collide(&node, request, result);
- return result.numContacts();
-}
-template<typename T_BVH>
-std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver* nsolver,
- const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
-
- MeshOcTreeCollisionTraversalNode<T_BVH> node (request);
- const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
- const OcTree* obj2 = static_cast<const OcTree*>(o2);
- OcTreeSolver otsolver(nsolver);
-
- initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, result);
- collide(&node, request, result);
- return result.numContacts();
-}
#endif
@@ -178,6 +110,28 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
return 0;
}
+namespace details
+{
+
+template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH>
+std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver* nsolver,
+ const CollisionRequest& request, CollisionResult& result)
+{
+ if(request.isSatisfied(result)) return result.numContacts();
+
+ OrientMeshShapeCollisionTraveralNode node (request);
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
+ fcl::collide(&node, request, result);
+ return result.numContacts();
+}
+
+}
+
+
template<typename T_BVH, typename T_SH>
struct BVHShapeCollider
{
@@ -201,28 +155,6 @@ struct BVHShapeCollider
}
};
-namespace details
-{
-
-template<typename OrientMeshShapeCollisionTraveralNode, typename T_BVH, typename T_SH>
-std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver* nsolver,
- const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
-
- OrientMeshShapeCollisionTraveralNode node (request);
- const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
- const T_SH* obj2 = static_cast<const T_SH*>(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result);
- fcl::collide(&node, request, result);
- return result.numContacts();
-}
-
-}
-
-
template<typename T_SH>
struct BVHShapeCollider<OBB, T_SH>
{
@@ -270,6 +202,24 @@ struct BVHShapeCollider<OBBRSS, T_SH>
}
};
+namespace details
+{
+template<typename OrientedMeshCollisionTraversalNode, typename T_BVH>
+std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
+{
+ if(request.isSatisfied(result)) return result.numContacts();
+
+ OrientedMeshCollisionTraversalNode node (request);
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, result);
+ collide(&node, request, result);
+
+ return result.numContacts();
+}
+
+}
template<typename T_BVH>
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
@@ -293,25 +243,6 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
return result.numContacts();
}
-namespace details
-{
-template<typename OrientedMeshCollisionTraversalNode, typename T_BVH>
-std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
-{
- if(request.isSatisfied(result)) return result.numContacts();
-
- OrientedMeshCollisionTraversalNode node (request);
- const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
- const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
-
- initialize(node, *obj1, tf1, *obj2, tf2, result);
- collide(&node, request, result);
-
- return result.numContacts();
-}
-
-}
-
template<>
std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
@@ -503,43 +434,43 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
#ifdef HPP_FCL_HAVE_OCTOMAP
- collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide<Box>;
- collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide<Sphere>;
- collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide<Capsule>;
- collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide<Cone>;
- collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide<Cylinder>;
- collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide<ConvexBase>;
- collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide<Plane>;
- collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide<Halfspace>;
-
- collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide<Box>;
- collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide<Sphere>;
- collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide<Capsule>;
- collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide<Cone>;
- collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide<Cylinder>;
- collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide<ConvexBase>;
- collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide<Plane>;
- collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide<Halfspace>;
-
- collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide;
-
- collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide<AABB>;
- collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide<OBB>;
- collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide<RSS>;
- collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide<OBBRSS>;
- collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide<kIOS>;
- collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide<KDOP<16> >;
- collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide<KDOP<18> >;
- collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide<KDOP<24> >;
-
- collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide<AABB>;
- collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide<OBB>;
- collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide<RSS>;
- collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide<OBBRSS>;
- collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide<kIOS>;
- collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<16> >;
- collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<18> >;
- collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide<KDOP<24> >;
+ collision_matrix[GEOM_OCTREE][GEOM_BOX] = &Collide<Box>;
+ collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Collide<Sphere>;
+ collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Collide<Capsule>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONE] = &Collide<Cone>;
+ collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Collide<Cylinder>;
+ collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Collide<ConvexBase>;
+ collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &Collide<Plane>;
+ collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Collide<Halfspace>;
+
+ collision_matrix[GEOM_BOX][GEOM_OCTREE] = &Collide<Box>;
+ collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Collide<Sphere>;
+ collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Collide<Capsule>;
+ collision_matrix[GEOM_CONE][GEOM_OCTREE] = &Collide<Cone>;
+ collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Collide<Cylinder>;
+ collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Collide<ConvexBase>;
+ collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &Collide<Plane>;
+ collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Collide<Halfspace>;
+
+ collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Collide;
+
+ collision_matrix[GEOM_OCTREE][BV_AABB] = &Collide<AABB>;
+ collision_matrix[GEOM_OCTREE][BV_OBB] = &Collide<OBB>;
+ collision_matrix[GEOM_OCTREE][BV_RSS] = &Collide<RSS>;
+ collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &Collide<OBBRSS>;
+ collision_matrix[GEOM_OCTREE][BV_kIOS] = &Collide<kIOS>;
+ collision_matrix[GEOM_OCTREE][BV_KDOP16] = &Collide<KDOP<16> >;
+ collision_matrix[GEOM_OCTREE][BV_KDOP18] = &Collide<KDOP<18> >;
+ collision_matrix[GEOM_OCTREE][BV_KDOP24] = &Collide<KDOP<24> >;
+
+ collision_matrix[BV_AABB][GEOM_OCTREE] = &Collide<AABB>;
+ collision_matrix[BV_OBB][GEOM_OCTREE] = &Collide<OBB>;
+ collision_matrix[BV_RSS][GEOM_OCTREE] = &Collide<RSS>;
+ collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide<OBBRSS>;
+ collision_matrix[BV_kIOS][GEOM_OCTREE] = &Collide<kIOS>;
+ collision_matrix[BV_KDOP16][GEOM_OCTREE] = &Collide<KDOP<16> >;
+ collision_matrix[BV_KDOP18][GEOM_OCTREE] = &Collide<KDOP<18> >;
+ collision_matrix[BV_KDOP24][GEOM_OCTREE] = &Collide<KDOP<24> >;
#endif
}
//template struct CollisionFunctionMatrix;
diff --git a/src/traits_traversal.h b/src/traits_traversal.h
index 312e27d5..f67db364 100644
--- a/src/traits_traversal.h
+++ b/src/traits_traversal.h
@@ -46,6 +46,7 @@ namespace hpp
{
namespace fcl
{
+
template <typename TypeA, typename TypeB>
struct TraversalTraits
{
@@ -54,31 +55,43 @@ struct TraversalTraits
template <typename T_SH>
struct TraversalTraits <T_SH, OcTree>
{
- typedef ShapeOcTreeCollisionTraversalNode<T_SH, GJKSolver> CollisionTraversal_t;
+ typedef ShapeOcTreeCollisionTraversalNode<T_SH> CollisionTraversal_t;
};
template <typename T_SH>
struct TraversalTraits <OcTree, T_SH>
{
- typedef OcTreeShapeCollisionTraversalNode<T_SH, GJKSolver> CollisionTraversal_t;
+ typedef OcTreeShapeCollisionTraversalNode<T_SH> CollisionTraversal_t;
};
template <>
struct TraversalTraits <OcTree, OcTree>
{
- typedef OcTreeCollisionTraversalNode<GJKSolver> CollisionTraversal_t;
+ typedef OcTreeCollisionTraversalNode<> CollisionTraversal_t;
};
template <typename T_BVH>
struct TraversalTraits <OcTree, BVHModel<T_BVH>>
{
- typedef OcTreeMeshCollisionTraversalNode<T_BVH, GJKSolver> CollisionTraversal_t;
+ typedef OcTreeMeshCollisionTraversalNode<T_BVH> CollisionTraversal_t;
};
template <typename T_BVH>
struct TraversalTraits <BVHModel<T_BVH>, OcTree>
{
- typedef MeshOcTreeCollisionTraversalNode<T_BVH, GJKSolver> CollisionTraversal_t;
+ typedef MeshOcTreeCollisionTraversalNode<T_BVH> CollisionTraversal_t;
+};
+
+template <typename OrientedMeshCollisionTraversalNode, typename T_BVH, typename T_SH>
+struct TraversalTraits <BVHModel<T_BVH>, T_SH>
+{
+ typedef OrientMeshShapeCollisionTraveralNode<> CollisionTraversal_t;
+};
+
+template <typename OrientedMeshCollisionTraversalNode, typename T_BVH>
+struct TraversalTraits <BVHModel<T_BVH>, BVHModel<T_BVH>>
+{
+ typedef OrientedMeshCollisionTraversalNode<> CollisionTraversal_t;
};
--
GitLab