From 2112037d3f0490f83c9e2d237886eb113b7b0d31 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Fri, 26 Apr 2019 10:08:19 -0700 Subject: [PATCH] Fix epa box support (#397) Make sure that the support point of a box is always one of the box vertices, not the middle of an edge or a face. This helps to reduce the "degenerate triangle issue" reported in #395 --- .../gjk_libccd-inl.h | 25 ++++++- .../test_gjk_libccd-inl_epa.cpp | 14 ++-- test/test_fcl_capsule_box_1.cpp | 2 +- test/test_fcl_signed_distance.cpp | 73 ++++++++++++++----- 4 files changed, 83 insertions(+), 31 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 8b51e0f0d..b39f08825 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,6 +1186,17 @@ static void ComputeVisiblePatchRecursive( // g is not a visible face 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."); + } border_edges->insert(f.edge[edge_index]); return; } @@ -2241,13 +2252,21 @@ static void convexToGJK(const Convex& s, const Transform3& tf, static inline void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { + // Use a customized sign function, so that the support of the box always + // appears in one of the box vertices. + // Picking support vertices on the interior of faces/edges can lead to + // degenerate triangles in the EPA algorithm and are no more correct than just + // picking box corners. + auto sign = [](ccd_real_t x) -> ccd_real_t { + return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0); + }; const ccd_box_t* o = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &o->rot_inv); - ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], - ccdSign(ccdVec3Y(&dir)) * o->dim[1], - ccdSign(ccdVec3Z(&dir)) * o->dim[2]); + ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->dim[0], + sign(ccdVec3Y(&dir)) * o->dim[1], + sign(ccdVec3Z(&dir)) * o->dim[2]); ccdQuatRotVec(v, &o->rot); ccdVec3Add(v, &o->pos); } 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 b7c2a2822..909b5072f 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 @@ -1289,7 +1289,7 @@ void SetUpBoxToBox(const Transform3& X_WF, void** o1, void** o2, ccd_t* ccd, fcl::Box box1(box1_size); fcl::Box box2(box2_size); *o1 = GJKInitializer>::createGJKObject(box1, X_WB1); - *o2 = GJKInitializer>::createGJKObject(box1, X_WB2); + *o2 = GJKInitializer>::createGJKObject(box2, X_WB2); // Set up ccd solver. CCD_INIT(ccd); @@ -1329,20 +1329,20 @@ void TestSimplexToPolytope3InGivenFrame(const Transform3& X_WF) { // We find three points Pa1, Pb1, Pc1 on box 1, and three points Pa2, Pb2, Pc2 // on box 2, such that the 2-simplex with vertices (Pa1 - Pa2, Pb1 - Pb2, // Pc1 - Pc2) contains the origin. - const Vector3 p_FPa1(-1, -1, -1); - const Vector3 p_FPa2(-0.1, 0.5, -1); + const Vector3 p_FPa1(-1, -1, 0.1); + const Vector3 p_FPa2(-0.1, 0.5, 0.1); pts[0].v = ToCcdVec3(p_FPa1 - p_FPa2); pts[0].v1 = ToCcdVec3(p_FPa1); pts[0].v2 = ToCcdVec3(p_FPa2); - const Vector3 p_FPb1(-1, 1, -1); - const Vector3 p_FPb2(-0.1, 0.5, -1); + const Vector3 p_FPb1(-1, 1, 0.1); + const Vector3 p_FPb2(-0.1, 0.5, 0.1); pts[1].v = ToCcdVec3(p_FPb1 - p_FPb2); pts[1].v1 = ToCcdVec3(p_FPb1); pts[1].v2 = ToCcdVec3(p_FPb2); - const Vector3 p_FPc1(1, 1, -1); - const Vector3 p_FPc2(-0.1, 0.5, -1); + const Vector3 p_FPc1(1, 1, 0.1); + const Vector3 p_FPc2(-0.1, 0.5, 0.1); pts[2].v = ToCcdVec3(p_FPc1 - p_FPc2); pts[2].v1 = ToCcdVec3(p_FPc1); pts[2].v2 = ToCcdVec3(p_FPc2); diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp index 0be5633cf..e6e7cd026 100644 --- a/test/test_fcl_capsule_box_1.cpp +++ b/test/test_fcl_capsule_box_1.cpp @@ -117,7 +117,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd) { - test_distance_capsule_box(fcl::GJKSolverType::GST_LIBCCD, 1e-6, 1e-4); + test_distance_capsule_box(fcl::GJKSolverType::GST_LIBCCD, 1e-8, 4e-4); } GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_indep) diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index d8e5889f6..1fdb28b08 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -296,34 +296,18 @@ void test_distance_cylinder_box() { box_size, X_WB); } -// This is a *specific* case that has cropped up in the wild that reaches the -// 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_helper(const Vector3& box1_size, + const Transform3& X_WB1, + const Vector3& box2_size, + const Transform3& X_WB2) { using CollisionGeometryPtr_t = std::shared_ptr; - const Vector3 box1_size(0.03, 0.12, 0.1); CollisionGeometryPtr_t box1_geo( new fcl::Box(box1_size(0), box1_size(1), box1_size(2))); - Transform3 X_WB1 = Transform3::Identity(); - X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978, - -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016, - -3.0627939739957803544e-08, 6.4729926918527511769e-08, - -0.48500002215636439651, -6.4729927722963847085e-08, - -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356, - 0, 0, 0, 1; fcl::CollisionObject box1(box1_geo, X_WB1); - const Vector3 box2_size(0.025, 0.35, 1.845); CollisionGeometryPtr_t box2_geo( new fcl::Box(box2_size(0), box2_size(1), box2_size(2))); - Transform3 X_WB2 = Transform3::Identity(); - // clang-format off - X_WB2.matrix() << 0, -1, 0, 0.8, - 1, 0, 0, -0.4575, - 0, 0, 1, 1.0225, - 0, 0, 0, 1; - // clang-format on fcl::CollisionObject box2(box2_geo, X_WB2); fcl::DistanceRequest request; @@ -347,6 +331,54 @@ void test_distance_box_box1() { EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all()); } +// This is a *specific* case that has cropped up in the wild that reaches the +// unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was +// reported in https://github.com/flexible-collision-library/fcl/issues/388 +template +void test_distance_box_box1() { + const Vector3 box1_size(0.03, 0.12, 0.1); + Transform3 X_WB1 = Transform3::Identity(); + X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978, + -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016, + -3.0627939739957803544e-08, 6.4729926918527511769e-08, + -0.48500002215636439651, -6.4729927722963847085e-08, + -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356, + 0, 0, 0, 1; + + const Vector3 box2_size(0.025, 0.35, 1.845); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << 0, -1, 0, 0.8, + 1, 0, 0, -0.4575, + 0, 0, 1, 1.0225, + 0, 0, 0, 1; + // clang-format on + 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 `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() { + const Vector3 box1_size(0.46, 0.48, 0.01); + Transform3 X_WB1 = Transform3::Identity(); + X_WB1.matrix() << 1,0,0, -0.72099999999999997424, + 0,1,0, -0.77200000000000001954, + 0,0,1, 0.81000000000000005329, + 0,0,0,1; + + const Vector3 box2_size(0.049521, 0.146, 0.0725); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << 0.10758262492983036718, -0.6624881850015212903, -0.74130653817877356637, -0.42677133002999478872, + 0.22682184885125472595, -0.709614040775253474, 0.6670830248314786326, -0.76596851247746788882, +-0.96797615037608542021, -0.23991106241273435495, 0.07392465377049164954, 0.80746731400091054098, + 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) @@ -380,6 +412,7 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) { GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) { test_distance_box_box1(); + test_distance_box_box2(); } //==============================================================================