Clean traversal node setup.

include/hpp/fcl/traversal/traversal_node_setup.h

@@ -548,7 +548,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
 
 /// @brief Initialize traversal node for collision between two meshes, given the current transforms
 template<typename BV>
-bool initialize(MeshCollisionTraversalNode<BV>& node,
+bool initialize(MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>& node,
                 BVHModel<BV>& model1, Transform3f& tf1,
                 BVHModel<BV>& model2, Transform3f& tf2,
                 CollisionResult& result,
@@ -607,31 +607,33 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
   return true;
 }
 
+template<typename BV>
+bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
+                const BVHModel<BV>& model1, const Transform3f& tf1,
+                const BVHModel<BV>& model2, const Transform3f& tf2,
+                CollisionResult& result)
+{
+  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
+    return false;
 
-/// @brief Initialize traversal node for collision between two meshes, specialized for OBB type
-bool initialize(MeshCollisionTraversalNodeOBB& node,
-                const BVHModel<OBB>& model1, const Transform3f& tf1,
-                const BVHModel<OBB>& model2, const Transform3f& tf2,
-                CollisionResult& result);
+  node.vertices1 = model1.vertices;
+  node.vertices2 = model2.vertices;
 
-/// @brief Initialize traversal node for collision between two meshes, specialized for RSS type
-bool initialize(MeshCollisionTraversalNodeRSS& node,
-                const BVHModel<RSS>& model1, const Transform3f& tf1,
-                const BVHModel<RSS>& model2, const Transform3f& tf2,
-                CollisionResult& result);
+  node.tri_indices1 = model1.tri_indices;
+  node.tri_indices2 = model2.tri_indices;
 
-/// @brief Initialize traversal node for collision between two meshes, specialized for OBBRSS type
-bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
-                const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
-                const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
-                CollisionResult& result);
+  node.model1 = &model1;
+  node.tf1 = tf1;
+  node.model2 = &model2;
+  node.tf2 = tf2;
 
-/// @brief Initialize traversal node for collision between two meshes, specialized for kIOS type
-bool initialize(MeshCollisionTraversalNodekIOS& node,
-                const BVHModel<kIOS>& model1, const Transform3f& tf1,
-                const BVHModel<kIOS>& model2, const Transform3f& tf2,
-                CollisionResult& result);
+  node.result = &result;
 
+  node.RT.R = tf1.getRotation().transpose() * tf2.getRotation();
+  node.RT.T = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
+
+  return true;
+}
 
 /// @brief Initialize traversal node for distance between two geometric shapes
 template<typename S1, typename S2, typename NarrowPhaseSolver>

src/traversal/traversal_node_setup.cpp

@@ -43,74 +43,6 @@ namespace hpp
 namespace fcl
 {
 
-
-namespace details
-{
-template<typename BV, typename OrientedNode>
-static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
-                                                  const BVHModel<BV>& model1, const Transform3f& tf1,
-                                                  const BVHModel<BV>& model2, const Transform3f& tf2,
-                                                  CollisionResult& result)
-{
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    return false;
-
-  node.vertices1 = model1.vertices;
-  node.vertices2 = model2.vertices;
-
-  node.tri_indices1 = model1.tri_indices;
-  node.tri_indices2 = model2.tri_indices;
-
-  node.model1 = &model1;
-  node.tf1 = tf1;
-  node.model2 = &model2;
-  node.tf2 = tf2;
-
-  node.result = &result;
-
-  relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.RT.R, node.RT.T);
-
-  return true;
-}
-
-}
-
-
-bool initialize(MeshCollisionTraversalNodeOBB& node,
-                const BVHModel<OBB>& model1, const Transform3f& tf1,
-                const BVHModel<OBB>& model2, const Transform3f& tf2,
-                CollisionResult& result)
-{
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
-}
-
-
-bool initialize(MeshCollisionTraversalNodeRSS& node,
-                const BVHModel<RSS>& model1, const Transform3f& tf1,
-                const BVHModel<RSS>& model2, const Transform3f& tf2,
-                CollisionResult& result)
-{
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
-}
-
-
-bool initialize(MeshCollisionTraversalNodekIOS& node,
-                const BVHModel<kIOS>& model1, const Transform3f& tf1,
-                const BVHModel<kIOS>& model2, const Transform3f& tf2,
-                CollisionResult& result)
-{
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
-}
-
-bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
-                const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
-                const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
-                CollisionResult& result)
-{
-  return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, result);
-}
-
-
 namespace details
 {
 template<typename BV, typename OrientedNode>