Fix "dir" in GJKDistance function

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

@@ -1010,7 +1010,7 @@ static int __ccdEPA(const void *obj1, const void *obj2,
 static void extractClosestPoints(ccd_simplex_t* simplex,
                                  ccd_vec3_t* p1, ccd_vec3_t* p2, ccd_vec3_t* p)
 {
-  int simplex_size = ccdSimplexSize(simplex);
+  const int simplex_size = ccdSimplexSize(simplex);
   assert(simplex_size <= 3);
   if (simplex_size == 1)
   {
@@ -1151,27 +1151,41 @@ static void extractClosestPoints(ccd_simplex_t* simplex,
   }
 }
 
-
+// Computes the distance between two non-penetrating convex objects, `obj1` and
+// `obj2` based on the warm-started witness simplex, returning the distance and
+// nearest points on each object.
+// @param obj1 The first convex object.
+// @param obj2 The second convex object.
+// @param ccd The libccd configuration.
+// @param simplex A witness to the objects' separation generated by the GJK
+// algorithm. NOTE: the simplex is not necessarily sufficiently refined to
+// report the actual distance and may be further refined in this method.
+// @param p1 If the objects are non-penetrating, the point on the surface of
+// obj1 closest to obj2 (expressed in the world frame).
+// @param p2 If the objects are non-penetrating, the point on the surface of
+// obj2 closest to obj1 (expressed in the world frame).
+// @returns The minimum distance between the two objects. If they are
+// penetrating, -1 is returned.
 static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
                                   const ccd_t *ccd,
                                   ccd_simplex_t* simplex,
                                   ccd_vec3_t* p1, ccd_vec3_t* p2)
 {
-  unsigned long iterations;
-  ccd_support_t last; // last support point
-  ccd_vec3_t dir; // direction vector
-  ccd_real_t dist, last_dist = CCD_REAL_MAX;
+  ccd_real_t last_dist = CCD_REAL_MAX;
 
-  for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
-  {
+  for (unsigned long iterations = 0UL; iterations < ccd->max_iterations;
+       ++iterations) {
+    ccd_vec3_t closest_p; // The point on the simplex that is closest to the
+                          // origin.
+    ccd_real_t dist;
     // get a next direction vector
     // we are trying to find out a point on the minkowski difference
     // that is nearest to the origin, so we obtain a point on the
     // simplex that is nearest and try to exapand the simplex towards
     // the origin
     if (ccdSimplexSize(simplex) == 1)
     {
-      ccdVec3Copy(&dir, &ccdSimplexPoint(simplex, 0)->v);
+      ccdVec3Copy(&closest_p, &ccdSimplexPoint(simplex, 0)->v);
       dist = ccdVec3Len2(&ccdSimplexPoint(simplex, 0)->v);
       dist = CCD_SQRT(dist);
     }
@@ -1180,7 +1194,7 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
       dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
                                       &ccdSimplexPoint(simplex, 0)->v,
                                       &ccdSimplexPoint(simplex, 1)->v,
-                                      &dir);
+                                      &closest_p);
       dist = CCD_SQRT(dist);
     }
     else if(ccdSimplexSize(simplex) == 3)
@@ -1189,26 +1203,29 @@ static inline ccd_real_t _ccdDist(const void *obj1, const void *obj2,
                                   &ccdSimplexPoint(simplex, 0)->v,
                                   &ccdSimplexPoint(simplex, 1)->v,
                                   &ccdSimplexPoint(simplex, 2)->v,
-                                  &dir);
+                                  &closest_p);
       dist = CCD_SQRT(dist);
     }
     else
     { // ccdSimplexSize(&simplex) == 4
-      dist = simplexReduceToTriangle(simplex, last_dist, &dir);
+      dist = simplexReduceToTriangle(simplex, last_dist, &closest_p);
     }
 
     // check whether we improved for at least a minimum tolerance
     if ((last_dist - dist) < ccd->dist_tolerance)
     {
-      extractClosestPoints(simplex, p1, p2, &dir);
+      extractClosestPoints(simplex, p1, p2, &closest_p);
       return dist;
     }
 
     // point direction towards the origin
+    ccd_vec3_t dir; // direction vector
+    ccdVec3Copy(&dir, &closest_p);
     ccdVec3Scale(&dir, -CCD_ONE);
     ccdVec3Normalize(&dir);
 
     // find out support point
+    ccd_support_t last; // last support point
     __ccdSupport(obj1, obj2, &dir, ccd, &last);
 
     // record last distance
@@ -1221,7 +1238,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)
     {
-      extractClosestPoints(simplex, p1, p2, &dir);
+      extractClosestPoints(simplex, p1, p2, &closest_p);
       return last_dist;
     }
 
@@ -1327,81 +1344,28 @@ static inline ccd_real_t ccdGJKSignedDist(const void* obj1, const void* obj2, co
 }
 
 
