Return the correct nearest points from distance(...)

.appveyor.yml

@@ -16,7 +16,7 @@ environment:
   CTEST_OUTPUT_ON_FAILURE: 1
 
 cache:
-  - C:\Program Files\libccd
+  - C:\Program Files\libccd -> .appveyor.yml
 
 configuration:
   - Debug

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

@@ -1005,6 +1005,152 @@ static int __ccdEPA(const void *obj1, const void *obj2,
     return 0;
 }
 
+/** Returns the points p1 and p2 on the original shapes that correspond to point
+ * p in the given simplex.*/
+static void extractClosestPoints(ccd_simplex_t* simplex,
+                                 ccd_vec3_t* p1, ccd_vec3_t* p2, ccd_vec3_t* p)
+{
+  int simplex_size = ccdSimplexSize(simplex);
+  assert(simplex_size <= 3);
+  if (simplex_size == 1)
+  {
+      // Closest points are the ones stored in the simplex
+      if(p1) *p1 = simplex->ps[simplex->last].v1;
+      if(p2) *p2 = simplex->ps[simplex->last].v2;
+  }
+  else if (simplex_size ==2)
+  {
+      // Closest points lie on the segment defined by the points in the simplex
+      // Let the segment be defined by points A and B. We can write p as
+      //
+      // p = A + s*AB, 0 <= s <= 1
+      // p - A = s*AB
+      ccd_vec3_t AB;
+      ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v));
+
+      // This defines three equations, but we only need one. Taking the i-th
+      // component gives
+      //
+      // p_i - A_i = s*AB_i.
+      //
+      // Thus, s is given by
+      //
+      // s = (p_i - A_i)/AB_i.
+      //
+      // To avoid dividing by an AB_i ≪ 1, we choose i such that |AB_i| is
+      // maximized
+      ccd_real_t abs_AB_x{std::abs(ccdVec3X(&AB))};
+      ccd_real_t abs_AB_y{std::abs(ccdVec3Y(&AB))};
+      ccd_real_t abs_AB_z{std::abs(ccdVec3Z(&AB))};
+      ccd_real_t s{0};
+
+      if (abs_AB_x >= abs_AB_y && abs_AB_x >= abs_AB_z) {
+        ccd_real_t A_x{ccdVec3X(&(simplex->ps[0].v))};
+        ccd_real_t AB_x{ccdVec3X(&AB)};
+        ccd_real_t p_x{ccdVec3X(p)};
+        s = (p_x - A_x) / AB_x;
+      } else if (abs_AB_y >= abs_AB_z) {
+        ccd_real_t A_y{ccdVec3Y(&(simplex->ps[0].v))};
+        ccd_real_t AB_y{ccdVec3Y(&AB)};
+        ccd_real_t p_y{ccdVec3Y(p)};
+        s = (p_y - A_y) / AB_y;
+      } else {
+        ccd_real_t A_z{ccdVec3Z(&(simplex->ps[0].v))};
+        ccd_real_t AB_z{ccdVec3Z(&AB)};
+        ccd_real_t p_z{ccdVec3Z(p)};
+        s = (p_z - A_z) / AB_z;
+      }
+
+      if (p1)
+      {
+        // p1 = A1 + s*A1B1
+        ccd_vec3_t sAB;
+        ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1));
+        ccdVec3Scale(&sAB, s);
+        ccdVec3Copy(p1, &(simplex->ps[0].v1));
+        ccdVec3Add(p1, &sAB);
+      }
+      if (p2)
+      {
+        // p2 = A2 + s*A2B2
+        ccd_vec3_t sAB;
+        ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2));
+        ccdVec3Scale(&sAB, s);
+        ccdVec3Copy(p2, &(simplex->ps[0].v2));
+        ccdVec3Add(p2, &sAB);
+      }
+  }
+  else // simplex_size == 3
+  {
+    // Closest points lie in the triangle defined by the points in the simplex
+    ccd_vec3_t AB, AC, n, v_prime, AB_cross_v_prime, AC_cross_v_prime;
+    // Let the triangle be defined by points A, B, and C. The triangle lies in
+    // the plane that passes through A, whose normal is given by n̂, where
+    //
+    // n = AB × AC
+    // n̂ = n / ‖n‖
+    ccdVec3Sub2(&AB, &(simplex->ps[1].v), &(simplex->ps[0].v));
+    ccdVec3Sub2(&AC, &(simplex->ps[2].v), &(simplex->ps[0].v));
+    ccdVec3Cross(&n, &AB, &AC);
+
+    // Since p lies in ABC, it can be expressed as
+    //
+    // p = A + p'
+    // p' = s*AB + t*AC
+    //
+    // where 0 <= s, t, s+t <= 1.
+    ccdVec3Sub2(&v_prime, p, &(simplex->ps[0].v));
+
+    // To find the corresponding v1 and v2, we need
+    // values for s and t. Taking cross products with AB and AC gives the
+    // following system:
+    //
+    // AB × p' =  t*(AB × AC) =  t*n
+    // AC × p' = -s*(AB × AC) = -s*n
+    ccdVec3Cross(&AB_cross_v_prime, &AB, &v_prime);
+    ccdVec3Cross(&AC_cross_v_prime, &AC, &v_prime);
+
+    // To convert this to a system of scalar equations, we take the dot product
+    // with n:
+    //
+    // n ⋅ (AB × p') =  t * ‖n‖²
+    // n ⋅ (AC × p') = -s * ‖n‖²
+    ccd_real_t norm_squared_n{ccdVec3Len2(&n)};
+
+    // Therefore, s and t are given by
+    //
+    // s = -n ⋅ (AC × p') / ‖n‖²
+    // t =  n ⋅ (AB × p') / ‖n‖²
+    ccd_real_t s{-ccdVec3Dot(&n, &AC_cross_v_prime) / norm_squared_n};
+    ccd_real_t t{ccdVec3Dot(&n, &AB_cross_v_prime) / norm_squared_n};
+
+    if (p1)
+    {
+      // p1 = A1 + s*A1B1 + t*A1C1
+      ccd_vec3_t sAB, tAC;
+      ccdVec3Sub2(&sAB, &(simplex->ps[1].v1), &(simplex->ps[0].v1));
+      ccdVec3Scale(&sAB, s);
+      ccdVec3Sub2(&tAC, &(simplex->ps[2].v1), &(simplex->ps[0].v1));
+      ccdVec3Scale(&tAC, t);
+      ccdVec3Copy(p1, &(simplex->ps[0].v1));
+      ccdVec3Add(p1, &sAB);
+      ccdVec3Add(p1, &tAC);
+    }
+    if (p2)
+    {
+      // p2 = A2 + s*A2B2 + t*A2C2
+      ccd_vec3_t sAB, tAC;
+      ccdVec3Sub2(&sAB, &(simplex->ps[1].v2), &(simplex->ps[0].v2));
+      ccdVec3Scale(&sAB, s);
+      ccdVec3Sub2(&tAC, &(simplex->ps[2].v2), &(simplex->ps[0].v2));
+      ccdVec3Scale(&tAC, t);
+      ccdVec3Copy(p2, &(simplex->ps[0].v2));
+      ccdVec3Add(p2, &sAB);
+      ccdVec3Add(p2, &tAC);
+    }
+  }
+}
+
 
 static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
                                   const ccd_t *ccd,
