[geometry] Add resolution_hint parameter for MakeCylinderVolumeMesh. (R…

…obotLocomotion#12184)

geometry/proximity/BUILD.bazel

@@ -385,6 +385,10 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "make_cylinder_mesh_test",
+    # This test includes generating the finest mesh allowed for a particular
+    # cylinder. The test size is increased to "medium" so that debug builds
+    # are successful in CI.
+    size = "medium",
     deps = [
         ":make_cylinder_mesh",
         ":sorted_triplet",

geometry/proximity/make_cylinder_mesh.h

@@ -16,7 +16,7 @@ namespace drake {
 namespace geometry {
 namespace internal {
 
-// Helper methods for MakeCylinderMesh().
+// Helper methods for MakeCylinderVolumeMesh().
 #ifndef DRAKE_DOXYGEN_CXX
 
 // TODO(DamrongGuoy): Consider removing the classification of vertex types.
@@ -393,11 +393,27 @@ MakeCylinderMeshLevel0(const double& height, const double& radius) {
 }
 
 #endif
+
 /// Generates a tetrahedral volume mesh of a cylinder whose bounding box is
-/// [-radius, radius]x[-radius,radius]x[-height/2, height/2], i.e., the center
+/// [-radius, radius]x[-radius,radius]x[-length/2, length/2], i.e., the center
 /// line of the cylinder is the z-axis, and the center of the cylinder is at
 /// the origin.
 ///
+/// The level of tessellation is guided by the `resolution_hint` parameter.
+/// Smaller values create higher-resolution meshes with smaller tetrahedra.
+/// The resolution hint is interpreted as an edge length, and the cylinder is
+/// subdivided to guarantee that edge lengths along the boundary circle of
+/// the top and bottom caps will be less than or equal to that edge length.
+///
+/// The resolution of the final mesh will change discontinuously. Small
+/// changes to `resolution_hint` will likely produce the same mesh.
+/// However, cutting `resolution_hint` in half will likely increase the
+/// number of tetrahedra.
+///
+/// Ultimately, successively smaller values of `resolution_hint` will no
+/// longer change the output mesh. This algorithm will not produce more than
+/// 100 million tetrahedra.
+///
 /// This method implements a variant of the generator described in
 /// [Everett, 1997]. The algorithm has diverged a bit from the one in the paper,
 /// but the main ideas used from the paper are:
@@ -420,35 +436,107 @@ MakeCylinderMeshLevel0(const double& height, const double& radius) {
 /// @param[in] cylinder
 ///    Specification of the parameterized cylinder the output mesh should
 ///    approximate.
-/// @param[in] refinement_level
-///    The number of subdivision steps to take. If the original mesh contains
-///    N tetrahedra, the resulting mesh with refinement_level = L will contain
-///    N·8ᴸ tetrahedra.
+/// @param[in] resolution_hint
+///    The positive characteristic edge length for the mesh. The coarsest
+///    possible mesh (a rectangular prism) is guaranteed for any value of
+///    `resolution_hint` greater than √2 times the radius of the cylinder.
 ///
 /// @throws std::exception if refinement_level is negative.
 ///
 /// [Everett, 1997]  Everett, M.E., 1997. A three-dimensional spherical mesh
 /// generator. Geophysical Journal International, 130(1), pp.193-200.
 template <typename T>
-VolumeMesh<T> MakeCylinderMesh(const Cylinder& cylinder, int refinement_level) {
-  const double height = cylinder.get_length();
+VolumeMesh<T> MakeCylinderVolumeMesh(const Cylinder& cylinder,
+                                     double resolution_hint) {
+  const double length = cylinder.get_length();
   const double radius = cylinder.get_radius();
-
-  DRAKE_THROW_UNLESS(refinement_level >= 0);
-
   std::pair<VolumeMesh<T>, std::vector<CylinderVertexType>> pair =
-      MakeCylinderMeshLevel0<T>(height, radius);
+      MakeCylinderMeshLevel0<T>(length, radius);
   VolumeMesh<T>& mesh = pair.first;
   std::vector<CylinderVertexType>& vertex_type = pair.second;
 
+  /*
+    Calculate the refinement level `L` to satisfy the resolution hint, which
+    bounds the length `e` of mesh edges on the boundary circle of the top
+    and bottom caps.
+        The volume mesh is formed by successively refining a rectangular
+    prism.  At the boundary circle of the top and bottom caps, we go from 4
+    edges, to 8 edges, to 16 edges, etc., doubling at each level of
+    refinement. These edges are simply chords across fixed-length arcs of the
+    circle. Based on that we can easily compute the length `e` of the chord
+    and bound it by resolution_hint.
+        We can calculate `e` from the radius r of the cylinder and the central
+    angle θ supported by the chord in this picture:
+
+                    x x x x x
+                 x      | \    x
+               x        |   \    x
+             x          |     \    x
+           x            |       \    x
+          x    radius r |       e \   x
+         x              |           \  x
+        x               |             \ x
+        x               | θ             \
+        x               +---------------x
+        x                   radius r    x
+        x                               x
+         x                             x
+          x                           x
+           x                         x
+             x                     x
+               x                 x
+                 x             x
+                    x x x x x
+
+    The chord length e = 2⋅r⋅sin(θ/2). Solving for θ we get: θ = 2⋅sin⁻¹(e/2⋅r).
+
+    We can relate θ with the refinement level L.
+
+       θ = 2π / 4⋅2ᴸ
+         = π / 2⋅2ᴸ
+         = π / 2ᴸ⁺¹
+
+     Substituting for θ, we get:
+
+       π / 2ᴸ⁺¹ = 2⋅sin⁻¹(e/2⋅r)
+       2ᴸ⁺¹ = π / 2⋅sin⁻¹(e/2⋅r)
+       L + 1 = log₂(π / 2⋅sin⁻¹(e/2⋅r))
+       L = log₂(π / 2⋅sin⁻¹(e/2⋅r)) - 1
+       L = ⌈log₂(π / sin⁻¹(e/2⋅r))⌉ - 2
+   */
+  DRAKE_DEMAND(resolution_hint > 0.0);
+  // Make sure the arcsin doesn't blow up.
+  const double e = std::min(resolution_hint, 2.0 * radius);
+  const int L = std::max(
+      0, static_cast<int>(
+             std::ceil(std::log2(M_PI / std::asin(e / (2.0 * radius)))) - 2));
+
+  // TODO(DamrongGuoy): Reconsider the limit of 100 million tetrahedra.
+  //  Should it be smaller like 1 million? It will depend on how the system
+  //  perform in practice.
+
+  // Limit refinement_level to 100 million tetrahedra. Each refinement level
+  // increases the number of tetrahedra by a factor of 8. Let N₀ be the
+  // number of initial tetrahedra. The number of tetrahedra N with refinement
+  // level L would be:
+  //   N = N₀ * 8ᴸ
+  //   L = log₈(N/N₀)
+  // We can limit N to 100 million by:
+  //   L ≤ log₈(1e8/N₀) = log₂(1e8/N₀)/3
+  const int refinement_level = std::min(
+      L,
+      static_cast<int>(std::floor(std::log2(1.e8 / mesh.num_elements()) / 3.)));
+
+  // If the original mesh contains N tetrahedra, the resulting mesh with
+  // refinement_level = L will contain N·8ᴸ tetrahedra.
   for (int level = 1; level <= refinement_level; ++level) {
     auto split_pair = RefineCylinderMesh<T>(mesh, vertex_type, radius);
     mesh = split_pair.first;
     vertex_type = split_pair.second;
     DRAKE_DEMAND(mesh.vertices().size() == vertex_type.size());
   }
 
-  return std::move(mesh);
+  return mesh;
 }
 
 }  // namespace internal

geometry/proximity/test/make_cylinder_mesh_test.cc

@@ -29,17 +29,76 @@ double CalcTetrahedronMeshVolume(const VolumeMesh<double>& mesh) {
   return volume;
 }
 
+GTEST_TEST(MakeCylinderVolumeMesh, CoarsestMesh) {
+  const double radius = 1.0;
+  const double length = 2.0;
+  // A `resolution_hint` greater than √2 times the radius of the cylinder
+  // should give the coarsest mesh. We use a scaling factor larger than
+  // √2 = 1.41421356... in the sixth decimal digit. It should give a mesh of
+  // rectangular prism with 24 tetrahedra.
+  const double resolution_hint_above = 1.414214 * radius;
+  auto mesh_coarse = MakeCylinderVolumeMesh<double>(Cylinder(radius, length),
+                                                    resolution_hint_above);
+  EXPECT_EQ(24, mesh_coarse.num_elements());
+
+  // A `resolution_hint` slightly below √2 times the radius of the cylinder
+  // should give the next refined mesh.  We use a scaling factor smaller than
+  // √2 = 1.41421356... in the sixth decimal digit. It should give a mesh of
+  // rectangular prism with 8 * 24 = 192 tetrahedra.
+  const double resolution_hint_below = 1.414213 * radius;
+  auto mesh_fine = MakeCylinderVolumeMesh<double>(Cylinder(radius, length),
+                                                  resolution_hint_below);
+  EXPECT_EQ(192, mesh_fine.num_elements());
+}
+
+// The test size is increased to "medium" so that debug builds are successful
+// in CI. See BUILD.bazel.
+GTEST_TEST(MakeCylinderVolumeMesh, FinestMesh) {
+  const double radius = 1.0;
+  const double length = 2.0;
+  // The initial mesh of a cylinder with radius 1 and length 2 has 24
+  // tetrahedra. Each refinement level increases the number of tetrahedra by
+  // a factor of 8. A refinement level ℒ that could increase tetrahedra beyond
+  // 100 million is ℒ = 8, which will give 24 * 8⁸ = 402,653,184 tetrahedra.
+  // (However, we will confirm later that the algorithm will limit the number
+  // of tetrahedra within 100 million, so it should give
+  // 24 * 8⁷ = 50,331,648 tetrahedra instead.) We will calculate the
+  // resolution hint that could trigger ℒ = 8.
+  //     Each edge of the mesh on the boundary circle of the top and bottom
+  // caps is a chord of the circle with a central angle θ. The edge length e
+  // is calculated from θ and radius r as:
+  //     e = 2⋅r⋅sin(θ/2)
+  // The central angle is initially π/2, and each level of refinement
+  // decreases it by a factor of 2. For ℒ = 8, we have:
+  //     θ = (π/2)/(2⁸) = π/512
+  // Thus, we have e = 2 sin (π/512) = 0.0122717...
+  //     We use a resolution hint smaller than e in the sixth decimal digit to
+  // request 402,653,184 tetrahedra, but the algorithm should limit the
+  // number of tetrahedra within 100 million. As a result, we should have
+  // 50,331,648 tetrahedra instead.
+  const double resolution_hint_limit = 1.2271e-2;
+  auto mesh_limit = MakeCylinderVolumeMesh<double>(Cylinder(radius, length),
+                                                   resolution_hint_limit);
+  EXPECT_EQ(50331648, mesh_limit.num_elements());
+
+  // Confirm that going ten times finer resolution hint does not increase the
+  // number of tetrahedra.
+  const double resolution_hint_finer = 1.2271e-3;
+  auto mesh_same = MakeCylinderVolumeMesh<double>(Cylinder(radius, length),
+                                                   resolution_hint_finer);
+  EXPECT_EQ(50331648, mesh_same.num_elements());
+}
+
 // This test verifies that the volume of the tessellated
 // cylinder converges to the exact value as the tessellation is refined.
-GTEST_TEST(MakeCylinderMesh, VolumeConvergence) {
+GTEST_TEST(MakeCylinderVolumeMesh, VolumeConvergence) {
   const double kTolerance = 10.0 * std::numeric_limits<double>::epsilon();
 
   const double height = 2;
   const double radius = 1;
-  const int refinement_level = 0;
-
-  auto mesh0 = MakeCylinderMesh<double>(
-      drake::geometry::Cylinder(radius, height), refinement_level);
+  double resolution_hint = 2.0;  // Hint to coarsest mesh.
+  auto mesh0 =
+      MakeCylinderVolumeMesh<double>(Cylinder(radius, height), resolution_hint);
 
   const double volume0 = CalcTetrahedronMeshVolume(mesh0);
   const double cylinder_volume = height * radius * radius * M_PI;
@@ -56,13 +115,14 @@ GTEST_TEST(MakeCylinderMesh, VolumeConvergence) {
   EXPECT_NEAR(volume0, expected_volume_0, kTolerance);
 
   for (int level = 1; level < 6; ++level) {
-    auto mesh = MakeCylinderMesh<double>(
-        drake::geometry::Cylinder(radius, height), level);
+    resolution_hint /= 2.0;
+    auto mesh = MakeCylinderVolumeMesh<double>(
+        drake::geometry::Cylinder(radius, height), resolution_hint);
 
     // Verify the correct size. There are initially 24 tetrahedra that each
     // split into 8 sub tetrahedra.
     const size_t num_tetrahedra = 24 * std::pow(8, level);
-    EXPECT_EQ(mesh.tetrahedra().size(), num_tetrahedra);
+    EXPECT_EQ(mesh.num_elements(), num_tetrahedra);
 
     // Verify that the volume monotonically converges towards the exact volume
     // of the given cylinder.
@@ -130,15 +190,15 @@ int ComputeEulerCharacteristic(const VolumeMesh<double>& mesh) {
 //
 // where k_i is the number of i-simplexes in the complex. For a convex mesh that
 // is homeomorphic to a 3 dimensional ball, χ = 1.
-GTEST_TEST(MakeCylinderMesh, EulerCharacteristic) {
+GTEST_TEST(MakeCylinderVolumeMesh, EulerCharacteristic) {
   const double height = 2;
   const double radius = 1;
 
   const int expected_euler_characteristic = 1;
 
-  for (int level = 0; level < 6; ++level) {
-    auto mesh = MakeCylinderMesh<double>(
-        drake::geometry::Cylinder(radius, height), level);
+  for (const double resolution_hint : {2., 1., 0.5, 0.25, 0.125, 0.0625}) {
+    auto mesh = MakeCylinderVolumeMesh<double>(
+        drake::geometry::Cylinder(radius, height), resolution_hint);
 
     EXPECT_EQ(ComputeEulerCharacteristic(mesh), expected_euler_characteristic);
   }