-/// change the libccd distance to add two closest points
+// Computes the distance between two non-penetrating convex objects, `obj1` and
+// `obj2`, returning the distance and
+// nearest points on each object.
+// @param obj1 The first convex object.
+// @param obj2 The second convex object.
+// @param ccd The libccd configuration.
+// @param p1 If the objects are non-penetrating, the point on the surface of
+// obj1 closest to obj2 (expressed in the world frame).
+// @param p2 If the objects are non-penetrating, the point on the surface of
+// obj2 closest to obj1 (expressed in the world frame).
+// @returns The minimum distance between the two objects. If they are
+// penetrating, -1 is returned.
+// @note Unlike _ccdDist function, this function does not need a warm-started
+// simplex as the input argument.
 static inline ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2)
 {
-  unsigned long iterations;
   ccd_simplex_t simplex;
-  ccd_support_t last; // last support point
-  ccd_vec3_t dir; // direction vector
-  ccd_real_t dist, last_dist;
-
   // first find an intersection
   if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0)
     return -CCD_ONE;
 
-  last_dist = CCD_REAL_MAX;
-
-  for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) {
-    // get a next direction vector
-    // we are trying to find out a point on the minkowski difference
-    // that is nearest to the origin, so we obtain a point on the
-    // simplex that is nearest and try to exapand the simplex towards
-    // the origin
-    if (ccdSimplexSize(&simplex) == 1){
-      ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v);
-      dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v);
-      dist = CCD_SQRT(dist);
-    }else if (ccdSimplexSize(&simplex) == 2){
-      dist = ccdVec3PointSegmentDist2(ccd_vec3_origin,
-                                      &ccdSimplexPoint(&simplex, 0)->v,
-                                      &ccdSimplexPoint(&simplex, 1)->v,
-                                      &dir);
-      dist = CCD_SQRT(dist);
-    }else if(ccdSimplexSize(&simplex) == 3){
-      dist = ccdVec3PointTriDist2(ccd_vec3_origin,
-                                  &ccdSimplexPoint(&simplex, 0)->v,
-                                  &ccdSimplexPoint(&simplex, 1)->v,
-                                  &ccdSimplexPoint(&simplex, 2)->v,
-                                  &dir);
-      dist = CCD_SQRT(dist);
-    }else{ // ccdSimplexSize(&simplex) == 4
-      dist = simplexReduceToTriangle(&simplex, last_dist, &dir);
-    }
-
-    // check whether we improved for at least a minimum tolerance
-    if ((last_dist - dist) < ccd->dist_tolerance)
-    {
-      extractClosestPoints(&simplex, p1, p2, &dir);
-      return dist;
-    }
-
-    // point direction towards the origin
-    ccdVec3Scale(&dir, -CCD_ONE);
-    ccdVec3Normalize(&dir);
-
-    // find out support point
-    __ccdSupport(obj1, obj2, &dir, ccd, &last);
-
-    // record last distance
-    last_dist = dist;
-
-    // check whether we improved for at least a minimum tolerance
-    // this is here probably only for a degenerate cases when we got a
-    // point that is already in the simplex
-    dist = ccdVec3Len2(&last.v);
-    dist = CCD_SQRT(dist);
-    if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance)
-    {
-      extractClosestPoints(&simplex, p1, p2, &dir);
-      return last_dist;
-    }
-
-    // add a point to simplex
-    ccdSimplexAdd(&simplex, &last);
-  }
-
-  return -CCD_REAL(1.);
+  return _ccdDist(obj1, obj2, ccd, &simplex, p1, p2);
 }
 
 } // namespace libccd_extension

test/CMakeLists.txt

@@ -59,6 +59,7 @@ set(tests
     test_fcl_signed_distance.cpp
     test_fcl_simple.cpp
     test_fcl_sphere_capsule.cpp
+    test_fcl_sphere_sphere.cpp
 )
 
 if (FCL_HAVE_OCTOMAP)
@@ -92,3 +93,5 @@ include_directories("${CMAKE_CURRENT_BINARY_DIR}")
 foreach(test ${tests})
   add_fcl_test(${test})
 endforeach(test)
+
+add_subdirectory(narrowphase)

test/narrowphase/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(detail)

test/narrowphase/detail/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(convexity_based_algorithm)

test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt

@@ -0,0 +1,8 @@
+set(tests
+    test_gjk_libccd-inl.cpp
+)
+
+# Build all the tests
+foreach(test ${tests})
+  add_fcl_test(${test})
+endforeach(test)

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp

