Add distance computation for OBB based BVH.

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

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

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>;

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);