[hydro] Add MeshBuilder -- interface for building ContactSurface mesh…

…es (RobotLocomotion#16134)

The MeshBuilder classes extract the knowledge of how to add intersection
polygons to mesh representations. The intersection algorithms can simply
generate the polygon (using whatever logic they require) and pass the data
over to the builder.

This also renames the internal `AddPolygonToMeshData()` function to better
reflect its purpose and contrast it with the new
`AddPolygonToPolygonMeshData()` function.

geometry/proximity/BUILD.bazel

@@ -142,6 +142,8 @@ drake_cc_library(
     srcs = ["contact_surface_utility.cc"],
     hdrs = ["contact_surface_utility.h"],
     deps = [
+        ":mesh_field",
+        ":polygon_surface_mesh",
         ":triangle_surface_mesh",
         "//common:default_scalars",
     ],

geometry/proximity/contact_surface_utility.cc

@@ -8,11 +8,26 @@
 #include "drake/common/default_scalars.h"
 
 namespace drake {
-
 namespace geometry {
 namespace internal {
 namespace {
 
+template <typename T>
+void ThrowForInvalidVertexIndex(const std::vector<int>& indices,
+                                const std::vector<Vector3<T>>& vertices,
+                                const char* func) {
+  const int v_count = static_cast<int>(vertices.size());
+  for (int index : indices) {
+    if (index < 0 || index >= v_count) {
+      throw std::logic_error(fmt::format(
+          "{}: Adding a polygon to a ContactSurface mesh builder requires that "
+          "the polygon vertex indices refer to vertices that have already been "
+          "added. New polygon references index {} out {} vertices.",
+          func, index, v_count));
+    }
+  }
+}
+
 /* Utility for CalcPolygonCentroid() to evaluate the correctness of the inputs
  to CalcPolygonCentroid(). CalcPolygonCentroid() uses area computations to
  compute the centroid position and this method verifies that the parameter
@@ -121,6 +136,90 @@ void ThrowIfInvalidForCentroid(const char* prefix,
 
 }  // namespace
 
+template <typename T>
+int TriMeshBuilder<T>::AddPolygon(
+    const std::vector<int>& polygon_vertices,
+    const Vector3<T>& nhat_B,
+    const Vector3<T>& grad_e_MN_B) {
+  // Vertices and pressure values at vertex positions must already have been
+  // explicitly added to use this method.
+  DRAKE_ASSERT_VOID(
+      ThrowForInvalidVertexIndex(polygon_vertices, vertices_B_, __func__));
+
+  const int initial_face_count = static_cast<int>(faces_.size());
+  AddPolygonToTriangleMeshData(polygon_vertices, nhat_B, &faces_, &vertices_B_);
+
+  // The call to AddPolygonToTriangleMeshData adds *one* more vertex at the
+  // polygon's centroid - it is the *last* vertex in vertices_B_; we need to
+  // sample its pressure as well. We can compute the pressure at the centroid
+  // based on the gradient of the function, and a previous vertex position and
+  // pressure value.
+  //
+  //    p(x⃗) = ∇e⋅x⃗ + d,
+  //    p(vₙ) = ∇e⋅vₙ + d  →  d = p(vₙ) - ∇e⋅vₙ
+  //    p(v_c) = ∇e⋅v_c + p(vₙ) - ∇e⋅vₙ
+  //           = ∇e⋅(v_c - vₙ) + p(vₙ)
+  const Vector3<T>& p_BC = vertices_B_.back();
+  const Vector3<T>& p_BN = vertices_B_[polygon_vertices[0]];
+  const T& pressure_at_N = pressures_[polygon_vertices[0]];
+  pressures_.emplace_back(grad_e_MN_B.dot(p_BC - p_BN) + pressure_at_N);
+  return faces_.size() - initial_face_count;
+}
+
+template <typename T>
+std::pair<std::unique_ptr<TriangleSurfaceMesh<T>>,
+          std::unique_ptr<TriangleSurfaceMeshFieldLinear<T, T>>>
+TriMeshBuilder<T>::MakeMeshAndField() {
+  auto mesh = std::make_unique<TriangleSurfaceMesh<T>>(std::move(faces_),
+                                                       std::move(vertices_B_));
+  auto* raw = mesh.get();
+  const bool calculate_gradient = false;
+  return {std::move(mesh),
+          std::make_unique<TriangleSurfaceMeshFieldLinear<T, T>>(
+              std::move(pressures_), raw, calculate_gradient)};
+}
+
+template <typename T>
+PolyMeshBuilder<T>::PolyMeshBuilder() {
+  // This amount is arbitrary -- it is certainly sufficient for coarse meshes
+  // in contact, but otherwise could be larger or smaller to mitigate
+  // allocations vs memory footprint.
+  grad_e_MN_B_per_face_.reserve(20);
+}
+
+template <typename T>
+int PolyMeshBuilder<T>::AddPolygon(
+    const std::vector<int>& polygon_vertices,
+    const Vector3<T>& /* nhat_B */,
+    const Vector3<T>& grad_e_MN_B) {
+  // Vertices and pressure values at vertex positions must already have been
+  // explicitly added to use this method.
+  DRAKE_ASSERT_VOID(
+      ThrowForInvalidVertexIndex(polygon_vertices, vertices_B_, __func__));
+
+  ++polygon_count_;
+
+  // TODO(SeanCurtis-TRI): Make use of the known face normal of the surface mesh
+  //  in adding the polygon to reduce the cost of computing polygon area. That
+  //  would entail accumulating the currently ignored face normals (nhat_B) in
+  //  this builder and passing collection into the PolygonSurfaceMesh
+  //  constructor.
+  AddPolygonToPolygonMeshData(polygon_vertices, &face_data_);
+  grad_e_MN_B_per_face_.push_back(grad_e_MN_B);
+  return 1;
+}
+
+template <typename T>
+std::pair<std::unique_ptr<PolygonSurfaceMesh<T>>,
+          std::unique_ptr<PolygonSurfaceMeshFieldLinear<T, T>>>
+PolyMeshBuilder<T>::MakeMeshAndField() {
+  auto mesh = std::make_unique<PolygonSurfaceMesh<T>>(std::move(face_data_),
+                                                      std::move(vertices_B_));
+  auto field = std::make_unique<PolygonSurfaceMeshFieldLinear<T, T>>(
+      std::move(pressures_), mesh.get(), std::move(grad_e_MN_B_per_face_));
+  return {std::move(mesh), std::move(field)};
+}
+
 template <typename T>
 Vector3<T> CalcPolygonCentroid(const std::vector<int>& polygon,
                                const Vector3<T>& n_F,
@@ -192,10 +291,10 @@ Vector3<T> CalcPolygonCentroid(const std::vector<int>& polygon,
 }
 
 template <typename T>
-void AddPolygonToMeshData(const std::vector<int>& polygon,
-                          const Vector3<T>& n_F,
-                          std::vector<SurfaceTriangle>* faces,
-                          std::vector<Vector3<T>>* vertices_F) {
+void AddPolygonToTriangleMeshData(const std::vector<int>& polygon,
+                                  const Vector3<T>& n_F,
+                                  std::vector<SurfaceTriangle>* faces,
+                                  std::vector<Vector3<T>>* vertices_F) {
   DRAKE_DEMAND(faces != nullptr);
   DRAKE_DEMAND(vertices_F != nullptr);
   DRAKE_DEMAND(polygon.size() >= 3);
@@ -221,6 +320,18 @@ void AddPolygonToMeshData(const std::vector<int>& polygon,
   }
 }
 
+void AddPolygonToPolygonMeshData(const std::vector<int>& polygon,
+                                 std::vector<int>* face_data) {
+  DRAKE_DEMAND(face_data != nullptr);
+  DRAKE_DEMAND(polygon.size() >= 3);
+
+  const int polygon_size = static_cast<int>(polygon.size());
+  face_data->push_back(polygon_size);
+  for (int v : polygon) {
+    face_data->push_back(v);
+  }
+}
+
 // TODO(SeanCurtis-TRI): This test is not currently directly tested in
 //  contact_surface_utility_test.cc. It is, however, tested in other tests
 //  indirectly (e.g., mesh_intersection_test.cc
@@ -249,11 +360,16 @@ bool IsFaceNormalInNormalDirection(const Vector3<T>& normal_F,
 
 // Instantiation to facilitate unit testing of this support function.
 DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
-    &AddPolygonToMeshData<T>,
+    &AddPolygonToTriangleMeshData<T>,
     &IsFaceNormalInNormalDirection<T>,
     &CalcPolygonCentroid<T>
 ))
 
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+  class ::drake::geometry::internal::TriMeshBuilder)
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+  class ::drake::geometry::internal::PolyMeshBuilder)

geometry/proximity/contact_surface_utility.h

@@ -7,14 +7,181 @@
  assist in maintaining those invariants.
  */
 
+#include <memory>
+#include <utility>
 #include <vector>
 
+#include "drake/geometry/proximity/polygon_surface_mesh.h"
+#include "drake/geometry/proximity/polygon_surface_mesh_field.h"
 #include "drake/geometry/proximity/triangle_surface_mesh.h"
+#include "drake/geometry/proximity/triangle_surface_mesh_field.h"
 
 namespace drake {
 namespace geometry {
 namespace internal {
 
+/* @name "MeshBuilder" implementations
+
+ These MeshBuilder classes are used as the function template parameter in
+ various contact surface algorithms. They provide sufficient infrastructure to
+ build meshes with different representations (e.g., triangle vs polygon).
+
+ MeshBuilders provide two services:
+
+   - Collect mesh and field data associated with the polygon that arises from
+     tet-tet, tet-tri, tet-plane, tri-half space, etc. intersections.
+   - Compute the mesh and field type tailored to that builder.
+
+ A MeshBuilder should be thought of as a frame-dependent quantity. The mesh it
+ builds is likewise a frame-dependent quantity. As such, it should be named with
+ the expected frame clearly identified: e.g., builder_W. Please note the frame
+ expectations on the various function parameters. In the classes' documentation,
+ we refer to the builder's frame as B. Some of the APIs require the fields or
+ meshes that are colliding, and their frames will be named in the scope of those
+ functions. */
+//@{
+
+/* A MeshBuilder type to build a triangle surface mesh. The mesh is built
+ incrementally. Vertices get added to the mesh (each with a corresponding
+ pressure field value). Subsequently, the polygon is declared, referencing
+ previously added vertices by index.
+
+ The TriMeshBuilder will always tessellate every polygon around its centroid
+ (adding an additional vertex to the declared vertices). */
+template <typename T>
+class TriMeshBuilder {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(TriMeshBuilder);
+
+  using ScalarType = T;
+
+  TriMeshBuilder() = default;
+
+  /* Adds a vertex V (and its corresponding field value) to the mesh.
+
+   @param p_BV         The position of the new vertex (measured and expressed in
+                       the builder's frame B).
+   @param field_value  The value of the pressure field evaluated at V.
+   @returns The index of the newly added vertex. */
+  int AddVertex(const Vector3<T>& p_BV, const T& field_value) {
+    vertices_B_.push_back(p_BV);
+    pressures_.push_back(field_value);
+    return static_cast<int>(vertices_B_.size() - 1);
+  }
+
+  /* Adds the polygon to the in-progress mesh. The polygon is defined by
+   indices into the set of vertices that have already been added to the builder.
+
+   @param polygon_vertices  The definition of the polygon to add, expressed as
+                            ordered indices into the currently existing
+                            vertices. They should be ordered in a counter-
+                            clockwise manner such that the implied face normal
+                            points "outward" (using the right-hand rule).
+   @param nhat_B            The normal to the polygon, measured and expressed in
+                            the builder's frame B.
+   @param grad_e_MN_B       The gradient of the pressure field in the domain of
+                            the polygon, expressed in the builder's frame B.
+   @returns The number of faces added to the mesh.
+
+   @sa AddVertex(). */
+  int AddPolygon(const std::vector<int>& polygon_vertices,
+                 const Vector3<T>& nhat_B, const Vector3<T>& grad_e_MN_B);
+
+  /* Returns the total number of vertices accumulated so far. */
+  int num_vertices() const { return static_cast<int>(vertices_B_.size()); }
+
+  /* Returns the total number of faces added by calls to AddPolygon(). */
+  int num_faces() const { return static_cast<int>(faces_.size()); }
+
+  /* Create a mesh and field from the mesh data that has been aggregated by
+   this builder. */
+  std::pair<std::unique_ptr<TriangleSurfaceMesh<T>>,
+            std::unique_ptr<TriangleSurfaceMeshFieldLinear<T, T>>>
+  MakeMeshAndField();
+
+ private:
+  /* The faces of the mesh being built. */
+  std::vector<SurfaceTriangle> faces_;
+  /* The vertices of the mesh being built. */
+  std::vector<Vector3<T>> vertices_B_;
+  /* The pressure values (e) of the surface being built. */
+  std::vector<T> pressures_;
+};
+
+/* A MeshBuilder type to build a polygon surface mesh. The mesh is built
+ incrementally. Vertices get added to the mesh (each with a corresponding
+ pressure field value). Subsequently, the polygon is declared, referencing
+ previously added vertices by index. */
+template <typename T>
+class PolyMeshBuilder {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(PolyMeshBuilder);
+
+  using ScalarType = T;
+
+  PolyMeshBuilder();
+
+  /* Adds a vertex V (and its corresponding field value) to the mesh.
+
+   @param p_BV         The position of the new vertex (measured and expressed in
+                       the builder's frame B).
+   @param field_value  The value of the pressure field evaluated at V.
+   @returns The index of the newly added vertex. */
+  int AddVertex(const Vector3<T>& p_BV, const T& field_value) {
+    vertices_B_.push_back(p_BV);
+    pressures_.push_back(field_value);
+    return static_cast<int>(vertices_B_.size() - 1);
+  }
+
+  /* Adds the polygon to the in-progress mesh. The polygon is defined by
+   indices into the set of vertices that have already been added to the builder.
+
+   @param polygon_vertices  The definition of the polygon to add, expressed as
+                            ordered indices into the currently existing
+                            vertices. They should be ordered in a counter-
+                            clockwise manner such that the implied face normal
+                            points "outward" (using the right-hand rule).
+   @param nhat_B            The normal to the polygon, measured and expressed in
+                            the builder's frame B.
+   @param grad_e_MN_B       The gradient of the pressure field in the domain of
+                            the polygon, expressed in the builder's frame B.
+   @returns The number of faces added to the mesh.
+
+   @sa AddVertex(). */
+  int AddPolygon(const std::vector<int>& polygon_vertices,
+                 const Vector3<T>& nhat_B, const Vector3<T>& grad_e_MN_B);
+
+  /* Returns the total number of vertices accumulated so far. */
+  int num_vertices() const { return static_cast<int>(vertices_B_.size()); }
+
+  /* Returns the total number of faces added by calls to AddPolygon(). */
+  int num_faces() const { return polygon_count_; }
+
+  /* Create a mesh and field from the mesh data that has been aggregated by
+   this builder. */
+  std::pair<std::unique_ptr<PolygonSurfaceMesh<T>>,
+            std::unique_ptr<PolygonSurfaceMeshFieldLinear<T, T>>>
+  MakeMeshAndField();
+
+  /* Expose the accumulated, per-face gradients for testing. */
+  std::vector<Vector3<T>>& mutable_per_element_gradients() {
+    return grad_e_MN_B_per_face_;
+  }
+
+ private:
+  /* The number of polygons that have been added. It can't simply be inferred
+   from face_data_.size() because of the face encoding. */
+  int polygon_count_{0};
+  /* The definition of all faces of the mesh being built. */
+  std::vector<int> face_data_;
+  /* The per-face gradients of the pressure field. */
+  std::vector<Vector3<T>> grad_e_MN_B_per_face_;
+  /* The vertices of the mesh being built. */
+  std::vector<Vector3<T>> vertices_B_;
+  /* The pressure values (e) of the surface being built. */
+  std::vector<T> pressures_;
+};
+
 /* Given a planar, N-sided convex `polygon`, computes its centroid. The
  `polygon` is represented as an ordered list of indices into the given set of
  `vertices_F`. The resulting centroid will be measured and expressed in the same
@@ -77,10 +244,24 @@ Vector3<T> CalcPolygonCentroid(const std::vector<int>& polygon,
  @pre `n_F` has non-trivial length.
  @tparam_nonsymbolic_scalar */
 template <typename T>
-void AddPolygonToMeshData(const std::vector<int>& polygon,
-                          const Vector3<T>& n_F,
-                          std::vector<SurfaceTriangle>* faces,
-                          std::vector<Vector3<T>>* vertices_F);
+void AddPolygonToTriangleMeshData(const std::vector<int>& polygon,
+                                  const Vector3<T>& n_F,
+                                  std::vector<SurfaceTriangle>* faces,
+                                  std::vector<Vector3<T>>* vertices_F);
+
+/* Adds a polygon (defined by indices into a set of vertices) into the polygon
+ face data (as defined by PolygonSurfaceMesh).
+ 
+ This is similar to AddPolygonToTriangleMeshData() in that the specified polygon
+ is added to some representation of mesh faces. It's different in the following
+ ways:
+
+   1. The face_data isn't literally a vector of discrete faces, but an encoding
+      of the entire set of mesh polygons (as documented by PolygonSurfaceMesh).
+   2. No normal or vertices are required because adding a polygon requires no
+      operation on pre-existing vertex data (i.e., calculation of centroid). */
+void AddPolygonToPolygonMeshData(const std::vector<int>& polygon,
+                                 std::vector<int>* face_data);
 
 /* Determines if the indicated triangle has a face normal that is "in the
  direction" of the given normal.