diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 90450f5d3..8380f117a 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -369,6 +369,11 @@ static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) { return 0; } + static bool isAbsValueLessThanEpsSquared(ccd_real_t val) { + return std::abs(val) < std::numeric_limits::epsilon() * + std::numeric_limits::epsilon(); + } + // TODO(SeanCurtis-TRI): Define the return value: // 1: (like doSimplex2) --> origin is "in" the simplex. // 0: @@ -386,10 +391,8 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) C = ccdSimplexPoint(simplex, 0); // check touching contact - // TODO(SeanCurtis-TRI): This is dist2 (i.e., dist_squared) and should be - // tested to be zero within a tolerance of ε² (and not just ε). dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr); - if (ccdIsZero(dist)){ + if (isAbsValueLessThanEpsSquared(dist)){ return 1; } @@ -460,11 +463,6 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) return 0; } -static bool isAbsValueLessThanEpsSquared(ccd_real_t val) { - return std::abs(val) < std::numeric_limits::epsilon() * - std::numeric_limits::epsilon(); -} - static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B, *C, *D; @@ -940,6 +938,28 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, return false; } +/** + * Determines if the point P is on the line segment AB. + * If A, B, P are coincident, report true. + */ +static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a, const ccd_vec3_t& b) { + if (are_coincident(a, b)) { + return are_coincident(a, p); + } + // A and B are not coincident, if the triangle ABP has non-zero area, then P + // is not on the line adjoining AB, and hence not on the line segment AB. + if (!triangle_area_is_zero(a, b, p)) { + return false; + } + // P is on the line adjoinging AB. If P is on the line segment AB, then + // PA.dot(PB) <= 0. + ccd_vec3_t PA, PB; + ccdVec3Sub2(&PA, &p, &a); + ccdVec3Sub2(&PB, &p, &b); + return ccdVec3Dot(&PA, &PB) <= 0; +} + + /** * Computes the normal vector of a triangular face on a polytope, and the normal * vector points outward from the polytope. Notice we assume that the origin @@ -1286,7 +1306,6 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, "The visible feature is a vertex. This should already have been " "identified as a touching contact"); } - // Start with the face on which the closest point lives if (el->type == CCD_PT_FACE) { start_face = reinterpret_cast(el); @@ -1381,7 +1400,7 @@ static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope, pointing outward from the polytope. */ ccd_vec3_t dir; - if (ccdIsZero(nearest_feature->dist)) { + if (isAbsValueLessThanEpsSquared(nearest_feature->dist)) { // nearest point is the origin. switch (nearest_feature->type) { case CCD_PT_VERTEX: { @@ -1533,6 +1552,95 @@ static int __ccdGJK(const void *obj1, const void *obj2, return -1; } +/** + * When the nearest feature of a polytope to the origin is an edge, and the + * origin is inside the polytope, it implies one of the following conditions + * 1. The origin lies exactly on that edge + * 2. The two neighbouring faces of that edge are coplanar, and the projection + * of the origin onto that plane is on the edge. Inside this function, we will + * verify if one of these two conditions are true. If not, we will modify the + * nearest feature stored inside @p polytope, such that it stores the right + * nearest feature and distance. + * @param polytope The polytope whose nearest feature is being verified (and + * corrected if the edge should not be nearest feature). + * @note Only call this function in the EPA functions, where the origin should + * be inside the polytope. + */ +static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { + if (polytope->nearest_type == CCD_PT_EDGE) { + // Only verify the feature if the nearest feature is an edge. + + ccd_pt_edge_t* nearest_edge = + reinterpret_cast(polytope->nearest); + // Find the outward normals on the two neighbouring faces of the edge, if + // the origin is on the "inner" side of these two faces, then we regard the + // origin to be inside the polytope. Note that these @p face_normals are + // normalized. + std::array face_normals; + std::array origin_to_face_distance; + for (int i = 0; i < 2; ++i) { + face_normals[i] = + faceNormalPointingOutward(polytope, nearest_edge->faces[i]); + ccdVec3Normalize(&(face_normals[i])); + // If the origin is on the "inner" side of the face, then + // face_normals[i].dot(origin - edge.vertex) <= 0. + origin_to_face_distance[i] = + -ccdVec3Dot(&(face_normals[i]), &(nearest_edge->vertex[0]->v.v)); + if (origin_to_face_distance[i] > 0) { + FCL_THROW_FAILED_AT_THIS_CONFIGURATION( + "The origin is outside of the polytope. This should already have " + "been identified as separating."); + } + } + // If the two neighbouring faces are co-planar, then their face normals are + // the same. + bool are_neighbouring_faces_coplanar = true; + for (int i = 0; i < 3; ++i) { + if (!ccdIsZero(face_normals[0].v[i] - face_normals[1].v[i])) { + are_neighbouring_faces_coplanar = false; + break; + } + } + bool is_edge_closest_feature = true; + if (!are_neighbouring_faces_coplanar) { + // If the closest feature is an edge, and the two neighouring faces are + // not co-planar, then the origin has to be on that edge, and hence the + // distance from the origin to the face has to be 0. + if (!ccdIsZero(origin_to_face_distance[0]) || + !ccdIsZero(origin_to_face_distance[1])) { + // The distance from the origin to the face is not 0. This violates the + // assumption that the closest feature is an edge. We will recompute the + // closest feature. + is_edge_closest_feature = false; + } + } else { + // The neighbouring faces are coplanar. + // We compute the projection of the origin onto the plane as + // -face_normals[0] * origin_to_face_distance[0] + ccd_vec3_t origin_projection_on_edge = face_normals[0]; + ccdVec3Scale(&origin_projection_on_edge, -origin_to_face_distance[0]); + if (!is_point_on_line_segment(origin_projection_on_edge, + nearest_edge->vertex[0]->v.v, + nearest_edge->vertex[1]->v.v)) { + is_edge_closest_feature = false; + } + } + if (!is_edge_closest_feature) { + // We assume that libccd is not crazily wrong. Although the closest + // feature should not be the edge, it is near that edge. Hence we pick one + // of the neighbouring faces as the closest feature. + polytope->nearest_type = CCD_PT_FACE; + // Note origin_to_face_distance is the signed distance. + const int closest_face = + origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0; + polytope->nearest = + reinterpret_cast(nearest_edge->faces[closest_face]); + // polytope->nearest_dist stores the SQUARED distance. + polytope->nearest_dist = origin_to_face_distance[closest_face] * + origin_to_face_distance[closest_face]; + } + } +} static int __ccdEPA(const void *obj1, const void *obj2, const ccd_t *ccd, @@ -1557,6 +1665,7 @@ static int __ccdEPA(const void *obj1, const void *obj2, ret = simplexToPolytope2(obj1, obj2, ccd, simplex, polytope, nearest); } + if (ret == -1){ // touching contact return 0; @@ -1566,12 +1675,19 @@ static int __ccdEPA(const void *obj1, const void *obj2, } while (1){ - // get triangle nearest to origin + // get triangle nearest to origin + *nearest = ccdPtNearest(polytope); + if (polytope->nearest_type == CCD_PT_EDGE) { + // When libccd thinks the nearest feature is an edge, that is often + // wrong, hence we validate the nearest feature by ourselves. + validateNearestFeatureOfPolytopeBeingEdge(polytope); *nearest = ccdPtNearest(polytope); + } // get next support point - if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) + if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) { break; + } // expand nearest triangle using new point - supp if (expandPolytope(polytope, *nearest, &supp) != 0) diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 153581707..fc676bcf9 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -164,6 +164,106 @@ void test_distance_spherecapsule(GJKSolverType solver_type) } } +template +void test_distance_cylinder_sphere1() { + // This is a specific case that has cropped up in the wild that reaches the + // unexpected `expandPolytope()` error, where the nearest point and the new + // vertiex botih lie on an edge. It turns out that libccd incorrectly thinks + // that the edge is the nearest feature, while actually one of the + // neighbouring face is. This test confirms that the bug is fixed, by the + // function validateNearestFeatureOfPolytopeBeingEdge. + using CollisionGeometryPtr_t = std::shared_ptr>; + const S cylinder_radius = 0.03; + const S cylinder_length = 0.65; + CollisionGeometryPtr_t cylinder_geo( + new fcl::Cylinder(cylinder_radius, cylinder_length)); + Transform3 X_WC = Transform3::Identity(); + X_WC.translation() <<0.6, 0,0.325; + fcl::CollisionObject cylinder(cylinder_geo, X_WC); + + const S sphere_radius = 0.055; + CollisionGeometryPtr_t sphere_geo(new fcl::Sphere(sphere_radius)); + Transform3 X_WS = Transform3::Identity(); + // clang-format off + X_WS.matrix() << -0.9954758066974283004, -0.029586630161622884394, 0.090291470227168560414, 0.54197940181714598928, + -0.085103478665251586222, -0.14494505659711237611, -0.98577295990868651909, -0.062117502536077888464, + 0.042253002250460247602, -0.98899725069574340175, 0.14177137199407846557, 0.60162365763662117857, + 0, 0, 0, 1; + // clang-format on + fcl::CollisionObject sphere(sphere_geo, X_WS); + + fcl::DistanceRequest request; + request.gjk_solver_type = GJKSolverType::GST_LIBCCD; + request.distance_tolerance = 9.9999999999999995475e-07; + request.enable_signed_distance = true; + fcl::DistanceResult result; + + EXPECT_NO_THROW(fcl::distance(&cylinder, &sphere, request, result)); + // The two objects are penetrating. + EXPECT_NEAR(-(result.nearest_points[0] - result.nearest_points[1]).norm(), + result.min_distance, request.distance_tolerance); + // p_CPc is the position of the witness point Pc on the cylinder, measured + // and expressed in the cylinder frame C. + const Vector3 p_CPc = X_WC.inverse() * result.nearest_points[0]; + EXPECT_LE(std::abs(p_CPc(2)), cylinder_length / 2); + EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius); + // p_SPs is the position of the witness point Ps on the sphere, measured and + // expressed in the sphere frame S. + const Vector3 p_SPs = X_WS.inverse() * result.nearest_points[1]; + EXPECT_LE(p_SPs.norm(), sphere_radius); +} + +// This is a *specific* case that has cropped up in the wild that reaches the +// unexpected `expandPolytope()` error. This error was reported in +// https://github.com/flexible-collision-library/fcl/issues/319 +template +void test_distance_cylinder_box1() { + using CollisionGeometryPtr_t = std::shared_ptr; + const S cylinder_radius = 0.05; + const S cylinder_length = 0.06; + CollisionGeometryPtr_t cylinder_geo( + new fcl::Cylinder(cylinder_radius, cylinder_length)); + Transform3 X_WC = Transform3::Identity(); + X_WC.matrix() << -0.99999999997999022838, 6.2572835802045040178e-10, + 6.3260669852976095481e-06, 0.57500009756757608503, + 6.3260669851683709551e-06, -6.3943303429958554955e-10, + 0.99999999997999056145, -0.42711963046787942977, + 6.2573180158128459924e-10, 1, 6.3942912945996747041e-10, + 1.1867093358746836351, 0, 0, 0, 1; + fcl::CollisionObject cylinder(cylinder_geo, X_WC); + + const Vector3 box_size(0.025, 0.35, 1.845); + CollisionGeometryPtr_t box_geo( + new fcl::Box(box_size(0), box_size(1), box_size(2))); + Transform3 X_WB = Transform3::Identity(); + X_WB.matrix() << 6.1232339957367660359e-17, -1, 0, 0.80000000000000004441, 1, + 6.1232339957367660359e-17, 0, -0.45750000000000001776, 0, 0, 1, + 1.0224999999999999645, 0, 0, 0, 1; + fcl::CollisionObject box(box_geo, X_WB); + + fcl::DistanceRequest request; + request.gjk_solver_type = GJKSolverType::GST_LIBCCD; + request.distance_tolerance = 9.9999999999999995475e-07; + request.enable_signed_distance = true; + fcl::DistanceResult result; + + EXPECT_NO_THROW(fcl::distance(&cylinder, &box, request, result)); + EXPECT_NEAR(result.min_distance, + (result.nearest_points[0] - result.nearest_points[1]).norm(), + request.distance_tolerance); + // p_CPc is the position of the witness point Pc on the cylinder, measured + // and expressed in the cylinder frame C. + const Vector3 p_CPc = X_WC.inverse() * result.nearest_points[0]; + EXPECT_LE(std::abs(p_CPc(2)), cylinder_length / 2); + EXPECT_LE(p_CPc.template head<2>().norm(), cylinder_radius); + // p_BPb is the position of the witness point Pb on the box, measured and + // expressed in the box frame B. + const Vector3 p_BPb = X_WB.inverse() * result.nearest_points[1]; + EXPECT_TRUE((p_BPb.array().abs() <= + box_size.array() / 2 + 10 * std::numeric_limits::epsilon()) + .all()); +} + //============================================================================== GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) @@ -186,6 +286,16 @@ GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) test_distance_spherecapsule(GST_INDEP); } +GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) +{ + test_distance_cylinder_sphere1(); +} + +GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box1_ccd) +{ + test_distance_cylinder_box1(); +} + //============================================================================== int main(int argc, char* argv[]) {