From c76bc1e90a016b9eb4159ac7612cf752e0959ecf Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 27 Aug 2019 15:59:03 +0200
Subject: [PATCH] Clean traversal node setup.
---
.../hpp/fcl/traversal/traversal_node_setup.h | 44 ++++++------
src/traversal/traversal_node_setup.cpp | 68 -------------------
2 files changed, 23 insertions(+), 89 deletions(-)
diff --git a/include/hpp/fcl/traversal/traversal_node_setup.h b/include/hpp/fcl/traversal/traversal_node_setup.h
index 041b3fa6..0912c231 100644
--- a/include/hpp/fcl/traversal/traversal_node_setup.h
+++ b/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>
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 72752860..862a3c51 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/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>
--
GitLab