From f723bd44bd87b42780d3f0f4aab84f6356e37cf1 Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Fri, 6 Sep 2019 09:54:55 -0700 Subject: [PATCH 1/5] try to reproduce box-box vertex colinear bug. --- test/test_fcl_signed_distance.cpp | 76 ++++++++++++++++++++----------- 1 file changed, 49 insertions(+), 27 deletions(-) diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 1fdb28b08..af4140ad6 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -379,40 +379,62 @@ void test_distance_box_box2() { test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2); } -//============================================================================== - -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); -} +template +void test_distance_box_box3() { + const Vector3 box1_size(0.6, 0.6, 0.1); + Transform3 X_WB1 = Transform3::Identity(); + X_WB1.matrix() << 0.99991900264432043155, -0.012705114136322817608, 0.00075380738289000973843, 0, + 0.012706303685710157447, 0.99991799982123408252, -0.0015948291911753885355, 0.00026625771951151698536, +-0.00073348308364803505629, 0.001604278119755673691, 0.99999844414592997666, 2.0000000607751289294, + 0, 0, 0, 1; -GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) -{ - test_distance_spherecapsule(GST_LIBCCD); -} + const Vector3 box2_size(0.5, 10, 2); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 1, + 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_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_box_ccd) { - test_distance_cylinder_box(); -} +//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); +//} +// +//GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) +//{ +// test_distance_cylinder_sphere1(); +//} +// +//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(); +// test_distance_box_box1(); +// test_distance_box_box2(); + test_distance_box_box3(); } //============================================================================== From 13a008185d0a9aef40859e4e67984b25db522864 Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Fri, 6 Sep 2019 17:35:17 -0700 Subject: [PATCH 2/5] Add the tests that would fail. --- .../test_gjk_libccd-inl_epa.cpp | 34 ++++++++ test/test_fcl_signed_distance.cpp | 80 +++++++++---------- 2 files changed, 72 insertions(+), 42 deletions(-) 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..7f27c3906 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,40 @@ GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) { {0, 1}, {0}); } +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]. + 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) { + EquilateralTetrahedron tet1(-0.05, -0.13, 0.12); + CheckComputeVisiblePatchColinearNewVertex(tet1, 1.9); + 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 af4140ad6..7f1e2f15b 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -316,7 +316,7 @@ void test_distance_box_box_helper(const Vector3& box1_size, request.enable_signed_distance = true; fcl::DistanceResult result; - ASSERT_NO_THROW(fcl::distance(&box1, &box2, request, result)); + fcl::distance(&box1, &box2, request, result); EXPECT_NEAR(abs(result.min_distance), (result.nearest_points[0] - result.nearest_points[1]).norm(), request.distance_tolerance); @@ -381,20 +381,21 @@ void test_distance_box_box2() { template void test_distance_box_box3() { - const Vector3 box1_size(0.6, 0.6, 0.1); + const Vector3 box1_size(0.49, 0.05, 0.21); Transform3 X_WB1 = Transform3::Identity(); - X_WB1.matrix() << 0.99991900264432043155, -0.012705114136322817608, 0.00075380738289000973843, 0, - 0.012706303685710157447, 0.99991799982123408252, -0.0015948291911753885355, 0.00026625771951151698536, --0.00073348308364803505629, 0.001604278119755673691, 0.99999844414592997666, 2.0000000607751289294, - 0, 0, 0, 1; - - const Vector3 box2_size(0.5, 10, 2); + // 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() << 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 1, - 0, 0, 0, 1; + 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); @@ -402,38 +403,33 @@ void test_distance_box_box3() { //============================================================================== -//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); -//} -// -//GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) -//{ -// test_distance_cylinder_sphere1(); -//} -// -//GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) { -// test_distance_cylinder_box(); -//} +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); +} + +GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) { + test_distance_cylinder_sphere1(); +} + +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(); + test_distance_box_box1(); + test_distance_box_box2(); test_distance_box_box3(); } From d6a287b0c9148d15e1fd47f8ab203abd7da87164 Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Fri, 6 Sep 2019 17:35:51 -0700 Subject: [PATCH 3/5] If the edge is colinear with the query point, then both faces should be visible --- .../gjk_libccd-inl.h | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 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..10b0f95c0 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,18 +1186,15 @@ 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 the query point is colinear with the edge, then both neighbouring + // faces of this edge (namely face f and g) should be visible. This edge + // is an internal edge. Othewise, this edge is a border edge. + if (!triangle_area_is_zero(query_point, + f.edge[edge_index]->vertex[0]->v.v, + f.edge[edge_index]->vertex[1]->v.v)) { + border_edges->insert(f.edge[edge_index]); + return; } - border_edges->insert(f.edge[edge_index]); - return; } visible_faces->insert(g); internal_edges->insert(f.edge[edge_index]); From dace2850b835f94555ff0dfac52549aea9b8c7a5 Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Tue, 10 Sep 2019 23:13:32 -0700 Subject: [PATCH 4/5] Address Sean's comments. --- .../gjk_libccd-inl.h | 20 ++++++++++++++++--- .../test_gjk_libccd-inl_epa.cpp | 18 ++++++++++++++++- test/test_fcl_signed_distance.cpp | 18 +++++++++-------- 3 files changed, 44 insertions(+), 12 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 10b0f95c0..9b4888275 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 @@ -1186,12 +1186,26 @@ static void ComputeVisiblePatchRecursive( if (!isOutsidePolytopeFace(&polytope, g, &query_point)) { // Cannot see the neighbouring face from the new vertex. - // If the query point is colinear with the edge, then both neighbouring - // faces of this edge (namely face f and g) should be visible. This edge - // is an internal edge. Othewise, this edge is a border edge. if (!triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v, f.edge[edge_index]->vertex[1]->v.v)) { + // 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. border_edges->insert(f.edge[edge_index]); return; } 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 7f27c3906..289d8a856 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,10 +690,22 @@ 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], + // 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]. ccd_vec3_t query_point; for (int i = 0; i < 3; ++i) { @@ -718,8 +730,12 @@ void CheckComputeVisiblePatchColinearNewVertex(EquilateralTetrahedron& tet, } 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 indpendently confirmed -- + // colinearity doesn't matter. EquilateralTetrahedron tet2(0.1, 0.2, 0.3); CheckComputeVisiblePatchColinearNewVertex(tet2, 0.3); } diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 7f1e2f15b..200fa0421 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -316,7 +316,7 @@ void test_distance_box_box_helper(const Vector3& box1_size, request.enable_signed_distance = true; fcl::DistanceResult result; - fcl::distance(&box1, &box2, request, result); + ASSERT_NO_THROW(fcl::distance(&box1, &box2, request, result)); EXPECT_NEAR(abs(result.min_distance), (result.nearest_points[0] - result.nearest_points[1]).norm(), request.distance_tolerance); @@ -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, @@ -380,7 +380,7 @@ void test_distance_box_box2() { } template -void test_distance_box_box3() { +void test_distance_box_box_regression3() { const Vector3 box1_size(0.49, 0.05, 0.21); Transform3 X_WB1 = Transform3::Identity(); // clang-format off @@ -427,10 +427,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(); - test_distance_box_box3(); +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(); } //============================================================================== From fc19887b29dc7a4874624d278a28f573bd2c4e8e Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Wed, 11 Sep 2019 17:00:03 -0700 Subject: [PATCH 5/5] Address Sherm's comments. --- .../gjk_libccd-inl.h | 44 ++++++++++++------- .../test_gjk_libccd-inl_epa.cpp | 4 +- test/test_fcl_signed_distance.cpp | 3 ++ 3 files changed, 33 insertions(+), 18 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 9b4888275..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 @@ -1189,27 +1189,37 @@ static void ComputeVisiblePatchRecursive( if (!triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v, f.edge[edge_index]->vertex[1]->v.v)) { - // 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. + // 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; } } + // 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 289d8a856..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 @@ -707,6 +707,8 @@ void CheckComputeVisiblePatchColinearNewVertex(EquilateralTetrahedron& tet, // 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]; @@ -734,7 +736,7 @@ GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchColinearNewVertex) { // 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 indpendently confirmed -- + // 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); diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 200fa0421..6a89c522d 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -379,6 +379,9 @@ void test_distance_box_box_regression2() { 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);