fix missing nearest points in Octree<->Mesh distance result (#427)

* fix missing nearest points in Octree<->Mesh distance result

* Extend test_fcl_octomap_distance, check nearest_points

Co-authored-by: Alberto Marnetto <58592261+AlbertoMarnetto-Work@users.noreply.github.com>

include/fcl/narrowphase/detail/traversal/octree/octree_solver-inl.h

@@ -506,7 +506,7 @@ bool OcTreeSolver<NarrowPhaseSolver>::OcTreeMeshDistanceRecurse(const OcTree<S>*
       Vector3<S> closest_p1, closest_p2;
       solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2);
 
-      dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
+      dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id, closest_p1, closest_p2);
 
       return drequest->isSatisfied(*dresult);
     }

test/test_fcl_octomap_distance.cpp

@@ -196,6 +196,7 @@ void octomap_distance_test_BVH(std::size_t n, double resolution)
     CollisionObject<S> obj1(m1_ptr, tf);
     CollisionObject<S> obj2(tree_ptr, tf);
     test::DistanceData<S> cdata;
+    cdata.request.enable_nearest_points = true;
     S dist1 = std::numeric_limits<S>::max();
     test::defaultDistanceFunction(&obj1, &obj2, &cdata, dist1);
 
@@ -219,6 +220,13 @@ void octomap_distance_test_BVH(std::size_t n, double resolution)
 
     std::cout << dist1 << " " << dist2 << std::endl;
     EXPECT_TRUE(std::abs(dist1 - dist2) < 0.001);
+
+    // Check that the nearest points are consistent with the distance
+    // Note that we cannot compare the result with the "boxes" approximation,
+    // since the problem is ill-posed (i.e. the nearest points may differ widely
+    // for slightly different geometries)
+    Vector3<S> nearestPointDistance = cdata.result.nearest_points[0] - cdata.result.nearest_points[1];
+    EXPECT_NEAR(nearestPointDistance.norm(), dist1, 0.001);
   }
 }