Distance queries return nearest points in the world frame

The DistanceResult struct documents the near points as being defined in the
*world* frame. This wasn't true in the code.

1. Correct both gjk solver implementations to put the pose in *world* frame.
2. Modify sphere-sphere and sphere-capsule custom implementations to do the
   same.
3. Fix some related typos in documentation.
4. Update tests to expect points in world frame.

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

@@ -622,8 +622,10 @@ struct ShapeDistanceIndepImpl
 
       if(distance) *distance = (w0 - w1).norm();
 
-      if(p1) *p1 = w0;
-      if(p2) (*p2).noalias() = shape.toshape0.inverse() * w1;
+      // Answer is solved in Shape1's local frame; answers are given in the
+      // world frame.
+      if(p1) p1->noalias() = tf1 * w0;
+      if(p2) p2->noalias() = tf1 * w1;
 
       return true;
     }
@@ -841,8 +843,9 @@ struct ShapeTriangleDistanceIndepImpl
       }
 
       if(distance) *distance = (w0 - w1).norm();
-      if(p1) *p1 = w0;
-      if(p2) (*p2).noalias() = shape.toshape0 * w1;
+      // The answers are produced in world coordinates. Keep them there.
+      if(p1) p1->noalias() = w0;
+      if(p2) p2->noalias() = w1;
       return true;
     }
     else
@@ -931,8 +934,8 @@ struct ShapeTransformedTriangleDistanceIndepImpl
       }
 
       if(distance) *distance = (w0 - w1).norm();
-      if(p1) *p1 = w0;
-      if(p2) (*p2).noalias() = shape.toshape0 * w1;
+      if(p1) p1->noalias() = tf1 * w0;
+      if(p2) p2->noalias() = tf1 * w1;
       return true;
     }
     else

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

@@ -564,12 +564,6 @@ struct ShapeSignedDistanceLibccdImpl
           p1,
           p2);
 
