Fixes orientation bug in unit sphere mesh generator. (RobotLocomotion…

…#11599)

* Fixes bug in MakeUnitSphereMesh():
VolumeMesh has the following convention for its tetrahedra: the first three vertices define the "base" of the tetrahedron with its right-handed normal pointing inwards. The fourth vertex is then on the positive side of this triangle.

* Adds unit test on tessellated sphere vs half-space represented with level set.

geometry/proximity/contact_surface_calculator.h

@@ -199,7 +199,10 @@ int IntersectTetWithLevelSet(const std::array<Vector3<T>, 4>& tet_vertices_N,
 /// described in [Bloomenthal, 1994].
 ///
 /// @returns A triangulation of the zero level set of `φ(V)` in the volume
-/// defined by the `mesh_M`.  The triangulation is expressed in frame N.
+/// defined by the `mesh_M`.  The triangulation is expressed in frame N. The
+/// right handed normal of each triangle points towards the positive side of the
+/// level set function `φ(V)` or in other words, the normal is aligned with the
+/// gradient of `φ(V)`.
 ///
 /// @note  The SurfaceMesh may have duplicated vertices.
 ///

geometry/proximity/make_unit_sphere_mesh.h

@@ -117,33 +117,45 @@ std::pair<VolumeMesh<T>, std::vector<bool>> MakeSphereMeshLevel0() {
   std::vector<VolumeVertex<T>> vertices;
 
   // Level "0" consists of the octahedron with vertices on the surface of the
-  // a sphere of unit radius.
-  vertices.emplace_back(0.0, 0.0, 0.0);
-  vertices.emplace_back(1.0, 0.0, 0.0);
-  vertices.emplace_back(0.0, 1.0, 0.0);
-  vertices.emplace_back(-1.0, 0.0, 0.0);
-  vertices.emplace_back(0.0, -1.0, 0.0);
-  vertices.emplace_back(0.0, 0.0, 1.0);
-  vertices.emplace_back(0.0, 0.0, -1.0);
+  // a sphere of unit radius, according to the diagram below:
+  //                +Z   -X
+  //                 |   /
+  //                 v5 v3
+  //                 | /
+  //                 |/
+  //  -Y---v4------v0+------v2---+Y
+  //                /|
+  //               / |
+  //             v1  v6
+  //             /   |
+  //           +X    |
+  //                -Z
+  vertices.emplace_back(0.0, 0.0, 0.0);   // v0
+  vertices.emplace_back(1.0, 0.0, 0.0);   // v1
+  vertices.emplace_back(0.0, 1.0, 0.0);   // v2
+  vertices.emplace_back(-1.0, 0.0, 0.0);  // v3
+  vertices.emplace_back(0.0, -1.0, 0.0);  // v4
+  vertices.emplace_back(0.0, 0.0, 1.0);   // v5
+  vertices.emplace_back(0.0, 0.0, -1.0);  // v6
 
   // Create tetrahedra. The convention is that the first three vertices define
   // the "base" of the tetrahedron with its right-handed normal vector
-  // pointing towards the outside. The fourth vertex is on the "negative" side
+  // pointing towards the inside. The fourth vertex is on the "positive" side
   // of the plane defined by this normal.
 
   using V = VolumeVertexIndex;
 
   // Top tetrahedra.
-  tetrahedra.emplace_back(V(0), V(2), V(1), V(5));
-  tetrahedra.emplace_back(V(0), V(3), V(2), V(5));
-  tetrahedra.emplace_back(V(0), V(4), V(3), V(5));
-  tetrahedra.emplace_back(V(0), V(1), V(4), V(5));
+  tetrahedra.emplace_back(V(2), V(0), V(1), V(5));
+  tetrahedra.emplace_back(V(3), V(0), V(2), V(5));
+  tetrahedra.emplace_back(V(4), V(0), V(3), V(5));
+  tetrahedra.emplace_back(V(1), V(0), V(4), V(5));
 
   // Bottom tetrahedra.
-  tetrahedra.emplace_back(V(0), V(1), V(2), V(6));
-  tetrahedra.emplace_back(V(0), V(2), V(3), V(6));
-  tetrahedra.emplace_back(V(0), V(3), V(4), V(6));
-  tetrahedra.emplace_back(V(0), V(4), V(1), V(6));
+  tetrahedra.emplace_back(V(1), V(0), V(2), V(6));
+  tetrahedra.emplace_back(V(2), V(0), V(3), V(6));
+  tetrahedra.emplace_back(V(3), V(0), V(4), V(6));
+  tetrahedra.emplace_back(V(4), V(0), V(1), V(6));
 
   // Indicate what vertices are on the surface of the sphere.
   // All vertices are boundaries but the first one at (0, 0, 0).

geometry/proximity/test/contact_surface_calculator_test.cc

@@ -18,9 +18,16 @@ using Eigen::Vector3d;
 namespace drake {
 namespace geometry {
 namespace internal {
-
 namespace {
 
+// This method computes the right handed normal defined by the three vertices
+// of a triangle in the input argument v.
+Vector3<double> CalcTriangleNormal(
+    const std::vector<SurfaceVertex<double>>& v) {
+  return ((v[1].r_MV() - v[0].r_MV()).cross(v[2].r_MV() - v[0].r_MV()))
+      .normalized();
+}
+
 // Fixture to test the internals of ContactSurfaceCalculator.
 // We will test the intersection of a level set with a single regular
 // tetrahedron. All cases in the marching tetrahedra algorithm are tested.
@@ -40,14 +47,6 @@ class TetrahedronIntersectionTest : public ::testing::Test {
     return vertices;
   }
 
-  // This method computes the right handed normal defined by the three vertices
-  // of a triangle in the input argument v.
-  static Vector3<double> CalcTriangleNormal(
-      const std::vector<SurfaceVertex<double>>& v) {
-    return ((v[1].r_MV() - v[0].r_MV()).cross(v[2].r_MV() - v[0].r_MV()))
-        .normalized();
-  }
-
   std::array<Vector3<double>, 4> unit_tet_;
 };
 
@@ -358,7 +357,7 @@ TEST_F(BoxPlaneIntersectionTest, NoIntersection) {
 // tessellated sphere and a half-space represented by a level set function.
 // In particular, we verify that the vertices on the contact surface are
 // properly interpolated to lie on the plane within a circle of the expected
-// radius.
+// radius, and normals point towards the positive side of the plane.
 GTEST_TEST(SpherePlaneIntersectionTest, VerticesProperlyInterpolated) {
   const double kTolerance = 5.0 * std::numeric_limits<double>::epsilon();
 
@@ -394,6 +393,25 @@ GTEST_TEST(SpherePlaneIntersectionTest, VerticesProperlyInterpolated) {
     const double radius = p_WV.norm();  // since z component is zero.
     EXPECT_LE(radius, surface_radius);
   }
+
+  // Verify all normals point towards the positive side of the plane.
+  for (SurfaceFaceIndex f(0); f < contact_surface_W.num_faces(); ++f) {
+    const SurfaceFace& face = contact_surface_W.element(f);
+    std::vector<SurfaceVertex<double>> vertices_W = {
+        contact_surface_W.vertex(face.vertex(0)),
+        contact_surface_W.vertex(face.vertex(1)),
+        contact_surface_W.vertex(face.vertex(2))};
+    const Vector3<double> normal_W = CalcTriangleNormal(vertices_W);
+    // We expect the normals to point in the same direction as the halfsapce's
+    // outward facing normal, according to the convention used by
+    // CalcZeroLevelSetInMeshDomain().
+    // Additional precision is lost during the computation of the surface. Most
+    // likely due to:
+    //   1) precision loss in the frame transformation between the sphere and
+    //      the plane.
+    //   2) Interpolation within the tetrahedra.
+    EXPECT_TRUE(CompareMatrices(normal_W, Vector3d::UnitZ(), 40 * kTolerance));
+  }
 }
 
 }  // namespace

geometry/proximity/test/make_unit_sphere_mesh_test.cc

@@ -28,14 +28,14 @@ namespace {
 
 // Computes the volume of a tetrahedron given the four vertices that define it.
 // The convention is that the first three vertices a, b, c define a triangle
-// with its right-handed normal pointing towards the outside of the tetrahedra.
-// The fourth vertex, d, is on the negative side of the plane defined by a, b,
+// with its right-handed normal pointing towards the inside of the tetrahedra.
+// The fourth vertex, d, is on the positive side of the plane defined by a, b,
 // c. With this convention, the computed volume will be positive, otherwise
 // negative.
 double CalcTetrahedronVolume(const Vector3<double>& a, const Vector3<double>& b,
                              const Vector3<double>& c,
                              const Vector3<double>& d) {
-  return (a - d).dot((b - d).cross(c - d)) / 6.0;
+  return (d - a).dot((b - a).cross(c - a)) / 6.0;
 }
 
 // Computes the total volume of a VolumeMesh by summing up the contribution