From 08d0bb25c290a4a5e713851d2a5ac514e4355801 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Thu, 12 Sep 2019 09:34:52 -0700 Subject: [PATCH] Resolves the error in EPA when the query point is colinear with an edge (#417) When the query point is colinear with an edge, treat this edge as an internal edge, and continue to expand the boundary. --- .../gjk_libccd-inl.h | 44 ++++++++++----- .../test_gjk_libccd-inl_epa.cpp | 52 ++++++++++++++++++ test/test_fcl_signed_distance.cpp | 53 +++++++++++++------ 3 files changed, 122 insertions(+), 27 deletions(-) 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 b39f08825..3f7e8e33b 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 @@ -1160,7 +1160,6 @@ static bool ComputeVisiblePatchRecursiveSanityCheck( return true; } #endif - /** * This function contains the implementation detail of ComputeVisiblePatch() * function. It should not be called by any function other than @@ -1187,19 +1186,40 @@ static void ComputeVisiblePatchRecursive( if (!isOutsidePolytopeFace(&polytope, g, &query_point)) { // Cannot see the neighbouring face from the new vertex. - // TODO(hongkai.dai@tri.global): when the new vertex is colinear with a - // border edge, we should remove the degenerate triangle. We could remove - // the middle vertex on that line from the polytope, and then reconnect - // the polytope. - if (triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v, - f.edge[edge_index]->vertex[1]->v.v)) { - FCL_THROW_FAILED_AT_THIS_CONFIGURATION( - "The new vertex is colinear with an existing edge. The added " - "triangle would be degenerate."); + if (!triangle_area_is_zero(query_point, + f.edge[edge_index]->vertex[0]->v.v, + f.edge[edge_index]->vertex[1]->v.v)) { + // If query point is outside of the face g, and the triangle + // (query_point, v[0], v[1]) has non-zero area, then the edge + // f.edge[edge_index] is a border edge, and we will connect the query + // point with this border edge to form a triangle. Otherwise, this edge + // is not a border edge. + border_edges->insert(f.edge[edge_index]); + return; } - border_edges->insert(f.edge[edge_index]); - return; } + // We regard the edge f.edge[edge_index] not as an internal edge (not a + // border edge), if it satisfies one of the following two conditions + // 1. The face g is visible to the query point. + // 2. The triangle formed by the edge and the query point has zero area. + // The first condition is obvious. Here we explain the second condition: + // For this triangle to have no area, the query point must be co-linear with + // a candidate border edge. That means it is simultaneously co-planar with + // the two faces adjacent to that edge. But to be in this branch, one face + // was considered to be visible and the other to not be visible -- an + // inconsistent classification. + + // The solution is to unify that classification. We can consider both + // faces as being visible or not. If we were to consider them not + // visible, we would shrink the size of the visible patch (making this + // slightly faster), but would risk introducing co-planar faces into the + // polytope. We choose to consider both faces as being visible. At the + // cost of a patch boundary with more edges, we guarantee that we don't + // add co-planar faces. + + // It may be that co-planar faces are permissible and a smaller + // patch is preferred. This is still an open problem.For now, we go with + // the "safe" choice. visible_faces->insert(g); internal_edges->insert(f.edge[edge_index]); for (int i = 0; i < 3; ++i) { diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp index 909b5072f..d87c18fd7 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp @@ -690,6 +690,58 @@ GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) { {0, 1}, {0}); } +/* + * Given an equilateral tetrahedron, create a query point that is co-linear with + * edge 0 as q = v₀ + ρ(v₀ - v₁), confirms that the correct tetrahedra faces are + * included in the visible patch. Point q is co-linear with edge 0 which is + * adjacent to faces f0 and f1. Face f3 is trivially visible from q. + * + * If the query point is co-linear with a tet edge, then both adjacent faces + * should be visible. The behavior is sensitive to numerical accuracy issues and + * we expose rho (ρ) as a parameter so that different scenarios can easily be + * authored which exercise different code paths to determine visibility. (In the + * code, "visibility" may be determined by multiple tests.) + */ +void CheckComputeVisiblePatchColinearNewVertex(EquilateralTetrahedron& tet, + double rho) { + // A new vertex is colinear with an edge e[0] of the tetrahedron. The border + // edges should be e[1], e[4], e[5]. The visible faces should be f[0], f[1], + // f[3], and the internal edges should be e[0], e[2], e[3]. + // For the numbering of the edges/vertices/faces in the equilateral + // tetrahedron, please refer to the documentation of EquilateralTetrahedron. + ccd_vec3_t query_point; + for (int i = 0; i < 3; ++i) { + query_point.v[i] = (1 + rho) * tet.v(0).v.v.v[i] - rho * tet.v(1).v.v.v[i]; + } + std::unordered_set border_edges; + std::unordered_set visible_faces; + std::unordered_set internal_edges; + libccd_extension::ComputeVisiblePatch(tet.polytope(), tet.f(3), query_point, + &border_edges, &visible_faces, + &internal_edges); + + EXPECT_EQ(border_edges.size(), 3u); + EXPECT_EQ(border_edges, std::unordered_set( + {&(tet.e(1)), &(tet.e(4)), &(tet.e(5))})); + EXPECT_EQ(visible_faces.size(), 3u); + EXPECT_EQ(visible_faces, std::unordered_set( + {&(tet.f(0)), &(tet.f(1)), &(tet.f(3))})); + EXPECT_EQ(internal_edges.size(), 3u); + EXPECT_EQ(internal_edges, std::unordered_set( + {&(tet.e(0)), &(tet.e(2)), &(tet.e(3))})); +} + +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchColinearNewVertex) { + // Case 1: Visibility of faces f0 and f1 is not immediately apparent -- + // requires recognition that q, v0, and v1 are colinear. + EquilateralTetrahedron tet1(-0.05, -0.13, 0.12); + CheckComputeVisiblePatchColinearNewVertex(tet1, 1.9); + // Case 2: Visibility of faces f0 and f1 are independently confirmed -- + // colinearity doesn't matter. + EquilateralTetrahedron tet2(0.1, 0.2, 0.3); + CheckComputeVisiblePatchColinearNewVertex(tet2, 0.3); +} + // Tests that the sanity check causes `ComputeVisiblePatch()` to throw in // debug builds. GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchSanityCheck) { diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 1fdb28b08..6a89c522d 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -335,7 +335,7 @@ void test_distance_box_box_helper(const Vector3& box1_size, // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was // reported in https://github.com/flexible-collision-library/fcl/issues/388 template -void test_distance_box_box1() { +void test_distance_box_box_regression1() { const Vector3 box1_size(0.03, 0.12, 0.1); Transform3 X_WB1 = Transform3::Identity(); X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978, @@ -360,7 +360,7 @@ void test_distance_box_box1() { // unexpected `triangle_size_is_zero` error. This error was // reported in https://github.com/flexible-collision-library/fcl/issues/395 template -void test_distance_box_box2() { +void test_distance_box_box_regression2() { const Vector3 box1_size(0.46, 0.48, 0.01); Transform3 X_WB1 = Transform3::Identity(); X_WB1.matrix() << 1,0,0, -0.72099999999999997424, @@ -379,30 +379,50 @@ void test_distance_box_box2() { test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2); } +// This is a *specific* case that has cropped up in the wild that reaches the +// unexpected `query point colinear with the edge` error. This error was +// reported in https://github.com/flexible-collision-library/fcl/issues/415 +template +void test_distance_box_box_regression3() { + const Vector3 box1_size(0.49, 0.05, 0.21); + Transform3 X_WB1 = Transform3::Identity(); + // clang-format off + X_WB1.matrix() << 4.8966386501092529215e-12, -1,0,-0.43999999999999994671, + 1, 4.8966386501092529215e-12,0,-0.61499999999858001587, + 0,0,1,0.35499999999999998224, + 0,0,0,1; + // clang-format on + const Vector3 box2_size(0.035, 0.12, 0.03); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << 0.83512153565236335595, -0.55006546945762568868, -9.4542360608233572896e-16, -0.40653441507331000704, + 0.55006546945762568868, 0.83512153565236313391, 1.1787444236552387666e-15, -0.69166166923735727945, +1.2902271444330665572e-16, -1.4878153530113264589e-15, 1, 0.43057093858718892276, + 0, 0, 0, 1; + // clang-format on + test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2); + +} + //============================================================================== -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) -{ +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) { test_distance_spheresphere(GST_LIBCCD); } -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) -{ +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) { test_distance_spheresphere(GST_INDEP); } -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) -{ +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) { test_distance_spherecapsule(GST_LIBCCD); } -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) -{ +GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) { test_distance_spherecapsule(GST_INDEP); } -GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) -{ +GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) { test_distance_cylinder_sphere1(); } @@ -410,9 +430,12 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) { test_distance_cylinder_box(); } -GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) { - test_distance_box_box1(); - test_distance_box_box2(); +GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) { + // A collection of scnarios observed in practice that have created error + // conditions in previous commits of the code. Each test is a unique instance. + test_distance_box_box_regression1(); + test_distance_box_box_regression2(); + test_distance_box_box_regression3(); } //==============================================================================