-    if (p1)
-      (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1;
-
-    if (p2)
-      (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2;
-
     detail::GJKInitializer<S, Shape1>::deleteGJKObject(o1);
     detail::GJKInitializer<S, Shape2>::deleteGJKObject(o2);
 
@@ -621,12 +615,6 @@ struct ShapeDistanceLibccdImpl
           p1,
           p2);
 
-    if (p1)
-      (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1;
-
-    if (p2)
-      (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2;
-
     detail::GJKInitializer<S, Shape1>::deleteGJKObject(o1);
     detail::GJKInitializer<S, Shape2>::deleteGJKObject(o2);
 
@@ -809,8 +797,6 @@ struct ShapeTriangleDistanceLibccdImpl
           dist,
           p1,
           p2);
-    if(p1)
-      (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1;
 
     detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
     detail::triDeleteGJKObject(o2);
@@ -884,10 +870,6 @@ struct ShapeTransformedTriangleDistanceLibccdImpl
           dist,
           p1,
           p2);
-    if(p1)
-      (*p1).noalias() = tf1.inverse(Eigen::Isometry) * *p1;
-    if(p2)
-      (*p2).noalias() = tf2.inverse(Eigen::Isometry) * *p2;
 
     detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
     detail::triDeleteGJKObject(o2);

include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h

@@ -137,19 +137,31 @@ bool sphereCapsuleDistance(const Sphere<S>& s1, const Transform3<S>& tf1,
 
   S distance = diff.norm() - s1.radius - s2.radius;
 
-  if(distance <= 0)
+  if(distance <= 0) {
+    // NOTE: By assigning this a negative value, it allows the ultimately
+    // calling code in distance-inl.h (distance() method) to use collision to
+    // determine penetration depth and contact points. NOTE: This is a
+    // *horrible* thing.
+    // TODO(SeanCurtis-TRI): Create a *signed* distance variant of this and use
+    // it to determined signed distance, penetration, and distance.
+    if (dist) *dist = -1;
     return false;
+  }
 
   if(dist) *dist = distance;
 
   if(p1 || p2) diff.normalize();
   if(p1)
   {
     *p1 = s_c - diff * s1.radius;
-    *p1 = tf1.inverse(Eigen::Isometry) * tf2 * (*p1);
+    *p1 = tf2 * (*p1);
   }
 
-  if(p2) *p2 = segment_point + diff * s1.radius;
+  if(p2)
+  {
+    *p2 = segment_point + diff * s2.radius;
+    *p2 = tf2 * (*p2);
+  }
 
   return true;
 }

include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h

@@ -99,8 +99,8 @@ bool sphereSphereDistance(const Sphere<S>& s1, const Transform3<S>& tf1,
   if(len > s1.radius + s2.radius)
   {
     if(dist) *dist = len - (s1.radius + s2.radius);
-    if(p1) *p1 = tf1.inverse(Eigen::Isometry) * (o1 - diff * (s1.radius / len));
-    if(p2) *p2 = tf2.inverse(Eigen::Isometry) * (o2 + diff * (s2.radius / len));
+    if(p1) *p1 = (o1 - diff * (s1.radius / len));
+    if(p2) *p2 = (o2 + diff * (s2.radius / len));
     return true;
   }
 

include/fcl/narrowphase/distance_result.h

@@ -63,9 +63,9 @@ struct FCL_EXPORT DistanceResult
   /// @sa DistanceRequest::enable_signed_distance
   S min_distance;
 
-  /// @brief Nearest points in the world coordinates
+  /// @brief Nearest points in the world coordinates.
   ///
-  /// @sa DeistanceRequest::enable_nearest_points
+  /// @sa DistanceRequest::enable_nearest_points
   Vector3<S> nearest_points[2];
 
   /// @brief collision object 1

test/test_fcl_capsule_box_1.cpp

@@ -71,7 +71,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc
   // Nearest point on box
   fcl::Vector3<S> o2 (distanceResult.nearest_points [1]);
   EXPECT_NEAR (distanceResult.min_distance, 0.5, test_tolerance);
-  EXPECT_NEAR (o1 [0], -2.0, test_tolerance);
+  EXPECT_NEAR (o1 [0],  1.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);
@@ -89,7 +89,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc
   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);
+  EXPECT_NEAR (o1 [2],  4.0, test_tolerance);
 
   EXPECT_NEAR (o2 [0],  0.0, test_tolerance);
   EXPECT_NEAR (o2 [1],  0.0, test_tolerance);
@@ -107,9 +107,9 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc
   o2 = distanceResult.nearest_points [1];
 
   EXPECT_NEAR (distanceResult.min_distance, 5.5, test_tolerance);
-  EXPECT_NEAR (o1 [0],  0.0, test_tolerance);
+  EXPECT_NEAR (o1 [0], -6.0, test_tolerance);
   EXPECT_NEAR (o1 [1],  0.0, test_tolerance);
-  EXPECT_NEAR (o1 [2],  4.0, test_tolerance);
+  EXPECT_NEAR (o1 [2],  0.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);

test/test_fcl_capsule_box_2.cpp

@@ -74,9 +74,9 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc
   fcl::Vector3<S> o2 = distanceResult.nearest_points [1];
 
   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 (o1 [0], -6.0, test_tolerance);
+  EXPECT_NEAR (o1 [1],  0.8, test_tolerance);
+  EXPECT_NEAR (o1 [2],  1.5, test_tolerance);
   EXPECT_NEAR (o2 [0], -0.5, test_tolerance);
   EXPECT_NEAR (o2 [1],  0.8, test_tolerance);
   EXPECT_NEAR (o2 [2],  1.5, test_tolerance);

test/test_fcl_distance.cpp

@@ -39,6 +39,7 @@
 
 #include "fcl/narrowphase/detail/traversal/collision_node.h"
 #include "test_fcl_utility.h"
+#include "eigen_matrix_compare.h"
 #include "fcl_resources/config.h"
 
 // TODO(SeanCurtis-TRI): A file called `test_fcl_distance.cpp` should *not* have
@@ -337,11 +338,19 @@ void NearestPointFromDegenerateSimplex() {
 
   // The values here have been visually confirmed from the computation.
   S expected_dist{0.053516322172152138};
-  Vector3<S> expected_p0{-1.375, -0.098881502700918666, -0.025000000000000022};
-  Vector3<S> expected_p1{0.21199965773384655, 0.074999692703297122,
-                         0.084299993303443954};
-  EXPECT_TRUE(nearlyEqual(result.nearest_points[0], expected_p0));
-  EXPECT_TRUE(nearlyEqual(result.nearest_points[1], expected_p1));
+  // The "nearest" points (N1 and N2) measured and expressed in box 1's and
+  // box 2's frames (B1 and B2, respectively).
+  Vector3<S> expected_p_B1N1{-1.375, -0.098881502700918666,
+                             -0.025000000000000022};
+  Vector3<S> expected_p_B2N2{0.21199965773384655, 0.074999692703297122,
+                            0.084299993303443954};
+  // The nearest points in the world frame.
+  Vector3<S> expected_p_WN1 = box_object_1.getTransform() * expected_p_B1N1;
+  Vector3<S> expected_p_WN2 = box_object_2.getTransform() * expected_p_B2N2;
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], expected_p_WN1,
+                              DELTA<S>(), MatrixCompareType::absolute));
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], expected_p_WN2,
+                              DELTA<S>(), MatrixCompareType::absolute));
   // TODO(SeanCurtis-TRI): Change this tolerance to constants<S>::eps_34() when
   // the mac single/double libccd problem is resolved.
   EXPECT_NEAR(expected_dist, result.min_distance,

test/test_fcl_signed_distance.cpp

@@ -34,6 +34,7 @@
 
 #include <gtest/gtest.h>
 
+#include "eigen_matrix_compare.h"
 #include "fcl/narrowphase/distance.h"
 #include "fcl/narrowphase/detail/traversal/collision_node.h"
 #include "fcl/narrowphase/detail/gjk_solver_libccd.h"
@@ -61,48 +62,128 @@ void test_distance_spheresphere(GJKSolverType solver_type)
 
   DistanceResult<S> result;
 
-  bool res{false};
+  // Expecting distance to be 10
+  result.clear();
+  tf2.translation() = Vector3<S>(40, 0, 0);
+  distance(&s1, tf1, &s2, tf2, request, result);
+  EXPECT_NEAR(result.min_distance, 10, 1e-6);
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
+                              request.distance_tolerance));
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(30, 0, 0),
+                              request.distance_tolerance));
+
+  // request.distance_tolerance is actually the square of the distance
+  // tolerance, namely the difference between distance returned from FCL's EPA
+  // implementation and the actual distance, is less than
+  // sqrt(request.distance_tolerance).
+  const S distance_tolerance = std::sqrt(request.distance_tolerance);
+
+  // Expecting distance to be -5
+  result.clear();
+  tf2.translation() = Vector3<S>(25, 0, 0);
+  distance(&s1, tf1, &s2, tf2, request, result);
+  EXPECT_NEAR(result.min_distance, -5, request.distance_tolerance);
+
+  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
+  // surface of the penetrating spheres.
+  if (solver_type == GST_LIBCCD)
+  {
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
+                                distance_tolerance));
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(15, 0, 0),
+                                distance_tolerance));
+  }
+
+  // Expecting distance to be -1.715728753
+  result.clear();
+  tf2.translation() = Vector3<S>(20, 0, 20);
+  distance(&s1, tf1, &s2, tf2, request, result);
+
+
+  EXPECT_NEAR(result.min_distance, -1.715728753, distance_tolerance);
+  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
+  // surface of the spheres.
+  if (solver_type == GST_LIBCCD)
+  {
+    Vector3<S> dir = tf2.translation().normalized();
+    Vector3<S> p0_expected = dir * 20;
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected,
+                                distance_tolerance));
+    Vector3<S> p1_expected = tf2.translation() - dir * 10;
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[1], p1_expected,
+                                distance_tolerance));
+  }
+}
+
+template <typename S>
+void test_distance_spherecapsule(GJKSolverType solver_type)
+{
+  Sphere<S> s1{20};
+  Capsule<S> s2{10, 20};
+
+  Transform3<S> tf1{Transform3<S>::Identity()};
+  Transform3<S> tf2{Transform3<S>::Identity()};
+
+  DistanceRequest<S> request;
+  request.enable_signed_distance = true;
+  request.enable_nearest_points = true;
+  request.gjk_solver_type = solver_type;
+
+  DistanceResult<S> result;
 
   // Expecting distance to be 10
   result.clear();
   tf2.translation() = Vector3<S>(40, 0, 0);
-  res = distance(&s1, tf1, &s2, tf2, request, result);
-  EXPECT_TRUE(res);
-  EXPECT_TRUE(std::abs(result.min_distance - 10) < 1e-6);
-  EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3<S>(20, 0, 0)));
-  EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3<S>(-10, 0, 0)));
+  distance(&s1, tf1, &s2, tf2, request, result);
+  EXPECT_NEAR(result.min_distance, 10, request.distance_tolerance);
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
+                              request.distance_tolerance));
+  EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(30, 0, 0),
+                              request.distance_tolerance));
 
   // Expecting distance to be -5
   result.clear();
   tf2.translation() = Vector3<S>(25, 0, 0);
