diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 82a7dfef85893b3bea481bc27295acd8614371fe..71dafc1f86dd815c74867631b3ba357c2db73c02 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -59,6 +59,10 @@ namespace hpp
{
namespace fcl
{
+enum {
+ RelativeTransformationIsIdentity = 1
+};
+
/// @brief Traversal node for collision between BVH models
template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
@@ -124,24 +128,6 @@ public:
{
return model2->getBV(b).rightChild();
}
-
- /// @brief BV culling test in one BVTT node
- bool BVTesting(int b1, int b2) const
- {
- if(enable_statistics) num_bv_tests++;
- return !model1->getBV(b1).overlap(model2->getBV(b2));
- }
-
- /// BV test between b1 and b2
- /// \param b1, b2 Bounding volumes to test,
- /// \retval sqrDistLowerBound square of a lower bound of the minimal
- /// distance between bounding volumes.
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
- {
- if(enable_statistics) num_bv_tests++;
- return !model1->getBV(b1).overlap(model2->getBV(b2), request,
- sqrDistLowerBound);
- }
/// @brief The first BVH model
const BVHModel<BV>* model1;
@@ -154,12 +140,38 @@ public:
mutable FCL_REAL query_time_seconds;
};
+namespace details
+{
+ template <bool enabled>
+ struct RelativeTransformation
+ {
+ RelativeTransformation () : R (Matrix3f::Identity()) {}
+
+ const Matrix3f& _R () const { return R; }
+ const Vec3f & _T () const { return T; }
+
+ Matrix3f R;
+ Vec3f T;
+ };
+
+ template <>
+ struct RelativeTransformation <false>
+ {
+ static const Matrix3f& _R () { throw std::logic_error ("should never reach this point"); }
+ static const Vec3f & _T () { throw std::logic_error ("should never reach this point"); }
+ };
+} // namespace details
/// @brief Traversal node for collision between two meshes
-template<typename BV>
+template<typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
+ enum {
+ Options = _Options,
+ RTIsIdentity = _Options & RelativeTransformationIsIdentity
+ };
+
MeshCollisionTraversalNode(const CollisionRequest& request) :
BVHCollisionTraversalNode<BV> (request)
{
@@ -169,6 +181,36 @@ public:
tri_indices2 = NULL;
}
+ /// @brief BV culling test in one BVTT node
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ if (RTIsIdentity)
+ return !this->model1->getBV(b1).overlap(this->model2->getBV(b2));
+ else
+ return !overlap(RT._R(), RT._T(),
+ this->model1->getBV(b1).bv, this->model2->getBV(b2).bv);
+ }
+
+ /// BV test between b1 and b2
+ /// \param b1, b2 Bounding volumes to test,
+ /// \retval sqrDistLowerBound square of a lower bound of the minimal
+ /// distance between bounding volumes.
+ bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ if (RTIsIdentity)
+ return !this->model1->getBV(b1).overlap(this->model2->getBV(b2),
+ this->request, sqrDistLowerBound);
+ else {
+ bool res = !overlap(RT._R(), RT._T(),
+ this->model1->getBV(b1).bv, this->model2->getBV(b2).bv,
+ this->request, sqrDistLowerBound);
+ assert (!res || sqrDistLowerBound > 0);
+ return res;
+ }
+ }
+
/// Intersection testing between leaves (two triangles)
///
/// \param b1, b2 id of primitive in bounding volume hierarchy
@@ -242,61 +284,16 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
-};
-
-
-/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
-class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
-{
-public:
- MeshCollisionTraversalNodeOBB (const CollisionRequest& request);
-
- bool BVTesting(int b1, int b2) const;
-
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
- Matrix3f R;
- Vec3f T;
+ details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
-class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
-{
-public:
- MeshCollisionTraversalNodeRSS (const CollisionRequest& request);
-
- bool BVTesting(int b1, int b2) const;
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
-{
-public:
- MeshCollisionTraversalNodekIOS (const CollisionRequest& request);
-
- bool BVTesting(int b1, int b2) const;
-
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
-{
-public:
- MeshCollisionTraversalNodeOBBRSS (const CollisionRequest& request);
-
- bool BVTesting(int b1, int b2) const;
-
- bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
-
- Matrix3f R;
- Vec3f T;
-};
+/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
+typedef MeshCollisionTraversalNode<OBB , 0> MeshCollisionTraversalNodeOBB ;
+typedef MeshCollisionTraversalNode<RSS , 0> MeshCollisionTraversalNodeRSS ;
+typedef MeshCollisionTraversalNode<kIOS , 0> MeshCollisionTraversalNodekIOS ;
+typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace details
{
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index d241780e58bd83bf0a4ecc8fe6832dbdb5f8480a..5fe97fa5a0eaf8b892e77971b24ca2854e8b02d2 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -86,92 +86,6 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
}
} // namespace details
-MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
-(const CollisionRequest& request) :
- MeshCollisionTraversalNode<OBB> (request)
-{
- R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodeOBB::BVTesting
-(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
- request, sqrDistLowerBound);
-}
-
-MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
-(const CollisionRequest& request) :
- MeshCollisionTraversalNode<RSS> (request)
-{
- R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2,
- FCL_REAL& sqrDistLowerBound) const
-{
- if(enable_statistics) num_bv_tests++;
- sqrDistLowerBound = 0;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
-(const CollisionRequest& request) :
- MeshCollisionTraversalNode<kIOS>(request)
-{
- R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-bool MeshCollisionTraversalNodekIOS::BVTesting
-(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
-{
- if(enable_statistics) num_bv_tests++;
- sqrDistLowerBound = 0;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
-(const CollisionRequest& request) :
- MeshCollisionTraversalNode<OBBRSS> (request)
-{
- R.setIdentity();
-}
-
-bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
- bool MeshCollisionTraversalNodeOBBRSS::BVTesting
- (int b1, int b2, FCL_REAL& sqrDistLowerBound) const
- {
- if(enable_statistics) num_bv_tests++;
- bool res (!overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv,
- request, sqrDistLowerBound));
- assert (!res || sqrDistLowerBound > 0);
- return res;
- }
-
namespace details
{
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 321ca128f8f2b6028b7b14981f5b59c1288b8169..7275286062ab008bbba9dcb3abbb7f5d3053bb87 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -68,7 +68,7 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.result = &result;
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
+ relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.RT.R, node.RT.T);
return true;
}