Fix face normal pointing outward

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

@@ -860,6 +860,8 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
   // this corner case.
   ccdVec3Cross(&dir, &e1, &e2);
   const ccd_real_t dir_norm = std::sqrt(ccdVec3Len2(&dir));
+  ccd_vec3_t unit_dir = dir;
+  ccdVec3Scale(&unit_dir, 1.0 / dir_norm);
   // The winding of the triangle is *not* guaranteed. The normal `n = e₁ × e₂`
   // may point inside or outside. We rely on the fact that the origin lies
   // within the polytope to resolve this ambiguity. A vector from the origin to
@@ -879,44 +881,55 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
   // seems large, the fall through case of comparing the maximum distance will
   // always guarantee correctness.
   const ccd_real_t dist_tol = 0.01;
-  ccd_real_t tol = dist_tol * dir_norm;
-  ccd_real_t projection = ccdVec3Dot(&dir, &(face->edge[0]->vertex[0]->v.v));
-  if (projection < -tol) {
+  // origin_distance_to_plane computes the signed distance from the origin to
+  // the plane nᵀ * (x - v) = 0 coinciding with the triangle,  where v is a
+  // point on the triangle.
+  const ccd_real_t origin_distance_to_plane =
+      ccdVec3Dot(&unit_dir, &(face->edge[0]->vertex[0]->v.v));
+  if (origin_distance_to_plane < -dist_tol) {
     // Origin is more than `dist_tol` away from the plane, but the negative
     // value implies that the normal vector is pointing in the wrong direction;
     // flip it.
     ccdVec3Scale(&dir, ccd_real_t(-1));
-  } else if (projection >= -tol && projection <= tol) {
+  } else if (-dist_tol <= origin_distance_to_plane &&
+             origin_distance_to_plane <= dist_tol) {
     // The origin is close to the plane of the face. Pick another vertex to test
     // the normal direction.
-    ccd_real_t max_projection = -CCD_REAL_MAX;
-    ccd_real_t min_projection = CCD_REAL_MAX;
+    ccd_real_t max_distance_to_plane = -CCD_REAL_MAX;
+    ccd_real_t min_distance_to_plane = CCD_REAL_MAX;
     ccd_pt_vertex_t* v;
-    // If the magnitude of the projection is larger than tolerance, then it
-    // means one of the vertices is at least 1cm away from the plane coinciding
-    // with the face.
+    // If the magnitude of the distance_to_plane is larger than dist_tol,
+    // then it means one of the vertices is at least `dist_tol` away from the
+    // plane coinciding with the face.
     ccdListForEachEntry(&polytope->vertices, v, ccd_pt_vertex_t, list) {
-      projection = ccdVec3Dot(&dir, &(v->v.v)); 
-      if (projection > tol) {
+      // distance_to_plane is the signed distance from the
+      // vertex v->v.v to the face, i.e., distance_to_plane = nᵀ *
+      // (v->v.v - face_point). Note that origin_distance_to_plane = nᵀ *
+      // face_point.
+      const ccd_real_t distance_to_plane =
+          ccdVec3Dot(&unit_dir, &(v->v.v)) - origin_distance_to_plane;
+      if (distance_to_plane > dist_tol) {
         // The vertex is at least dist_tol away from the face plane, on the same
         // direction of `dir`. So we flip dir to point it outward from the
         // polytope.
         ccdVec3Scale(&dir, ccd_real_t(-1));
-        break;
-      } else if (projection < -tol) {
-        // The vertex is at least 1cm away from the face plane, on the opposite
-        // direction of `dir`. So `dir` points outward already.
-        break;
+        return dir;
+      } else if (distance_to_plane < -dist_tol) {
+        // The vertex is at least `dist_tol` away from the face plane, on the
+        // opposite direction of `dir`. So `dir` points outward already.
+        return dir;
       } else {
-        max_projection = std::max(max_projection, projection);
-        min_projection = std::min(min_projection, projection);
+        max_distance_to_plane =
+            std::max(max_distance_to_plane, distance_to_plane);
+        min_distance_to_plane =
+            std::min(min_distance_to_plane, distance_to_plane);
       }
     }
-    // If max_projection > |min_projection|, it means that the vertices that are 
-    // on the positive side of the plane, has a larger maximal distance than the
-    // vertices on the negative side of the plane. Thus we regard that `dir`
-    // points into the polytope. Hence we flip `dir`. 
-    if (max_projection > std::abs(min_projection)) {
+    // If max_distance_to_plane > |min_distance_to_plane|, it means that the
+    // vertices that are on the positive side of the plane, has a larger maximal
+    // distance than the vertices on the negative side of the plane. Thus we
+    // regard that `dir` points into the polytope. Hence we flip `dir`.
+    if (max_distance_to_plane > std::abs(min_distance_to_plane)) {
       ccdVec3Scale(&dir, ccd_real_t(-1));
     }
   }

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp

@@ -41,6 +41,7 @@
 
 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h"
 
+#include <array>
 #include <memory>
 
 #include <gtest/gtest.h>
@@ -92,6 +93,53 @@ class Polytope {
   ccd_pt_t* polytope_;
 };
 
+/**
+ * A tetrahedron with some specific ordering on its edges, and faces.
+ * The user should notice that due to the specific order of the edges, each face
+ * has its own orientations. Namely for some faces f, f.e(0).cross(f.e(1))
+ * points inward to the tetrahedron, for some other faces it points outward.
+ */
+class Tetrahedron : public Polytope {
+ public:
+  Tetrahedron(const std::array<fcl::Vector3<ccd_real_t>, 4>& vertices)
+      : Polytope() {
+    v().resize(4);
+    e().resize(6);
+    f().resize(4);
+    for (int i = 0; i < 4; ++i) {
+      v()[i] = ccdPtAddVertexCoords(&this->polytope(), vertices[i](0),
+                                    vertices[i](1), vertices[i](2));
+    }
+    e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
+    e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
+    e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
+    e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
+    e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
+    e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
+    f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
+    f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
+    f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
+    f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
+  }
+};
+
+std::array<fcl::Vector3<ccd_real_t>, 4> EquilateralTetrahedronVertices(
+    ccd_real_t bottom_center_x, ccd_real_t bottom_center_y,
+    ccd_real_t bottom_center_z, ccd_real_t edge_length) {
+  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
+  auto compute_vertex = [bottom_center_x, bottom_center_y, bottom_center_z,
+                         edge_length](ccd_real_t x, ccd_real_t y, ccd_real_t z,
+                                      fcl::Vector3<ccd_real_t>* vertex) {
+    *vertex << x * edge_length + bottom_center_x,
+        y * edge_length + bottom_center_y, z * edge_length + bottom_center_z;
+  };
+  compute_vertex(0.5, -0.5 / std::sqrt(3), 0, &vertices[0]);
+  compute_vertex(-0.5, -0.5 / std::sqrt(3), 0, &vertices[1]);
+  compute_vertex(0, 1 / std::sqrt(3), 0, &vertices[2]);
+  compute_vertex(0, 0, std::sqrt(2.0 / 3.0), &vertices[3]);
+  return vertices;
+}
+
 /**
   Simple equilateral tetrahedron.
 
@@ -112,53 +160,30 @@ exercise the functionality for computing an outward normal.
 
 All property accessors are *mutable*.
 */
-class EquilateralTetrahedron : public Polytope {
+class EquilateralTetrahedron : public Tetrahedron {
  public:
   EquilateralTetrahedron(ccd_real_t bottom_center_x = 0,
                          ccd_real_t bottom_center_y = 0,
                          ccd_real_t bottom_center_z = 0,
                          ccd_real_t edge_length = 1)
-      : Polytope() {
-    v().resize(4);
-    e().resize(6);
-    f().resize(4);
-    auto AddTetrahedronVertex = [bottom_center_x, bottom_center_y,
-                                 bottom_center_z, edge_length, this](
-        ccd_real_t x, ccd_real_t y, ccd_real_t z) {
-      return ccdPtAddVertexCoords(
-          &this->polytope(), x * edge_length + bottom_center_x,
-          y * edge_length + bottom_center_y, z * edge_length + bottom_center_z);
-    };
-    v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), 0);
-    v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), 0);
-    v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), 0);
-    v()[3] = AddTetrahedronVertex(0, 0, std::sqrt(2.0 / 3.0));
-    e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
-    e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
-    e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
-    e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
-    e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
-    e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
-    f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
-    f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
-    f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
-    f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
-  }
+      : Tetrahedron(EquilateralTetrahedronVertices(
+            bottom_center_x, bottom_center_y, bottom_center_z, edge_length)) {}
 };
 
