From 91d9d4d5735e44f91ba9df013c9fcd16a22938e4 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Sun, 12 Aug 2018 14:42:50 -0700 Subject: [PATCH] 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. --- .../narrowphase/detail/gjk_solver_indep-inl.h | 13 +- .../detail/gjk_solver_libccd-inl.h | 18 --- .../sphere_capsule-inl.h | 18 ++- .../sphere_sphere-inl.h | 4 +- include/fcl/narrowphase/distance_result.h | 4 +- test/test_fcl_capsule_box_1.cpp | 8 +- test/test_fcl_capsule_box_2.cpp | 6 +- test/test_fcl_distance.cpp | 31 +++-- test/test_fcl_signed_distance.cpp | 121 +++++++++++++++--- test/test_fcl_sphere_sphere.cpp | 36 +++--- 10 files changed, 177 insertions(+), 82 deletions(-) diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 3e386737a..cf93d4558 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/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 diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 4e6903215..d9ffffe80 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/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::deleteGJKObject(o1); detail::GJKInitializer::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::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -848,8 +836,6 @@ struct ShapeTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1; detail::GJKInitializer::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::deleteGJKObject(o1); detail::triDeleteGJKObject(o2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h index dd524ad6a..c85d56991 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h @@ -137,8 +137,16 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& 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; @@ -146,10 +154,14 @@ bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, 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; } diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h index efeeca4eb..d12671cfb 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h @@ -99,8 +99,8 @@ bool sphereSphereDistance(const Sphere& s1, const Transform3& 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; } diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index fb53b36d8..045d2c80d 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/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 nearest_points[2]; /// @brief collision object 1 diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp index e37b0762a..0be5633cf 100644 --- a/test/test_fcl_capsule_box_1.cpp +++ b/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 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); diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp index 41218cfa4..d3ee42521 100644 --- a/test/test_fcl_capsule_box_2.cpp +++ b/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 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); diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index d49dc7e1f..e703c12ff 100644 --- a/test/test_fcl_distance.cpp +++ b/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 result; DistanceRequest 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 expected_p0{-1.375, -0.098881502700918666, -0.025000000000000022}; - Vector3 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::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 expected_p_B1N1{-1.375, -0.098881502700918666, + -0.025000000000000022}; + const Vector3 expected_p_B2N2{0.21199965773384655, 0.074999692703297122, + 0.084299993303443954}; + // The nearest points in the world frame. + const Vector3 expected_p_WN1 = + box_object_1.getTransform() * expected_p_B1N1; + const Vector3 expected_p_WN2 = + box_object_2.getTransform() * expected_p_B2N2; + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], expected_p_WN1, + DELTA(), MatrixCompareType::absolute)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], expected_p_WN2, + DELTA(), MatrixCompareType::absolute)); EXPECT_NEAR(expected_dist, result.min_distance, constants::eps_34()); } diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index c67eb9f89..153581707 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -34,6 +34,7 @@ #include +#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" @@ -48,8 +49,10 @@ bool verbose = false; template void test_distance_spheresphere(GJKSolverType solver_type) { - Sphere s1{20}; - Sphere s2{10}; + const S radius_1 = 20; + const S radius_2 = 10; + Sphere s1{radius_1}; + Sphere s2{radius_2}; Transform3 tf1{Transform3::Identity()}; Transform3 tf2{Transform3::Identity()}; @@ -61,48 +64,128 @@ void test_distance_spheresphere(GJKSolverType solver_type) DistanceResult result; - bool res{false}; + // Expecting distance to be 10 + result.clear(); + tf2.translation() = Vector3(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(20, 0, 0), + request.distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(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(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(20, 0, 0), + distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(15, 0, 0), + distance_tolerance)); + } + + result.clear(); + tf2.translation() = Vector3(20, 0, 20); + distance(&s1, tf1, &s2, tf2, request, result); + + S expected_dist = + (tf1.translation() - tf2.translation()).norm() - radius_1 - radius_2; + EXPECT_NEAR(result.min_distance, expected_dist, 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 dir = (tf2.translation() - tf1.translation()).normalized(); + Vector3 p0_expected = dir * radius_1; + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected, + distance_tolerance)); + Vector3 p1_expected = tf2.translation() - dir * radius_2; + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], p1_expected, + distance_tolerance)); + } +} + +template +void test_distance_spherecapsule(GJKSolverType solver_type) +{ + Sphere s1{20}; + Capsule s2{10, 20}; + + Transform3 tf1{Transform3::Identity()}; + Transform3 tf2{Transform3::Identity()}; + + DistanceRequest request; + request.enable_signed_distance = true; + request.enable_nearest_points = true; + request.gjk_solver_type = solver_type; + + DistanceResult result; // Expecting distance to be 10 result.clear(); tf2.translation() = Vector3(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(20, 0, 0))); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-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(20, 0, 0), + request.distance_tolerance)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(30, 0, 0), + request.distance_tolerance)); // Expecting distance to be -5 result.clear(); tf2.translation() = Vector3(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(20, 0, 0), - distance_tolerance)); - EXPECT_TRUE(result.nearest_points[1].isApprox(Vector3(-10, 0, 0), - distance_tolerance)); + // NOTE: Currently, only GST_LIBCCD computes the pair of nearest points. + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], Vector3(20, 0, 0), + distance_tolerance * 100)); + EXPECT_TRUE(CompareMatrices(result.nearest_points[1], Vector3(15, 0, 0), + distance_tolerance * 100)); } } //============================================================================== -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere) + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) { test_distance_spheresphere(GST_LIBCCD); +} + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) +{ test_distance_spheresphere(GST_INDEP); } +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) +{ + test_distance_spherecapsule(GST_LIBCCD); +} + +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) +{ + test_distance_spherecapsule(GST_INDEP); +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/test/test_fcl_sphere_sphere.cpp b/test/test_fcl_sphere_sphere.cpp index 66db15cc5..78e9cc8ee 100644 --- a/test/test_fcl_sphere_sphere.cpp +++ b/test/test_fcl_sphere_sphere.cpp @@ -41,6 +41,7 @@ #include #include +#include "eigen_matrix_compare.h" #include "fcl/narrowphase/collision_object.h" #include "fcl/narrowphase/distance.h" #include "fcl/narrowphase/distance_request.h" @@ -106,8 +107,8 @@ void CheckSphereToSphereDistance(const SphereSpecification& sphere1, enable_nearest_points, enable_signed_distance, &result); EXPECT_NEAR(min_distance, min_distance_expected, tol); EXPECT_NEAR(result.min_distance, min_distance_expected, tol); - EXPECT_TRUE(result.nearest_points[0].isApprox(p1_expected, tol)); - EXPECT_TRUE(result.nearest_points[1].isApprox(p2_expected, tol)); + EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[0], p1_expected, tol)); + EXPECT_TRUE(fcl::CompareMatrices(result.nearest_points[1], p2_expected, tol)); } template @@ -118,18 +119,16 @@ struct SphereSphereDistance { min_distance = (sphere1.p_FC - sphere2.p_FC).norm() - sphere1.radius - sphere2.radius; const fcl::Vector3 AB = (sphere1.p_FC - sphere2.p_FC).normalized(); - p_S1P1 = AB * -sphere1.radius; - p_S2P2 = AB * sphere2.radius; + p_WP1 = sphere1.p_FC + AB * -sphere1.radius; + p_WP2 = sphere2.p_FC + AB * sphere2.radius; } SphereSpecification sphere1; SphereSpecification sphere2; S min_distance; - // The closest point P1 on sphere 1 is expressed in the sphere 1 body frame - // S1. - // The closest point P2 on sphere 2 is expressed in the sphere 2 body frame - // S2. - fcl::Vector3 p_S1P1; - fcl::Vector3 p_S2P2; + // The closest point P1 on sphere 1 is expressed in the world frame W. + fcl::Vector3 p_WP1; + // The closest point P2 on sphere 2 is expressed in the world frame W. + fcl::Vector3 p_WP2; }; template @@ -146,27 +145,28 @@ void TestSeparatingSpheres(S tol, fcl::GJKSolverType solver_type) { bool enable_signed_distance = true; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, - sphere_sphere_distance.p_S2P2, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, + sphere_sphere_distance.p_WP2, tol); // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, - sphere_sphere_distance.p_S1P1, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, + sphere_sphere_distance.p_WP1, tol); enable_signed_distance = false; CheckSphereToSphereDistance( spheres[i], spheres[j], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S1P1, - sphere_sphere_distance.p_S2P2, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP1, + sphere_sphere_distance.p_WP2, tol); + // Now switch the order of sphere 1 with sphere 2 in calling // fcl::distance function, and test again. CheckSphereToSphereDistance( spheres[j], spheres[i], solver_type, true, enable_signed_distance, - sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2, - sphere_sphere_distance.p_S1P1, tol); + sphere_sphere_distance.min_distance, sphere_sphere_distance.p_WP2, + sphere_sphere_distance.p_WP1, tol); // TODO (hongkai.dai@tri.global): Test enable_nearest_points=false. }