@@ -0,0 +1,163 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018. Toyota Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of CNRS-LAAS and AIST nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Hongkai Dai*/
+#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h"
+
+#include <gtest/gtest.h>
+#include <Eigen/Dense>
+
+#include "fcl/narrowphase/detail/gjk_solver_libccd.h"
+
+namespace fcl {
+namespace detail {
+
+// Given two spheres, sphere 1 has radius1, and centered at point A, whose
+// position is p_FA measured and expressed in frame F; sphere 2 has radius2,
+// and centered at point B, whose position is p_FB measured and expressed in
+// frame F. Computes the signed distance between the two spheres, together with
+// the two closest points Na on sphere 1 and Nb on sphere 2, returns the
+// position of Na and Nb expressed in frame F.
+// We use the monogram notation on spatial vectors. The monogram notation is
+// explained in
+// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
+template <typename S>
+S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3<S>& p_FA,
+                              const Vector3<S>& p_FB, Vector3<S>* p_FNa,
+                              Vector3<S>* p_FNb) {
+  S min_distance = (p_FA - p_FB).norm() - radius1 - radius2;
+  const Vector3<S> p_AB_F =
+      p_FB - p_FA;  // The vector AB measured and expressed
+                    // in frame F.
+  *p_FNa = p_FA + p_AB_F.normalized() * radius1;
+  *p_FNb = p_FB - p_AB_F.normalized() * radius2;
+  return min_distance;
+}
+
+template <typename S>
+void TestSphereToSphereGJKSignedDistance(S radius1, S radius2,
+                                         const Vector3<S>& p_FA,
+                                         const Vector3<S>& p_FB, S tol,
+                                         S min_distance_expected,
+                                         const Vector3<S>& p_FNa_expected,
+                                         const Vector3<S>& p_FNb_expected) {
+  // Test if GJKSignedDistance computes the right distance. Here we used sphere
+  // to sphere as the geometries. The distance between sphere and sphere should
+  // be computed using distance between primitives, instead of the GJK
+  // algorithm. But here we choose spheres for simplicity.
+
+  fcl::Sphere<S> s1(radius1);
+  fcl::Sphere<S> s2(radius2);
+  fcl::Transform3<S> tf1, tf2;
+  tf1.setIdentity();
+  tf2.setIdentity();
+  tf1.translation() = p_FA;
+  tf2.translation() = p_FB;
+  void* o1 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s1, tf1);
+  void* o2 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s2, tf2);
+
+  S dist;
+  Vector3<S> p1, p2;
+  GJKSolver_libccd<S> gjkSolver;
+  bool res = GJKSignedDistance(
+      o1, detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(), o2,
+      detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(),
+      gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
+      &p1, &p2);
+
+  EXPECT_EQ(res, min_distance_expected >= 0);
+
+  EXPECT_NEAR(dist, min_distance_expected, tol);
+  EXPECT_TRUE(p1.isApprox(p_FNa_expected, tol));
+  EXPECT_TRUE(p2.isApprox(p_FNb_expected, tol));
+
+  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
+  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
+}
+
+template <typename S>
+struct SphereSpecification {
+  SphereSpecification<S>(S radius_, const Vector3<S>& center_)
+      : radius{radius_}, center{center_} {}
+  S radius;
+  Vector3<S> center;
+};
+
+template <typename S>
+void TestNonCollidingSphereGJKSignedDistance(S tol) {
+  std::vector<SphereSpecification<S>> spheres;
+  spheres.emplace_back(0.5, Vector3<S>(0, 0, -1.2));
+  spheres.emplace_back(0.5, Vector3<S>(1.25, 0, 0));
+  spheres.emplace_back(0.3, Vector3<S>(-0.2, 0, 0));
+  spheres.emplace_back(0.4, Vector3<S>(-0.2, 0, 1.1));
+  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
+    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
+      if ((spheres[i].center - spheres[j].center).norm() >
+          spheres[i].radius + spheres[j].radius) {
+        // Not in collision.
+        Vector3<S> p_FNa, p_FNb;
+        const S min_distance_expected = ComputeSphereSphereDistance(
+            spheres[i].radius, spheres[j].radius, spheres[i].center,
+            spheres[j].center, &p_FNa, &p_FNb);
+        TestSphereToSphereGJKSignedDistance<S>(
+            spheres[i].radius, spheres[j].radius, spheres[i].center,
+            spheres[j].center, tol, min_distance_expected, p_FNa, p_FNb);
+      } else {
+        GTEST_FAIL() << "The two spheres collide."
+                     << "\nSpheres[" << i << "] with radius "
+                     << spheres[i].radius << ", centered at "
+                     << spheres[i].center.transpose() << "\nSpheres[" << j
+                     << "] with radius " << spheres[j].radius
+                     << ", centered at " << spheres[j].center.transpose()
+                     << "\n";
+      }
+    }
+  }
+}
+
+GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) {
+  // TODO(hongkai.dai@tri.global): By setting gjkSolver.distance_tolerance to
+  // the default value (1E-6), the tolerance we get on the closest points are
+  // only up to 1E-3. Should investigate why there is such a big difference.
+  TestNonCollidingSphereGJKSignedDistance<double>(1E-3);
+  TestNonCollidingSphereGJKSignedDistance<float>(1E-3);
+}
+}  // namespace detail
+}  // namespace fcl
+
+//==============================================================================
+int main(int argc, char* argv[]) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}