From 3e3eb8523a930fd78817df442e02fe78a848de2e Mon Sep 17 00:00:00 2001 From: hongkai-dai Date: Wed, 8 May 2019 16:53:41 -0700 Subject: [PATCH] Add PointToTriangleDistanceSquare which computes the distance more robustly. --- .../gjk_libccd-inl.h | 33 ++++++++++- test/test_fcl_signed_distance.cpp | 58 +++++++++++++++++++ 2 files changed, 89 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..96cceebea 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 @@ -228,6 +228,35 @@ _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, ccdVec3Cross(d, &e, c); } +_ccd_inline ccd_real_t PointToTriangleDistanceSquared(const ccd_vec3_t& P, + const ccd_vec3_t& A, + const ccd_vec3_t& B, + const ccd_vec3_t& C, + ccd_vec3_t* witness) { + const Vector3 a(A.v[0], A.v[1], A.v[2]); + const Vector3 b(B.v[0], B.v[1], B.v[2]); + const Vector3 c(C.v[0], C.v[1], C.v[2]); + const Vector3 p(P.v[0], P.v[1], P.v[2]); + // n is the normal vector of the plane on which A, B, C live. + const Vector3 n = ((b - a).cross(c - a)).normalized(); + // q is the projection point from P to the plane on which A, B, C live. + const Vector3 q = p - n.dot(p - a) * n; + // Now determine if q is within the triangle ABC. + if ((n.cross(b - a)).dot(q - a) < 0 || (n.cross(c - b)).dot(q - b) < 0 || + (n.cross(a - c)).dot(q - c) < 0) { + // q is not inside the triangle. + return ccdVec3PointTriDist2(&P, &A, &B, &C, witness); + } else { + // q is inside the triangle. + if (witness) { + witness->v[0] = q(0); + witness->v[1] = q(1); + witness->v[2] = q(2); + } + return (p - q).squaredNorm(); + } +} + /* This is *not* an implementation of the general function: what's the nearest point on the line segment AB to the origin O? It is not intended to be. This is a limited, special case which exploits the known (or at least @@ -397,8 +426,8 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) // https://github.com/danfis/libccd/issues/55. ccd_vec3_t origin_projection_unused; - const ccd_real_t dist_squared = ccdVec3PointTriDist2( - ccd_vec3_origin, &A->v, &B->v, &C->v, &origin_projection_unused); + const ccd_real_t dist_squared = PointToTriangleDistanceSquared( + *ccd_vec3_origin, A->v, B->v, C->v, &origin_projection_unused); if (isAbsValueLessThanEpsSquared(dist_squared)) { return 1; } 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(); } //==============================================================================