diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 4cd8482c5..e34e81b68 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/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 diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 745583ef7..d9cb266db 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/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::deleteGJKObject(o1); detail::GJKInitializer::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::deleteGJKObject(o1); detail::GJKInitializer::deleteGJKObject(o2); @@ -809,8 +797,6 @@ struct ShapeTriangleDistanceLibccdImpl dist, p1, p2); - if(p1) - (*p1).noalias() = tf.inverse(Eigen::Isometry) * *p1; detail::GJKInitializer::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::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..5b020b251 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 @@ -337,11 +338,19 @@ void NearestPointFromDegenerateSimplex() { // The values here have been visually confirmed from the computation. 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)); + // The "nearest" points (N1 and N2) measured and expressed in box 1's and + // box 2's frames (B1 and B2, respectively). + Vector3 expected_p_B1N1{-1.375, -0.098881502700918666, + -0.025000000000000022}; + Vector3 expected_p_B2N2{0.21199965773384655, 0.074999692703297122, + 0.084299993303443954}; + // The nearest points in the world frame. + Vector3 expected_p_WN1 = box_object_1.getTransform() * expected_p_B1N1; + 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)); // TODO(SeanCurtis-TRI): Change this tolerance to constants::eps_34() when // the mac single/double libccd problem is resolved. EXPECT_NEAR(expected_dist, result.min_distance, diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index c67eb9f89..09ddd4097 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" @@ -61,48 +62,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)); + } + + // Expecting distance to be -1.715728753 + result.clear(); + tf2.translation() = Vector3(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 dir = tf2.translation().normalized(); + Vector3 p0_expected = dir * 20; + EXPECT_TRUE(CompareMatrices(result.nearest_points[0], p0_expected, + distance_tolerance)); + Vector3 p1_expected = tf2.translation() - dir * 10; + 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. }