From aca98050348fa296ccd5f29304b74e135e83144f Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Mon, 6 May 2019 16:26:08 -0700 Subject: [PATCH] In DoSimplex4, check if O and A are on the same the same side of BCD --- .../gjk_libccd-inl.h | 20 ++++++- test/test_fcl_signed_distance.cpp | 58 +++++++++++++++++++ 2 files changed, 76 insertions(+), 2 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..aa59b10e3 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 @@ -523,19 +523,30 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) ccdVec3Cross(&ACD, &AC, &AD); ccdVec3Cross(&ADB, &AD, &AB); + ccd_vec3_t BO; + ccdVec3Copy(&BO, &B->v); + ccdVec3Scale(&BO, -CCD_ONE); + ccd_vec3_t BC, BD; + ccdVec3Sub2(&BC, &C->v, &B->v); + ccdVec3Sub2(&BD, &D->v, &B->v); + ccd_vec3_t BCD; + ccdVec3Cross(&BCD, &BC, &BD); + // side (positive or negative) of B, C, D relative to planes ACD, ADB // and ABC respectively B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB)); C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC)); D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD)); + const int A_on_BCD = ccdSign(-ccdVec3Dot(&BCD, &AB)); // whether origin is on same side of ACD, ADB, ABC as B, C, D // respectively AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD; AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB; AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC; + const bool BA_O = ccdSign(ccdVec3Dot(&BCD, &BO)) == A_on_BCD; - if (AB_O && AC_O && AD_O){ + if (AB_O && AC_O && AD_O && BA_O) { // origin is in tetrahedron return 1; @@ -554,11 +565,16 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) ccdSimplexSet(simplex, 0, B); ccdSimplexSet(simplex, 2, A); ccdSimplexSetSize(simplex, 3); - }else{ // (!AD_O) + } else if (!AD_O) { // (!AD_O) ccdSimplexSet(simplex, 0, C); ccdSimplexSet(simplex, 1, B); ccdSimplexSet(simplex, 2, A); ccdSimplexSetSize(simplex, 3); + } else if (!BA_O) { + ccdSimplexSet(simplex, 0, C); + ccdSimplexSet(simplex, 1, D); + ccdSimplexSet(simplex, 2, B); + ccdSimplexSetSize(simplex, 4); } return doSimplex3(simplex, dir); diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index 1fdb28b08..0edbb393d 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -379,6 +379,62 @@ 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 `validateNearestFeatureOfPolytopeBeingEdge` error. This error was +// reported in https://github.com/flexible-collision-library/fcl/issues/399 +template +void test_distance_box_box3() { + const Vector3 box1_size(0.23768037557601928711, 0.15702305734157562256, + 0.25); + Transform3 X_WB1 = Transform3::Identity(); + // clang-format off + X_WB1.matrix() << -0.099432387353076273628, 0.99504432079443971837, 0, 0.29526406526565551758, + -0.99504432079443971837, -0.099432387353076273628, 0, -0.013230856508016586304, + 0, 0, 1, 0, + 0, 0, 0, 1; + // clang-format on + + const Vector3 box2_size(0.010000000000000000208, 0.78165709972381591797, + 0.25); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << 0.99506598442099225554, 0.099215354901756730444, 0, 0.22759585694545214629, +-0.099215354901756730444, 0.99506598442099225554, 0, 0.14974093549228112421, + 0, 0, 1, 0, + 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 `validateNearestFeatureOfPolytopeBeingEdge` error. This error was +// reported in https://github.com/flexible-collision-library/fcl/issues/399 +template +void test_distance_box_box4() { + const Vector3 box1_size(0.23431687057018280029, 0.16946108639240264893, + 0.25); + + Transform3 X_WB1 = Transform3::Identity(); + // clang-format off + X_WB1.matrix() << 0.1612859416241739785, 0.98690771860108761349, 0, -0.20553381741046905518, +-0.98690771860108761349, 0.1612859416241739785, 0, 0.25915133953094482422, + 0, 0, 1, 0, + 0, 0, 0, 1; + // clang-format on + + const Vector3 box2_size(0.010000000000000000208, 1.4315791130065917969, + 0.25); + Transform3 X_WB2 = Transform3::Identity(); + // clang-format off + X_WB2.matrix() << +-0.16128686395059221859, -0.98690756786893829577, 0, -0.38020379396316156262, + 0.98690756786893829577, -0.16128686395059221859, 0, 0.10682664338807786042, + 0, 0, 1, 0, + 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) @@ -413,6 +469,8 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) { GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) { test_distance_box_box1(); test_distance_box_box2(); + test_distance_box_box3(); + test_distance_box_box4(); } //==============================================================================