use the actual distance instead of the squared distance to determine …

…if the origin is on the facet of the simplex. (#365)

Use the actual distance instead of the squared distance to determine if the origin is on the facet of the simplex.

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

@@ -348,13 +348,18 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
   return 0;
 }
 
+static bool isAbsValueLessThanEpsSquared(ccd_real_t val) {
+  return std::abs(val) < std::numeric_limits<ccd_real_t>::epsilon() *
+                             std::numeric_limits<ccd_real_t>::epsilon();
+}
+
 static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
 {
   const ccd_support_t *A, *B, *C, *D;
   ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
   int B_on_ACD, C_on_ADB, D_on_ABC;
   int AB_O, AC_O, AD_O;
-  ccd_real_t dist;
+  ccd_real_t dist_squared;
 
   // get last added as A
   A = ccdSimplexLast(simplex);
@@ -366,25 +371,25 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
   // check if tetrahedron is really tetrahedron (has volume > 0)
   // if it is not simplex can't be expanded and thus no intersection is
   // found
-  dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
-  if (ccdIsZero(dist)){
+  dist_squared = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
+  if (isAbsValueLessThanEpsSquared(dist_squared)) {
     return -1;
   }
 
   // check if origin lies on some of tetrahedron's face - if so objects
   // intersect
-  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
-  if (ccdIsZero(dist))
-    return 1;
-  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
-  if (ccdIsZero(dist))
-    return 1;
-  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
-  if (ccdIsZero(dist))
-    return 1;
-  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
-  if (ccdIsZero(dist))
-    return 1;
+  dist_squared =
+      ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
+  if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
+  dist_squared =
+      ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
+  if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
+  dist_squared =
+      ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
+  if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;
+  dist_squared =
+      ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
+  if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;
 
   // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
   ccdVec3Copy(&AO, &A->v);