Adds capsule mesh (RobotLocomotion#14642)

geometry/proximity/BUILD.bazel

@@ -26,6 +26,7 @@ drake_cc_package_library(
         ":hydroelastic_internal",
         ":make_box_field",
         ":make_box_mesh",
+        ":make_capsule_mesh",
         ":make_cylinder_field",
         ":make_cylinder_mesh",
         ":make_ellipsoid_field",
@@ -211,6 +212,7 @@ drake_cc_library(
         ":bvh",
         ":make_box_field",
         ":make_box_mesh",
+        ":make_capsule_mesh",
         ":make_cylinder_field",
         ":make_cylinder_mesh",
         ":make_ellipsoid_field",
@@ -284,6 +286,21 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "make_capsule_mesh",
+    srcs = ["make_capsule_mesh.cc"],
+    hdrs = ["make_capsule_mesh.h"],
+    deps = [
+        ":meshing_utilities",
+        ":proximity_utilities",
+        ":surface_mesh",
+        ":volume_mesh",
+        ":volume_to_surface_mesh",
+        "//common:essential",
+        "//geometry:shape_specification",
+    ],
+)
+
 drake_cc_library(
     name = "make_ellipsoid_field",
     hdrs = ["make_ellipsoid_field.h"],
@@ -692,6 +709,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "make_capsule_mesh_test",
+    deps = [
+        ":make_capsule_mesh",
+        ":mesh_to_vtk",
+        ":proximity_utilities",
+    ],
+)
+
 drake_cc_googletest(
     name = "make_ellipsoid_field_test",
     deps = [

geometry/proximity/make_capsule_mesh.cc

@@ -0,0 +1,190 @@
+#include "drake/geometry/proximity/make_capsule_mesh.h"
+
+#include <cmath>
+
+#include "drake/geometry/proximity/meshing_utilities.h"
+#include "drake/geometry/proximity/proximity_utilities.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+template <typename T>
+VolumeMesh<T> MakeCapsuleVolumeMesh(const Capsule& capsule,
+                                    double resolution_hint) {
+  // Z coordinates of the endpoints of the medial axis.
+  const double medial_top_z = capsule.length() / 2.;
+  const double medial_bottom_z = -medial_top_z;
+  // Z coordinates of the poles of the caps.
+  const double top_z = medial_top_z + capsule.radius();
+  const double bottom_z = -top_z;
+
+  // Let n be the number of vertices on each circular rim of the capsule.
+  // The total number of vertices, V = 2n⌊n/2⌋ + 4
+  // The total number of tetrahedra, F = 4n(⌊n/2⌋ - 1) + 5n
+  // We need to be able to index both the vertices and tetrahedra with type safe
+  // indices, thus the absolute maximum number of tetrahedra is INT_MAX. However
+  // we choose a more reasonable maximum of 1 million tetrahedra.
+  //
+  // F = 4n(⌊n/2⌋ - 1) + 5n ≤ MAX
+  // F ≤ 4n(n/2 - 1) + 5n ≤ MAX
+  // 2n² + n - MAX ≤ 0
+  // n ≤ ((√ 8 MAX + 1) - 1) / 4
+  // n ≤ ⌊ 706.86... ⌋  for MAX = 1'000'000
+  // n ≤ 706
+
+  // The mesh will have a minimum of 3 vertices on its rim.
+  const int num_vertices_per_circle = static_cast<int>(
+      std::clamp(2. * M_PI * capsule.radius() / resolution_hint, 3.0, 706.0));
+
+  // Each vertex on the rim corresponds to a line of longitude on the
+  // hemispheres of the caps. num_circles_per_cap is how many lines of latitude
+  // to subdivide each cap into.
+  const int num_circles_per_cap = num_vertices_per_circle / 2;
+
+  std::vector<VolumeVertex<T>> mesh_vertices;
+  // 2 caps * number of circles per cap * number of verts per circle
+  // + the two medial vertices and the two cap pole vertices.
+  mesh_vertices.reserve(2 * num_circles_per_cap * num_vertices_per_circle + 4);
+
+  // Add the cap pole vertices and the medial vertices.
+  VolumeVertexIndex medial_top(mesh_vertices.size());
+  mesh_vertices.emplace_back(0, 0, medial_top_z);
+  VolumeVertexIndex medial_bottom(mesh_vertices.size());
+  mesh_vertices.emplace_back(0, 0, medial_bottom_z);
+  VolumeVertexIndex top(mesh_vertices.size());
+  mesh_vertices.emplace_back(0, 0, top_z);
+  VolumeVertexIndex bottom(mesh_vertices.size());
+  mesh_vertices.emplace_back(0, 0, bottom_z);
+
+  // Storage for the vertex indices of each cap.
+  std::vector<VolumeVertexIndex> top_cap(num_circles_per_cap *
+                                         num_vertices_per_circle);
+  std::vector<VolumeVertexIndex> bottom_cap(num_circles_per_cap *
+                                            num_vertices_per_circle);
+
+  // The vertices of the caps are spaced in an equal subdivision of the sphere
+  // with spherical coordinates. Vertex (i, j) will have spherical coordinates:
+  // (r, θ, φ) = (radius, π - i*theta_step, j*phi_step) where θ is the polar
+  // angle and φ the azimuthal angle. One of the peculiarities of this
+  // parameterization is the anisotropic behavior of contact patch resolution
+  // at different points of the sphere. For instance, the pole vertices have
+  // a degree of `num_vertices_per_circle` while all other vertices on the
+  // sphere have a degree of 4. Thus a shallow contact with a plane at and
+  // tangent to the pole will produce a much denser contact surface than that
+  // produced by the same type of shallow contact at a point at the equator
+  // of the sphere. It is unclear at this moment whether that artifact has an
+  // effect on the simulation.
+  const double theta_step = M_PI_2 / num_circles_per_cap;
+  const double phi_step = 2 * M_PI / num_vertices_per_circle;
+
+  // Subdivide the sphere and partition into top and bottom caps.
+  for (int i = 0; i < num_circles_per_cap; ++i) {
+    const double theta = M_PI_2 - i * theta_step;
+    const double s = sin(theta);
+    // Offset the z value of each cap.
+    const double top_circle_z = capsule.radius() * cos(theta) + medial_top_z;
+    const double bottom_circle_z = -top_circle_z;
+
+    for (int j = 0; j < num_vertices_per_circle; ++j) {
+      const double phi = j * phi_step;
+      const double x = capsule.radius() * s * cos(phi);
+      const double y = capsule.radius() * s * sin(phi);
+
+      top_cap[num_vertices_per_circle * i + j] =
+          VolumeVertexIndex(mesh_vertices.size());
+      mesh_vertices.emplace_back(x, y, top_circle_z);
+      bottom_cap[num_vertices_per_circle * i + j] =
+          VolumeVertexIndex(mesh_vertices.size());
+      mesh_vertices.emplace_back(x, y, bottom_circle_z);
+    }
+  }
+
+  std::vector<VolumeElement> mesh_elements;
+
+  // Each quadrilateral of each hemispherical cap is the base of a pyramid
+  // whose top vertex is the center of the sphere of the cap (one of the medial
+  // vertices.) The pyramids are split into tetrahedra in such a way that they
+  // conform to each other. That is to say they share common faces where they
+  // meet by the appropriate choice of diagonal. The quadrilateral faces of the
+  // pyramid are: [(i,j), (i, j+1), (i+1, j+1), (i+1, j)] where those vertices
+  // are given counterclockwise when viewed from outside of the capsule.
+  //
+  //      (i+1,j)  ______
+  //             o        +
+  //          ／   \         ＼
+  //         /      \         \   (one slice of the top cap)
+  // (i,j)  o---------o M₁     +
+
+  // i indexes latitude and j indexes longitude.
+  for (int i = 0; i < num_circles_per_cap - 1; ++i) {
+    for (int j = 0; j < num_vertices_per_circle; ++j) {
+      // Connect to the next longitude, wrapping around the circle.
+      const int j1 = (j + 1) % num_vertices_per_circle;
+
+      // The arguments to SplitPyramidToTetrahedra are given in CLOCKWISE order
+      // for the base.
+      Append(SplitPyramidToTetrahedra(
+                 top_cap[(i + 1) * num_vertices_per_circle + j],
+                 top_cap[(i + 1) * num_vertices_per_circle + j1],
+                 top_cap[i * num_vertices_per_circle + j1],
+                 top_cap[i * num_vertices_per_circle + j], medial_top),
+             &mesh_elements);
+
+      Append(
+          SplitPyramidToTetrahedra(
+              bottom_cap[i * num_vertices_per_circle + j],
+              bottom_cap[i * num_vertices_per_circle + j1],
+              bottom_cap[(i + 1) * num_vertices_per_circle + j1],
+              bottom_cap[(i + 1) * num_vertices_per_circle + j], medial_bottom),
+          &mesh_elements);
+    }
+  }
+
+  // The vertices on the last circle of latitude on each cap all connect to the
+  // pole vertices forming a circular cap divided into triangular wedges. Each
+  // of those wedges connects to the corresponding medial vertex, forming a
+  // single tetrahedra.
+  //
+  // The two equatorial circles of each cap connect corresponding vertices to
+  // each other, forming the cylindrical middle portion of the capsule. Each
+  // quadrilateral face of the cylinder connects to the two medial vertices,
+  // forming a triangular prism. Each prism is then subdivided into 3 tetrahedra
+  // such that adjacent prisms are conforming.
+
+  // Offset into the index vector of the last circle of the caps.
+  const int last_circle_offset =
+      (num_circles_per_cap - 1) * num_vertices_per_circle;
+  for (int j = 0; j < num_vertices_per_circle; ++j) {
+    const int j1 = (j + 1) % num_vertices_per_circle;
+
+    // Add tetrahedra of the top and bottom caps.
+    // Vertices are oriented such that the signed volume of the tetrahedra
+    // is positive, assuming the right hand rule.
+    mesh_elements.emplace_back(top, top_cap[last_circle_offset + j1],
+                               top_cap[last_circle_offset + j], medial_top);
+
+    mesh_elements.emplace_back(bottom, bottom_cap[last_circle_offset + j],
+                               bottom_cap[last_circle_offset + j1],
+                               medial_bottom);
+
+    // Add the cylindrical prisms.
+    // Vertices are passed in the order: bottom triangular cap (oriented
+    // inwards) and then top triangular cap (oriented outwards).
+    Append(SplitTriangularPrismToTetrahedra(medial_bottom, bottom_cap[j],
+                                            bottom_cap[j1], medial_top,
+                                            top_cap[j], top_cap[j1]),
+           &mesh_elements);
+  }
+
+  return {std::move(mesh_elements), std::move(mesh_vertices)};
+}
+
+template VolumeMesh<double> MakeCapsuleVolumeMesh(const Capsule&,
+                                                  double resolution_hint);
+template VolumeMesh<AutoDiffXd> MakeCapsuleVolumeMesh(const Capsule&,
+                                                      double resolution_hint);
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/make_capsule_mesh.h

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <algorithm>
+#include <limits>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/common/sorted_pair.h"
+#include "drake/geometry/proximity/surface_mesh.h"
+#include "drake/geometry/proximity/volume_mesh.h"
+#include "drake/geometry/proximity/volume_to_surface_mesh.h"
+#include "drake/geometry/shape_specification.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+/* Generates a tetrahedral volume mesh approximating a given capsule by medial
+ axis (MA) subdivision. The capsule is subdivided into blocks using its MA,
+ and then the blocks are subdivided into tetrahedra. Using this mesh with
+ MeshFieldLinear, we can represent the distance-to-boundary function φ of the
+ capsule (which can be scaled by a constant elastic modulus to create a
+ hydroelastic pressure field) accurately and economically. However, this mesh
+ may not be suitable for other kinds of pressure fields, e.g., a squared
+ distance field.
+
+ The capsule is parameterized by two parameters (radius, length). Its length
+ describes the extent of the cylindrical portion of the capsule aligned with
+ the Z axis. The radius describes the radius of the two hemi-spherical "caps"
+ of the capsule, symmetric around the Z axis. The capsule's MA is the set of all
+ points having more than one closest point on the capsule's boundary. (See
+ https://en.wikipedia.org/wiki/Medial_axis for more about the medial axis.) For
+ any parameterization of the capsule, the MA consists of a line segment between
+ the points M₀ = (0, 0, -0.5 * length) and M₁ = (0, 0, 0.5 * length):
+
+
+
+                        Z (axis of rotation)
+                        ↑
+                        |
+     ___             _______
+      |           -          -
+   radius      ／               ＼
+      |       /                  \
+     ___     +       M₁ o         +
+      |      |          ║         |
+      |      |          ║         |
+      |      |          ║         |
+      |      |          ║         |
+   length    |          o         |  ----→ X
+      |      |          ║         |
+      |      |          ║         |
+      |      |          ║         |
+      |      |          ║         |
+     ___     +       M₀ o         +
+              \                  /
+                ＼             ／
+                   - _______ -
+
+                        |-radius--|
+
+ The following picture files show examples of meshes generated from
+ MakeCapsuleVolumeMesh(). The files are distributed with the source code.
+
+ | geometry/proximity/images/capsule_mesh_medial_axis.png |
+ | Meshes of a capsule viewed from the outside. |
+
+ | geometry/proximity/images/capsule_mesh_medial_axis_slice.png |
+ | x = 0 slice of the capsule mesh, showing the triangulation. |
+
+ The output mesh will have these properties:
+
+   1. The generated vertices are unique. There are no repeating vertices in
+      the list of vertex coordinates.
+   2. The generated tetrahedra are _conforming_. Two tetrahedra intersect in
+      their shared face, or shared edge, or shared vertex, or not at all.
+      There is no partial overlapping of two tetrahedra.
+   3. The number of vertices per one circular rim of the capsule is ⌈2πr/h⌉,
+      where r = capsule.radius(), and h is resolution_hint.
+
+ @param[in] capsule
+     The capsule shape specification.
+ @param[in] resolution_hint
+     The target length of each edge of the mesh on each of the bottom and the
+     top circular rims of the capsule. The coarsest possible mesh (a triangular
+     prism with tetrahedral caps) is guaranteed for any value of
+     `resolution_hint` greater than or equal to 2·π·radius/3. The finest
+     possible mesh (with 1 million tetrahedra) is guaranteed for any value of
+     `resolution_hint` less than or equal to 2·π·radius/706.
+ @retval volume_mesh
+ @tparam_nonsymbolic_scalar
+ */
+template <typename T>
+VolumeMesh<T> MakeCapsuleVolumeMesh(const Capsule& capsule,
+                                    double resolution_hint);
+
+// Creates a surface mesh for the given `capsule`; the level of
+// tessellation is guided by the `resolution_hint` parameter in the same way
+// as MakeCapsuleVolumeMesh.
+//
+// @param[in] capsule
+//    Specification of the parameterized capsule the output surface mesh
+//    should approximate.
+// @param[in] resolution_hint
+//    The positive characteristic edge length for the mesh. The coarsest
+//    possible mesh (a triangular prism with tetrahedral caps) is guaranteed
+//    for any value of `resolution_hint` greater than or equal to 2·π·radius/3.
+//    The finest possible mesh is guaranteed for any value of `resolution_hit`
+//    less than or equal to 2·π·radius/706.
+// @returns The triangulated surface mesh for the given capsule.
+// @pre resolution_hint is positive.
+// @tparam T
+//    The Eigen-compatible scalar for representing the mesh vertex positions.
+template <typename T>
+SurfaceMesh<T> MakeCapsuleSurfaceMesh(const Capsule& capsule,
+                                      double resolution_hint) {
+  DRAKE_DEMAND(resolution_hint > 0.0);
+  return ConvertVolumeToSurfaceMesh<T>(
+      MakeCapsuleVolumeMesh<T>(capsule, resolution_hint));
+}
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/make_cylinder_mesh.cc

@@ -11,12 +11,6 @@ namespace internal {
 
 namespace {
 
-void Append(const std::vector<VolumeElement>& new_elements,
-            std::vector<VolumeElement>* mesh_elements) {
-  mesh_elements->insert(mesh_elements->end(), new_elements.begin(),
-                        new_elements.end());
-}
-
 /* Generates tetrahedral elements of a long cylinder conforming to its medial
  axis. It assumes the mesh vertices on the surface of the cylinder was already
  generated on the bottom and the top circular rims of the cylinder together

geometry/proximity/meshing_utilities.cc

@@ -6,6 +6,12 @@ namespace drake {
 namespace geometry {
 namespace internal {
 
+void Append(const std::vector<VolumeElement>& new_elements,
+            std::vector<VolumeElement>* mesh_elements) {
+  mesh_elements->insert(mesh_elements->end(), new_elements.begin(),
+                        new_elements.end());
+}
+
 using std::unordered_set;
 
 std::vector<VolumeElement> SplitTriangularPrismToTetrahedra(