-  res = distance(&s1, tf1, &s2, tf2, request, result);
+  distance(&s1, tf1, &s2, tf2, request, result);
 
-  EXPECT_TRUE(res);
   // request.distance_tolerance is actually the square of the distance
   // tolerance, namely the difference between distance returned from FCL's EPA
   // implementation and the actual distance, is less than
   // sqrt(request.distance_tolerance).
   const S distance_tolerance = std::sqrt(request.distance_tolerance);
-  EXPECT_NEAR(result.min_distance, -5, distance_tolerance);
-
-  // TODO(JS): Only GST_LIBCCD can compute the pair of nearest points on the
-  // surface of the spheres.
+  ASSERT_NEAR(result.min_distance, -5, distance_tolerance);
   if (solver_type == GST_LIBCCD)
   {
-    EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3<S>(20, 0, 0),
-                                                  distance_tolerance));
-    EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3<S>(-10, 0, 0),
-                                                  distance_tolerance));
+    // NOTE: Currently, only GST_LIBCCD computes the pair of nearest points.
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3<S>(20, 0, 0),
+                                distance_tolerance * 100));
+    EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3<S>(15, 0, 0),
+                                distance_tolerance * 100));
   }
 }
 
 //==============================================================================
-GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere)
+
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
 {
   test_distance_spheresphere<double>(GST_LIBCCD);
+}
+
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep)
+{
   test_distance_spheresphere<double>(GST_INDEP);
 }
 
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd)
+{
+  test_distance_spherecapsule<double>(GST_LIBCCD);
+}
+
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep)
+{
+  test_distance_spherecapsule<double>(GST_INDEP);
+}
+
 //==============================================================================
 int main(int argc, char* argv[])
 {