Correct distance queries to report nearest point in world frame (#288)

* 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

@@ -625,8 +625,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;
     }
@@ -880,8 +882,9 @@ struct ShapeTriangleDistanceIndepImpl
       }
 
       if(distance) *distance = (w0 - w1).norm();
+      // The answers are produced in world coordinates. Keep them there.
       if(p1) *p1 = w0;
-      if(p2) (*p2).noalias() = shape.toshape0 * w1;
+      if(p2) *p2 = w1;
       return true;
     }
     else
@@ -970,8 +973,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

@@ -567,12 +567,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);
 
@@ -624,12 +618,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);
 
@@ -848,8 +836,6 @@ struct ShapeTriangleDistanceLibccdImpl
           dist,
           p1,
           p2);
-    if(p1)
-      (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1;
 
     detail::GJKInitializer<S, Shape>::deleteGJKObject(o1);
     detail::triDeleteGJKObject(o2);
@@ -923,10 +909,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
@@ -315,6 +316,10 @@ void NearestPointFromDegenerateSimplex() {
   // line segment. As a result, nearest points were populated with NaN values.
   // See https://github.com/flexible-collision-library/fcl/issues/293 for
   // more discussion.
+  // This test is only relevant if box-box distance is computed via GJK. If
+  // a custom test is created, this may no longer be relevant.
+  // TODO(SeanCurtis-TRI): Provide some mechanism where we can assert what the
+  // solving algorithm is (i.e., default convex vs. custom).
   DistanceResult<S> result;
   DistanceRequest<S> request;
   request.enable_nearest_points = true;
@@ -335,15 +340,25 @@ void NearestPointFromDegenerateSimplex() {
 
   EXPECT_NO_THROW(fcl::distance(&box_object_1, &box_object_2, request, result));
 
-  // The values here have been visually confirmed from the computation.
+  // These hard-coded values have been previously computed and visually
+  // inspected and considered to be the ground truth for this very specific
+  // test configuration.
   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));
-  // TODO(SeanCurtis-TRI): Change this tolerance to constants<S>::eps_34() when
-  // the mac single/double libccd problem is resolved.
+  // The "nearest" points (N1 and N2) measured and expressed in box 1's and
+  // box 2's frames (B1 and B2, respectively).
+  const Vector3<S> expected_p_B1N1{-1.375, -0.098881502700918666,
+                                   -0.025000000000000022};
+  const Vector3<S> expected_p_B2N2{0.21199965773384655, 0.074999692703297122,
+                                   0.084299993303443954};
+  // The nearest points in the world frame.
+  const Vector3<S> expected_p_WN1 =
+      box_object_1.getTransform() * expected_p_B1N1;
+  const 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));
   EXPECT_NEAR(expected_dist, result.min_distance,
               constants<ccd_real_t>::eps_34());
 }