[geometry] Add face_normal() to SurfaceMesh, and fix centroid() after…

… TransformVertices(). (#12101)

geometry/proximity/surface_mesh.h

@@ -207,10 +207,12 @@ class SurfaceMesh {
    */
   SurfaceMesh(std::vector<SurfaceFace>&& faces,
               std::vector<SurfaceVertex<T>>&& vertices)
-      : faces_(std::move(faces)), vertices_(std::move(vertices)),
-        area_(faces_.size()) {  // Pre-allocate here, not yet calculated.
+      : faces_(std::move(faces)),
+        vertices_(std::move(vertices)),
+        area_(faces_.size()),  // Pre-allocate here, not yet calculated.
+        face_normals_(faces_.size()) {  // Pre-allocate, not yet calculated.
     SetReferringTriangles();
-    CalcAreasAndCentroid();
+    CalcAreasNormalsAndCentroid();
   }
 
   /** Transforms the vertices of this mesh from its initial frame M to the new
@@ -220,6 +222,10 @@ class SurfaceMesh {
     for (auto& v : vertices_) {
       v.TransformInPlace(X_NM);
     }
+    for (auto& n : face_normals_) {
+      n = X_NM.rotation() * n;
+    }
+    p_MSc_ = X_NM * p_MSc_;
   }
 
   /** Reverses the ordering of all the faces' indices -- see
@@ -229,6 +235,9 @@ class SurfaceMesh {
     for (auto& f : faces_) {
       f.ReverseWinding();
     }
+    for (auto& n : face_normals_) {
+      n = -n;
+    }
   }
 
   /** Returns the number of triangular elements in the mesh.
@@ -243,6 +252,16 @@ class SurfaceMesh {
    */
   const T& total_area() const { return total_area_; }
 
+  /** Returns the unit face normal vector of a triangle. It respects the
+   right-handed normal rule. A near-zero-area triangle may get an unreliable
+   normal vector. A zero-area triangle will get a zero vector.
+   @pre f ∈ {0, 1, 2,..., num_faces()-1}.
+   */
+  const Vector3<T>& face_normal(SurfaceFaceIndex f) const {
+    DRAKE_DEMAND(0 <= f && f < num_faces());
+    return face_normals_[f];
+  }
+
   /** Returns the area-weighted geometric centroid of this surface mesh. The
    returned value is the position vector p_MSc from M's origin to the
    centroid Sc, expressed in frame M. (M is the frame in which this mesh's
@@ -345,9 +364,9 @@ class SurfaceMesh {
   //
 
  private:
-  // Calculates the areas of each triangle, the total area, and the centorid of
-  // the surface.
-  void CalcAreasAndCentroid();
+  // Calculates the areas and face normals of each triangle, the total area,
+  // and the centroid of the surface.
+  void CalcAreasNormalsAndCentroid();
 
   // Determines the triangular faces that refer to each vertex.
   void SetReferringTriangles() {
@@ -374,13 +393,16 @@ class SurfaceMesh {
   std::vector<T> area_;
   T total_area_{};
 
+  // Face normal vector of the triangles.
+  std::vector<Vector3<T>> face_normals_;
+
   // Area-weighted geometric centroid Sc of the surface mesh as an offset vector
   // from the origin of Frame M to point Sc, expressed in Frame M.
   Vector3<T> p_MSc_;
 };
 
 template <class T>
-void SurfaceMesh<T>::CalcAreasAndCentroid() {
+void SurfaceMesh<T>::CalcAreasNormalsAndCentroid() {
   total_area_ = 0;
   p_MSc_.setZero();
 
@@ -393,10 +415,18 @@ void SurfaceMesh<T>::CalcAreasAndCentroid() {
     const auto r_UW_M = r_MC - r_MA;
 
     const auto cross = r_UV_M.cross(r_UW_M);
-    const T face_area = T(0.5) * cross.norm();
+    const T norm = cross.norm();
+    const T face_area = T(0.5) * norm;
     area_[f] = face_area;
     total_area_ += face_area;
 
+    // TODO(DamrongGuoy): Provide a mechanism for users to set the face
+    //  normals of skinny triangles since this calculation is not reliable.
+    //  For example, the code that creates ContactSurface by
+    //  triangle-tetrahedron intersection can set more reliable normal vectors
+    //  for the skinny intersecting triangles.  Related to issue# 12110.
+    face_normals_[f] = (norm != T(0.0))? cross / norm : cross;
+
     // Accumulate area-weighted surface centroid; must be divided by 3X the
     // total area afterwards.
     p_MSc_ += face_area * (r_MA + r_MB + r_MC);

geometry/proximity/test/surface_mesh_test.cc

@@ -188,6 +188,29 @@ GTEST_TEST(SurfaceMeshTest, TestArea) {
   EXPECT_NEAR(GenerateZeroAreaMesh()->total_area(), 0.0, tol);
 }
 
+// Checks the face normal calculations.
+GTEST_TEST(SurfaceMeshTest, TestFaceNormal) {
+  const math::RigidTransform<double> X_WM(
+      math::RollPitchYaw<double>(M_PI / 6.0, M_PI / 3.0, M_PI / 4.0),
+      Vector3<double>(1.0, 2.0, 3.0));
+  // We estimate the rounding errors from rotation (multiply a vector by a
+  // 3x3 matrix) about 3 machine epsilons.
+  const double tol = 3.0 * std::numeric_limits<double>::epsilon();
+  const auto surface_mesh = TestSurfaceMesh<double>(X_WM);
+  const Vector3<double> expect_normal =
+      X_WM.rotation() * Vector3<double>::UnitZ();
+  EXPECT_TRUE(CompareMatrices(
+      expect_normal, surface_mesh->face_normal(SurfaceFaceIndex(0)), tol));
+  EXPECT_TRUE(CompareMatrices(
+      expect_normal, surface_mesh->face_normal(SurfaceFaceIndex(1)), tol));
+
+  // Verify that the zero-area mesh has zero-vector face normal.
+  const auto zero_mesh = GenerateZeroAreaMesh();
+  const Vector3<double> zero_normal = Vector3<double>::Zero();
+  EXPECT_EQ(zero_normal, zero_mesh->face_normal(SurfaceFaceIndex(0)));
+  EXPECT_EQ(zero_normal, zero_mesh->face_normal(SurfaceFaceIndex(1)));
+}
+
 // Checks the centroid calculations.
 GTEST_TEST(SurfaceMeshTest, TestCentroid) {
   const double tol = 10 * std::numeric_limits<double>::epsilon();
@@ -324,6 +347,11 @@ GTEST_TEST(SurfaceMeshTest, ReverseFaceWinding) {
     SurfaceFaceIndex i(value);
     EXPECT_TRUE(winding_reversed(ref_mesh->element(i), test_mesh->element(i)));
   }
+
+  for (int value : {0, 1}) {
+    SurfaceFaceIndex i(value);
+    EXPECT_EQ(ref_mesh->face_normal(i), - test_mesh->face_normal(i));
+  }
 }
 
 GTEST_TEST(SurfaceMeshTest, TransformVertices) {
@@ -342,6 +370,18 @@ GTEST_TEST(SurfaceMeshTest, TransformVertices) {
     const Vector3d p_FV_ref = X_FM * p_MV_ref;
     EXPECT_TRUE(CompareMatrices(p_FV_test, p_FV_ref));
   }
+
+  for (SurfaceFaceIndex f(0); f < test_mesh->num_faces(); ++f) {
+    const Vector3d& nhat_F_test = test_mesh->face_normal(f);
+    const Vector3d& nhat_M_ref = ref_mesh->face_normal(f);
+    const Vector3d nhat_F_ref = X_FM.rotation() * nhat_M_ref;
+    EXPECT_TRUE(CompareMatrices(nhat_F_test, nhat_F_ref));
+  }
+
+  const Vector3d& p_FSc_test = test_mesh->centroid();
+  const Vector3d& p_MSc_ref = ref_mesh->centroid();
+  const Vector3d p_FSc_ref = X_FM * p_MSc_ref;
+  EXPECT_TRUE(CompareMatrices(p_FSc_test, p_FSc_ref));
 }
 
 }  // namespace