@@ -1054,8 +1200,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
     // check whether we improved for at least a minimum tolerance
     if ((last_dist - dist) < ccd->dist_tolerance)
     {
-      if(p1) *p1 = last.v1;
-      if(p2) *p2 = last.v2;
+      extractClosestPoints(simplex, p1, p2, &dir);
       return dist;
     }
 
@@ -1076,8 +1221,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
     dist = CCD_SQRT(dist);
     if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
     {
-      if(p1) *p1 = last.v1;
-      if(p2) *p2 = last.v2;
+      extractClosestPoints(simplex, p1, p2, &dir);
       return last_dist;
     }
 
@@ -1228,8 +1372,7 @@ static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const c
     // check whether we improved for at least a minimum tolerance
     if ((last_dist - dist) < ccd->dist_tolerance)
     {
-      if(p1) *p1 = last.v1;
-      if(p2) *p2 = last.v2;
+      extractClosestPoints(&simplex, p1, p2, &dir);
       return dist;
     }
 
@@ -1250,8 +1393,7 @@ static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const c
     dist = CCD_SQRT(dist);
     if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
     {
-      if(p1) *p1 = last.v1;
-      if(p2) *p2 = last.v2;
+      extractClosestPoints(&simplex, p1, p2, &dir);
       return last_dist;
     }
 