+void CheckTetrahedronFaceNormal(const Tetrahedron& p) {
+  for (int i = 0; i < 4; ++i) {
+    const ccd_vec3_t n =
+        libccd_extension::faceNormalPointingOutward(&p.polytope(), &p.f(i));
+    for (int j = 0; j < 4; ++j) {
+      EXPECT_LE(ccdVec3Dot(&n, &p.v(j).v.v),
+                ccdVec3Dot(&n, &p.f(i).edge[0]->vertex[0]->v.v) +
+                    constants<ccd_real_t>::eps_34());
+    }
+  }
+}
+
 GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward) {
   // Construct a equilateral tetrahedron, compute the normal on each face.
-  auto CheckTetrahedronFaceNormal = [](const EquilateralTetrahedron& p) {
-    for (int i = 0; i < 4; ++i) {
-      const ccd_vec3_t n =
-          libccd_extension::faceNormalPointingOutward(&p.polytope(), &p.f(i));
-      for (int j = 0; j < 4; ++j) {
-        EXPECT_LE(ccdVec3Dot(&n, &p.v(j).v.v),
-                  ccdVec3Dot(&n, &p.f(i).edge[0]->vertex[0]->v.v) +
-                      constants<ccd_real_t>::eps_34());
-      }
-    }
-  };
   /*
    p1-p4: The tetrahedron is positioned so that the origin is placed on each
    face (some requiring flipping, some not)
@@ -198,6 +223,57 @@ GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward) {
   CheckTetrahedronFaceNormal(p8);
 }
 
+GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace1) {
+  // Creates a downward pointing tetrahedron which contains the origin. The
+  // origin is just below the "top" face of this tetrahedron. The remaining
+  // vertex is far enough away from the top face that it is considered a
+  // reliable witness to determine the direction of the face's normal. The top
+  // face is not quite parallel with the z = 0 plane. This test captures the
+  // failure condition reported in PR 334 -- a logic error made it so the
+  // reliable witness could be ignored.
+  const double face0_origin_distance = 0.005;
+  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
+  vertices[0] << 0.5, -0.5, face0_origin_distance;
+  vertices[1] << 0, 1, face0_origin_distance;
+  vertices[2] << -0.5, -0.5, face0_origin_distance;
+  vertices[3] << 0, 0, -1;
+  Eigen::AngleAxisd rotation(0.05 * M_PI, Eigen::Vector3d::UnitX());
+  for (int i = 0; i < 4; ++i) {
+    vertices[i] = rotation * vertices[i];
+  }
+  Tetrahedron p(vertices);
+  {
+    // Make sure that the e₀ × e₁ points upward.
+    ccd_vec3_t f0_e0, f0_e1;
+    ccdVec3Sub2(&f0_e0, &(p.f(0).edge[0]->vertex[1]->v.v),
+                &(p.f(0).edge[0]->vertex[0]->v.v));
+    ccdVec3Sub2(&f0_e1, &(p.f(0).edge[1]->vertex[1]->v.v),
+                &(p.f(0).edge[1]->vertex[0]->v.v));
+    ccd_vec3_t f0_e0_cross_e1;
+    ccdVec3Cross(&f0_e0_cross_e1, &f0_e0, &f0_e1);
+    EXPECT_GE(f0_e0_cross_e1.v[2], 0);
+  }
+  CheckTetrahedronFaceNormal(p);
+}
+
+GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace2) {
+  // Similar to faceNormalPointingOutwardOriginNearFace1 with an important
+  // difference: the fourth vertex is no longer a reliable witness; it lies
+  // within the distance tolerance. However, it is unambiguously farther off the
+  // plane of the top face than those that form the face. This confirms that
+  // when there are no obviously reliable witness that the most distant point
+  // serves.
+  const double face0_origin_distance = 0.005;
+  std::array<fcl::Vector3<ccd_real_t>, 4> vertices;
+  vertices[0] << 0.5, -0.5, face0_origin_distance;
+  vertices[1] << 0, 1, face0_origin_distance;
+  vertices[2] << -0.5, -0.5, face0_origin_distance;
+  vertices[3] << 0, 0, -0.001;
+
+  Tetrahedron p(vertices);
+  CheckTetrahedronFaceNormal(p);
+}
+
 GTEST_TEST(FCL_GJK_EPA, supportEPADirection) {
   auto CheckSupportEPADirection = [](
       const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt,