diff --git a/include/hpp/fcl/traversal/traversal_node_base.h b/include/hpp/fcl/traversal/traversal_node_base.h
index 81e1d61c8e6f6419c34787a4c6a92def8b5d4699..918505de5e55d4f0678e8dad64603c96378559ba 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/include/hpp/fcl/traversal/traversal_node_base.h
@@ -137,6 +137,8 @@ public:
virtual ~DistanceTraversalNodeBase();
/// @brief BV test between b1 and b2
+ /// \return a lower bound of the distance between the two BV.
+ /// \note except for OBB, this method returns the distance.
virtual FCL_REAL BVTesting(int b1, int b2) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
diff --git a/include/hpp/fcl/traversal/traversal_node_bvhs.h b/include/hpp/fcl/traversal/traversal_node_bvhs.h
index 972955e0f6f74ef0d69ba6509f77c82ccdf24228..82a7dfef85893b3bea481bc27295acd8614371fe 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvhs.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvhs.h
@@ -298,6 +298,32 @@ public:
Vec3f T;
};
+namespace details
+{
+ template<typename BV> struct DistanceTraversalBVTesting_impl
+ {
+ static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2)
+ {
+ return b1.distance(b2);
+ }
+ };
+
+ template<> struct DistanceTraversalBVTesting_impl<OBB>
+ {
+ static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2)
+ {
+ FCL_REAL sqrDistLowerBound;
+ CollisionRequest request (DISTANCE_LOWER_BOUND, 0);
+ // request.break_distance = ?
+ if (b1.overlap(b2, request, sqrDistLowerBound)) {
+ // TODO A penetration upper bound should be computed.
+ return -1;
+ }
+ return sqrt (sqrDistLowerBound);
+ }
+ };
+} // namespace details
+
/// @brief Traversal node for distance computation between BVH models
template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
@@ -367,7 +393,8 @@ public:
FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
- return model1->getBV(b1).distance(model2->getBV(b2));
+ return details::DistanceTraversalBVTesting_impl<BV>
+ ::run (model1->getBV(b1), model2->getBV(b2));
}
/// @brief The first BVH model
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index c83148c07e5bc412692f9eef213e3f9391d78c3b..423d8d62e52306a70bf19559e5a5020479ee8c30 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -379,6 +379,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer<AABB, Halfspace, NarrowPhaseSolver>::distance;
+ */
distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
@@ -388,7 +389,6 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer<OBB, Halfspace, NarrowPhaseSolver>::distance;
- */
distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
@@ -448,6 +448,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer<OBBRSS, Halfspace, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
+ distance_matrix[BV_OBB][BV_OBB] = &BVHDistance<OBB, NarrowPhaseSolver>;
distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp
index 192a9508120dabf0dd6eacd146477c1b484246ed..35eb58408d061fbafbed6f6f8ff60ad9ffebfae8 100644
--- a/test/test_fcl_distance.cpp
+++ b/test/test_fcl_distance.cpp
@@ -198,7 +198,36 @@ BOOST_AUTO_TEST_CASE(mesh_distance)
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
+ distance_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
+
+ BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
+ BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
+
distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);