include/fcl/narrowphase/detail/gjk_solver_indep-inl.h

@@ -620,7 +620,7 @@ struct ShapeDistanceIndepImpl
       if(distance) *distance = (w0 - w1).norm();
 
       if(p1) *p1 = w0;
-      if(p2) (*p2).noalias() = shape.toshape0 * w1;
+      if(p2) (*p2).noalias() = shape.toshape0.inverse() * w1;
 
       return true;
     }

include/fcl/narrowphase/distance-inl.h

@@ -205,11 +205,13 @@ S distance(
   case GST_LIBCCD:
     {
       detail::GJKSolver_libccd<S> solver;
+      solver.distance_tolerance = request.distance_tolerance;
       return distance(o1, o2, &solver, request, result);
     }
   case GST_INDEP:
     {
       detail::GJKSolver_indep<S> solver;
+      solver.gjk_tolerance = request.distance_tolerance;
       return distance(o1, o2, &solver, request, result);
     }
   default:
@@ -229,11 +231,13 @@ S distance(
   case GST_LIBCCD:
     {
       detail::GJKSolver_libccd<S> solver;
+      solver.distance_tolerance = request.distance_tolerance;
       return distance(o1, tf1, o2, tf2, &solver, request, result);
     }
   case GST_INDEP:
     {
       detail::GJKSolver_indep<S> solver;
+      solver.gjk_tolerance = request.distance_tolerance;
       return distance(o1, tf1, o2, tf2, &solver, request, result);
     }
   default:

include/fcl/narrowphase/distance_request-inl.h

@@ -56,11 +56,13 @@ DistanceRequest<S>::DistanceRequest(
     bool enable_signed_distance_,
     S rel_err_,
     S abs_err_,
+    S distance_tolerance_,
     GJKSolverType gjk_solver_type_)
   : enable_nearest_points(enable_nearest_points_),
     enable_signed_distance(enable_signed_distance_),
     rel_err(rel_err_),
     abs_err(abs_err_),
+    distance_tolerance(distance_tolerance_),
     gjk_solver_type(gjk_solver_type_)
 {
   // Do nothing

include/fcl/narrowphase/distance_request.h

@@ -95,6 +95,9 @@ struct DistanceRequest
   S rel_err; // relative error, between 0 and 1
   S abs_err; // absoluate error
 
+  /// @brief the threshold used in GJK algorithm to stop distance iteration
+  S distance_tolerance;
+
   /// @brief narrow phase solver type
   GJKSolverType gjk_solver_type;
 
@@ -103,6 +106,7 @@ struct DistanceRequest
       bool enable_signed_distance = false,
       S rel_err_ = 0.0,
       S abs_err_ = 0.0,
+      S distance_tolerance = 1e-6,
       GJKSolverType gjk_solver_type_ = GST_LIBCCD);
 
   bool isSatisfied(const DistanceResult<S>& result) const;

test/test_fcl_capsule_box_1.cpp

@@ -37,12 +37,13 @@
 #include <gtest/gtest.h>
 
 #include <cmath>
+#include <limits>
 #include "fcl/narrowphase/distance.h"
 #include "fcl/narrowphase/collision.h"
 #include "fcl/narrowphase/collision_object.h"
 
 template <typename S>
-void test_distance_capsule_box()
+void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_tolerance, S test_tolerance)
 {
   using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
 
@@ -55,6 +56,9 @@ void test_distance_capsule_box()
   fcl::DistanceRequest<S> distanceRequest (true);
   fcl::DistanceResult<S> distanceResult;
 
+  distanceRequest.gjk_solver_type = solver_type;
+  distanceRequest.distance_tolerance = solver_tolerance;
+
   fcl::Transform3<S> tf1(fcl::Translation3<S>(fcl::Vector3<S> (3., 0, 0)));
   fcl::Transform3<S> tf2 = fcl::Transform3<S>::Identity();
   fcl::CollisionObject<S> capsule (capsuleGeometry, tf1);
@@ -66,11 +70,11 @@ void test_distance_capsule_box()
   fcl::Vector3<S> o1 (distanceResult.nearest_points [0]);
   // Nearest point on box
   fcl::Vector3<S> o2 (distanceResult.nearest_points [1]);
-  EXPECT_NEAR (distanceResult.min_distance, 0.5, 1e-4);
-  EXPECT_NEAR (o1 [0], -2.0, 1e-4);
-  EXPECT_NEAR (o1 [1],  0.0, 1e-4);
-  EXPECT_NEAR (o2 [0],  0.5, 1e-4);
-  EXPECT_NEAR (o1 [1],  0.0, 1e-4); // TODO(JS): maybe o2 rather than o1?
+  EXPECT_NEAR (distanceResult.min_distance, 0.5, test_tolerance);
+  EXPECT_NEAR (o1 [0], -2.0, test_tolerance);
+  EXPECT_NEAR (o1 [1],  0.0, test_tolerance);
+  EXPECT_NEAR (o2 [0],  0.5, test_tolerance);
+  EXPECT_NEAR (o2 [1],  0.0, test_tolerance);
 
   // Move capsule above box
   tf1 = fcl::Translation3<S>(fcl::Vector3<S> (0., 0., 8.));
@@ -82,15 +86,14 @@ void test_distance_capsule_box()
   o1 = distanceResult.nearest_points [0];
   o2 = distanceResult.nearest_points [1];
 
-  EXPECT_NEAR (distanceResult.min_distance, 2.0, 1e-4);
-  EXPECT_NEAR (o1 [0],  0.0, 1e-4);
-  EXPECT_NEAR (o1 [1],  0.0, 1e-4);
-  EXPECT_NEAR (o1 [2], -4.0, 1e-4);
+  EXPECT_NEAR (distanceResult.min_distance, 2.0, test_tolerance);
+  EXPECT_NEAR (o1 [0],  0.0, test_tolerance);
+  EXPECT_NEAR (o1 [1],  0.0, test_tolerance);
+  EXPECT_NEAR (o1 [2], -4.0, test_tolerance);
 
-  // Disabled broken test lines. Please see #25.
-  // CHECK_CLOSE_TO_0 (o2 [0], 1e-4);
-  EXPECT_NEAR (o2 [1],  0.0, 1e-4);
-  EXPECT_NEAR (o2 [2],  2.0, 1e-4);
+  EXPECT_NEAR (o2 [0],  0.0, test_tolerance);
+  EXPECT_NEAR (o2 [1],  0.0, test_tolerance);
+  EXPECT_NEAR (o2 [2],  2.0, test_tolerance);
 
   // Rotate capsule around y axis by pi/2 and move it behind box
   tf1.translation() = fcl::Vector3<S>(-10., 0., 0.);
@@ -103,19 +106,23 @@ void test_distance_capsule_box()
   o1 = distanceResult.nearest_points [0];
   o2 = distanceResult.nearest_points [1];
 
-  EXPECT_NEAR (distanceResult.min_distance, 5.5, 1e-4);
-  EXPECT_NEAR (o1 [0],  0.0, 1e-4);
-  EXPECT_NEAR (o1 [1],  0.0, 1e-4);
-  EXPECT_NEAR (o1 [2],  4.0, 1e-4);
-  EXPECT_NEAR (o2 [0], -0.5, 1e-4);
-  EXPECT_NEAR (o2 [1],  0.0, 1e-4);
-  EXPECT_NEAR (o2 [2],  0.0, 1e-4);
+  EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance);
+  EXPECT_NEAR (o1 [0],  0.0, test_tolerance);
+  EXPECT_NEAR (o1 [1],  0.0, test_tolerance);
+  EXPECT_NEAR (o1 [2],  4.0, test_tolerance);
+  EXPECT_NEAR (o2 [0], -0.5, test_tolerance);
+  EXPECT_NEAR (o2 [1],  0.0, test_tolerance);
+  EXPECT_NEAR (o2 [2],  0.0, test_tolerance);
+}
+
+GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
+{
+  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-6, 1e-4);
 }
 
-GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box)
+GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_indep)
 {
-//  test_distance_capsule_box<float>();
-  test_distance_capsule_box<double>();
+  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_INDEP, 1e-8, 1e-4);
 }
 
 //==============================================================================