[geometry] Adds surface mesh, bvh, and volume mesh topology to SoftMe…

…sh. (RobotLocomotion#22098)

geometry/proximity/BUILD.bazel

@@ -431,6 +431,8 @@ drake_cc_library(
         ":tessellation_strategy",
         ":triangle_surface_mesh",
         ":volume_mesh",
+        ":volume_mesh_topology",
+        ":volume_to_surface_mesh",
         "//common:copyable_unique_ptr",
         "//common:essential",
         "//geometry:geometry_ids",

geometry/proximity/hydroelastic_internal.cc

@@ -90,6 +90,22 @@ bool is_primitive(const Shape& shape) {
 
 using std::make_unique;
 
+SoftMesh::SoftMesh(
+    std::unique_ptr<VolumeMesh<double>> mesh,
+    std::unique_ptr<VolumeMeshFieldLinear<double, double>> pressure)
+    : mesh_(std::move(mesh)),
+      pressure_(std::move(pressure)),
+      bvh_(std::make_unique<Bvh<Obb, VolumeMesh<double>>>(*mesh_)) {
+  DRAKE_ASSERT(mesh_.get() == &pressure_->mesh());
+  tri_to_tet_ = std::make_unique<std::vector<TetFace>>();
+  surface_mesh_ = std::make_unique<TriangleSurfaceMesh<double>>(
+      ConvertVolumeToSurfaceMeshWithBoundaryVertices(*mesh_, nullptr,
+                                                     tri_to_tet_.get()));
+  surface_mesh_bvh_ =
+      std::make_unique<Bvh<Obb, TriangleSurfaceMesh<double>>>(*surface_mesh_);
+  mesh_topology_ = std::make_unique<VolumeMeshTopology>(*mesh_);
+}
+
 SoftMesh& SoftMesh::operator=(const SoftMesh& s) {
   if (this == &s) return *this;
 
@@ -98,7 +114,12 @@ SoftMesh& SoftMesh::operator=(const SoftMesh& s) {
   // the new mesh. So, we use CloneAndSetMesh() instead.
   pressure_ = s.pressure().CloneAndSetMesh(mesh_.get());
   bvh_ = make_unique<Bvh<Obb, VolumeMesh<double>>>(s.bvh());
-
+  surface_mesh_ =
+      std::make_unique<TriangleSurfaceMesh<double>>(s.surface_mesh());
+  tri_to_tet_ = std::make_unique<std::vector<TetFace>>(s.tri_to_tet());
+  surface_mesh_bvh_ = std::make_unique<Bvh<Obb, TriangleSurfaceMesh<double>>>(
+      s.surface_mesh_bvh());
+  mesh_topology_ = std::make_unique<VolumeMeshTopology>(s.mesh_topology());
   return *this;
 }
 

geometry/proximity/hydroelastic_internal.h

@@ -6,6 +6,7 @@
 #include <unordered_set>
 #include <utility>
 #include <variant>
+#include <vector>
 
 #include "drake/common/copyable_unique_ptr.h"
 #include "drake/common/drake_assert.h"
@@ -15,6 +16,8 @@
 #include "drake/geometry/proximity/bvh.h"
 #include "drake/geometry/proximity/triangle_surface_mesh.h"
 #include "drake/geometry/proximity/volume_mesh_field.h"
+#include "drake/geometry/proximity/volume_mesh_topology.h"
+#include "drake/geometry/proximity/volume_to_surface_mesh.h"
 #include "drake/geometry/proximity_properties.h"
 #include "drake/geometry/shape_specification.h"
 
@@ -25,44 +28,77 @@ namespace hydroelastic {
 
 // TODO(SeanCurtis-TRI): When we do soft-soft contact, we'll need ∇p̃(e) as well.
 //  ∇p̃(e) is piecewise constant -- one ℜ³ vector per tetrahedron.
-/* Defines a soft mesh -- a mesh, its linearized pressure field, p̃(e), and its
- bounding volume hierarchy. While this class retains ownership of the mesh,
- we assume that both the pressure field and the bounding volume hierarchy
- are derived from the mesh. */
+/* Defines a soft mesh -- a volume mesh, its linearized pressure field, p̃(e),
+and derived objects:
+  - The bounding volume hierarchy of the volume mesh
+  - The surface of the volume mesh
+  - The bounding volume hierarchy of the surface
+  - The mapping from surface mesh triangles to volume mesh tetrahedra
+  - The topology of the volume mesh.
+*/
 class SoftMesh {
  public:
   SoftMesh() = default;
 
   SoftMesh(std::unique_ptr<VolumeMesh<double>> mesh,
-           std::unique_ptr<VolumeMeshFieldLinear<double, double>> pressure)
-      : mesh_(std::move(mesh)),
-        pressure_(std::move(pressure)),
-        bvh_(std::make_unique<Bvh<Obb, VolumeMesh<double>>>(*mesh_)) {
-    DRAKE_ASSERT(mesh_.get() == &pressure_->mesh());
-  }
+           std::unique_ptr<VolumeMeshFieldLinear<double, double>> pressure);
 
   SoftMesh(const SoftMesh& s) { *this = s; }
   SoftMesh& operator=(const SoftMesh& s);
   SoftMesh(SoftMesh&&) = default;
   SoftMesh& operator=(SoftMesh&&) = default;
 
+  /* The mesh representing this SoftMesh. */
   const VolumeMesh<double>& mesh() const {
     DRAKE_DEMAND(mesh_ != nullptr);
     return *mesh_;
   }
+
+  /* The surface of the mesh provided by mesh(). */
+  const TriangleSurfaceMesh<double>& surface_mesh() const {
+    DRAKE_DEMAND(surface_mesh_ != nullptr);
+    return *surface_mesh_;
+  }
+
+  /* The pressure field associated with mesh(). */
   const VolumeMeshFieldLinear<double, double>& pressure() const {
     DRAKE_DEMAND(pressure_ != nullptr);
     return *pressure_;
   }
+
+  /* The BVH of the mesh provided by mesh(). */
   const Bvh<Obb, VolumeMesh<double>>& bvh() const {
     DRAKE_DEMAND(bvh_ != nullptr);
     return *bvh_;
   }
 
+  /* The BVH of the mesh provided by surface_mesh(). */
+  const Bvh<Obb, TriangleSurfaceMesh<double>>& surface_mesh_bvh() const {
+    DRAKE_DEMAND(surface_mesh_bvh_ != nullptr);
+    return *surface_mesh_bvh_;
+  }
+
+  /* A record of which TetFace each triangle of surface_mesh() came from. The
+  iᵗʰ entry in the returned vector corresponds to the iᵗʰ triangle in
+  surface_mesh(). */
+  const std::vector<TetFace>& tri_to_tet() const { return *tri_to_tet_; }
+
+  /* The VolumeMeshTopology computed from the mesh provided by mesh(). */
+  const VolumeMeshTopology& mesh_topology() const {
+    DRAKE_DEMAND(mesh_topology_ != nullptr);
+    return *mesh_topology_;
+  }
+
  private:
+  // TODO(SeanCurtis-TRI): Determine if there is a need for these to all be
+  // unique_ptr and remove the indirection if not necessary.
   std::unique_ptr<VolumeMesh<double>> mesh_;
   std::unique_ptr<VolumeMeshFieldLinear<double, double>> pressure_;
   std::unique_ptr<Bvh<Obb, VolumeMesh<double>>> bvh_;
+  std::unique_ptr<TriangleSurfaceMesh<double>> surface_mesh_;
+  std::unique_ptr<Bvh<Obb, TriangleSurfaceMesh<double>>> surface_mesh_bvh_;
+  std::unique_ptr<VolumeMeshTopology> mesh_topology_;
+  std::unique_ptr<std::vector<TetFace>> tri_to_tet_;
 };
 
 /* Defines a soft half space. The half space is defined such that the half
@@ -125,12 +161,34 @@ class SoftGeometry {
 
    This can be accomplished by querying `is_half_space()`. Attempting to access
    data members of the *wrong* type will throw an exception.  */
+  // TODO(SeanCurtis-TRI): remove all of these legacy API wrappers for SoftMesh
+  // and SoftHalfSpace.
   //@{
 
   bool is_half_space() const {
     return std::holds_alternative<SoftHalfSpace>(geometry_);
   }
 
+  /* Returns a reference to the SoftMesh -- calling this will throw if
+   is_half_space() returns `true`.  */
+  const SoftMesh& soft_mesh() const {
+    if (is_half_space()) {
+      throw std::runtime_error(
+          "SoftGeometry::soft_mesh() cannot be invoked for soft half space.");
+    }
+    return std::get<SoftMesh>(geometry_);
+  }
+
+  /* Returns a reference to the SoftHalfSpace -- calling this will throw if
+   is_half_space() returns `false`.  */
+  const SoftHalfSpace& soft_half_space() const {
+    if (!is_half_space()) {
+      throw std::runtime_error(
+          "SoftGeometry::soft_half_space() cannot be invoked for soft mesh.");
+    }
+    return std::get<SoftHalfSpace>(geometry_);
+  }
+
   /* Returns a reference to the volume mesh -- calling this will throw if
    is_half_space() returns `true`.  */
   const VolumeMesh<double>& mesh() const {

geometry/proximity/test/hydroelastic_internal_test.cc

@@ -49,6 +49,10 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
     EXPECT_NE(&original.mesh(), &copy.mesh());
     EXPECT_NE(&original.pressure(), &copy.pressure());
     EXPECT_NE(&original.bvh(), &copy.bvh());
+    EXPECT_NE(&original.surface_mesh(), &copy.surface_mesh());
+    EXPECT_NE(&original.surface_mesh_bvh(), &copy.surface_mesh_bvh());
+    EXPECT_NE(&original.tri_to_tet(), &copy.tri_to_tet());
+    EXPECT_NE(&original.mesh_topology(), &copy.mesh_topology());
 
     EXPECT_TRUE(copy.mesh().Equal(original.mesh()));
 
@@ -61,6 +65,11 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
     EXPECT_TRUE(copy_pressure.Equal(original_pressure));
 
     EXPECT_TRUE(copy.bvh().Equal(original.bvh()));
+
+    EXPECT_TRUE(copy.surface_mesh().Equal(original.surface_mesh()));
+    EXPECT_TRUE(copy.surface_mesh_bvh().Equal(original.surface_mesh_bvh()));
+    EXPECT_EQ(copy.tri_to_tet(), original.tri_to_tet());
+    EXPECT_TRUE(copy.mesh_topology().Equal(original.mesh_topology()));
   }
 
   // Test copy constructor.
@@ -71,6 +80,10 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
     EXPECT_NE(&original.mesh(), &copy.mesh());
     EXPECT_NE(&original.pressure(), &copy.pressure());
     EXPECT_NE(&original.bvh(), &copy.bvh());
+    EXPECT_NE(&original.surface_mesh(), &copy.surface_mesh());
+    EXPECT_NE(&original.surface_mesh_bvh(), &copy.surface_mesh_bvh());
+    EXPECT_NE(&original.tri_to_tet(), &copy.tri_to_tet());
+    EXPECT_NE(&original.mesh_topology(), &copy.mesh_topology());
 
     EXPECT_TRUE(copy.mesh().Equal(original.mesh()));
 
@@ -83,6 +96,11 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
     EXPECT_TRUE(copy_pressure.Equal(original_pressure));
 
     EXPECT_TRUE(copy.bvh().Equal(original.bvh()));
+
+    EXPECT_TRUE(copy.surface_mesh().Equal(original.surface_mesh()));
+    EXPECT_TRUE(copy.surface_mesh_bvh().Equal(original.surface_mesh_bvh()));
+    EXPECT_EQ(copy.tri_to_tet(), original.tri_to_tet());
+    EXPECT_TRUE(copy.mesh_topology().Equal(original.mesh_topology()));
   }
 
   // Test move constructor and move-assignment operator.
@@ -98,19 +116,33 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
     const VolumeMeshFieldLinear<double, double>* const pressure_ptr =
         &start.pressure();
     const Bvh<Obb, VolumeMesh<double>>* const bvh_ptr = &start.bvh();
+    const TriangleSurfaceMesh<double>* const surface_mesh_ptr =
+        &start.surface_mesh();
+    const Bvh<Obb, TriangleSurfaceMesh<double>>* const surface_mesh_bvh_ptr =
+        &start.surface_mesh_bvh();
+    const std::vector<TetFace>* const tri_to_tet_ptr = &start.tri_to_tet();
+    const VolumeMeshTopology* const mesh_topology_ptr = &start.mesh_topology();
 
     // Test move constructor.
     SoftMesh move_constructed(std::move(start));
     EXPECT_EQ(&move_constructed.mesh(), mesh_ptr);
     EXPECT_EQ(&move_constructed.pressure(), pressure_ptr);
     EXPECT_EQ(&move_constructed.bvh(), bvh_ptr);
+    EXPECT_EQ(&move_constructed.surface_mesh(), surface_mesh_ptr);
+    EXPECT_EQ(&move_constructed.surface_mesh_bvh(), surface_mesh_bvh_ptr);
+    EXPECT_EQ(&move_constructed.tri_to_tet(), tri_to_tet_ptr);
+    EXPECT_EQ(&move_constructed.mesh_topology(), mesh_topology_ptr);
 
     // Test move-assignment operator.
     SoftMesh move_assigned;
     move_assigned = std::move(move_constructed);
     EXPECT_EQ(&move_assigned.mesh(), mesh_ptr);
     EXPECT_EQ(&move_assigned.pressure(), pressure_ptr);
     EXPECT_EQ(&move_assigned.bvh(), bvh_ptr);
+    EXPECT_EQ(&move_assigned.surface_mesh(), surface_mesh_ptr);
+    EXPECT_EQ(&move_assigned.surface_mesh_bvh(), surface_mesh_bvh_ptr);
+    EXPECT_EQ(&move_assigned.tri_to_tet(), tri_to_tet_ptr);
+    EXPECT_EQ(&move_assigned.mesh_topology(), mesh_topology_ptr);
   }
 }
 
@@ -122,6 +154,8 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) {
 // std::variant and the move/copy semantics of the underlying data types
 // (already tested). If SoftGeometry changes its implementation details, this
 // logic would need to be revisited.
+// TODO(SeanCurtis-TRI): Clean up these tests to remove usage of legacy API
+// wrappers in SoftGeometry.
 GTEST_TEST(SoftGeometryTest, TestCopyMoveAssignConstruct) {
   const Sphere sphere(0.5);
   const double resolution_hint = 0.5;
@@ -133,6 +167,11 @@ GTEST_TEST(SoftGeometryTest, TestCopyMoveAssignConstruct) {
 
   const SoftGeometry original(SoftMesh(std::move(mesh), std::move(pressure)));
 
+  // In all of the following tests we are only looking for evidence of
+  // copying/moving. Since SoftGeometry relies on the copy/move semantics of the
+  // underlying data types, we therefore do not check exhaustively for a deep
+  // copy.
+
   // Test copy-assignment operator.
   {
     // Initialize `dut` as a SoftGeometry representing a half space.
@@ -965,6 +1004,11 @@ TEST_F(HydroelasticSoftGeometryTest, HalfSpace) {
   EXPECT_EQ(half_space->pressure_scale(),
             properties.GetProperty<double>(kHydroGroup, kElastic) / thickness);
 
+  DRAKE_EXPECT_NO_THROW(half_space->soft_half_space());
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      half_space->soft_mesh(),
+      "SoftGeometry::soft_mesh.* cannot be invoked for soft half space.*");
+
   DRAKE_EXPECT_THROWS_MESSAGE(
       half_space->mesh(),
       "SoftGeometry::mesh.* cannot be invoked .* half space");
@@ -998,6 +1042,12 @@ TEST_F(HydroelasticSoftGeometryTest, Sphere) {
   // This is the only test where we confirm that bvh() *doesn't* throw for
   // meshes and slab_thickness() does.
   EXPECT_NO_THROW(sphere1->bvh());
+
+  DRAKE_EXPECT_NO_THROW(sphere1->soft_mesh());
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      sphere1->soft_half_space(),
+      "SoftGeometry::soft_half_space.* cannot be invoked for soft mesh.*");
+
   DRAKE_EXPECT_THROWS_MESSAGE(
       sphere1->pressure_scale(),
       "SoftGeometry::pressure_scale.* cannot be invoked .* soft mesh");
@@ -1041,7 +1091,7 @@ TEST_F(HydroelasticSoftGeometryTest, Sphere) {
     // of two --> sphere 2's level of refinement is one greater than sphere
     // 1's. Both are missing the "tessellation_strategy" property so it should
     // default to kSingleInteriorVertex. So, sphere 2 must have 4X the
-    // tetrahedra as sphere 1.
+    // tetrahedra and surface faces as sphere 1.
     EXPECT_EQ(sphere1->mesh().num_elements() * 4,
               sphere2->mesh().num_elements());
   }
@@ -1051,7 +1101,7 @@ TEST_F(HydroelasticSoftGeometryTest, Sphere) {
     // of tets (compared to an otherwise identical mesh declared to sparse).
 
     // Starting with sphere 1's properties, we'll set it to dense and observe
-    // more tets.
+    // more tets but the same amount of surface faces.
     ProximityProperties dense_properties(properties1);
     dense_properties.AddProperty(kHydroGroup, "tessellation_strategy",
                                  TessellationStrategy::kDenseInteriorVertices);

geometry/proximity/volume_mesh_topology.cc

@@ -65,6 +65,10 @@ void VolumeMeshTopology::Initialize(
   }
 }
 
+bool VolumeMeshTopology::Equal(const VolumeMeshTopology& topology) const {
+  return tetrahedra_neighbors_ == topology.tetrahedra_neighbors_;
+}
+
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake

geometry/proximity/volume_mesh_topology.h

@@ -23,29 +23,36 @@ class VolumeMeshTopology {
 
   ~VolumeMeshTopology();
 
-  /*
-   Returns the index of `i`-th neighbor of tet `e` (i.e. the tet across from
+  /* Returns the index of `i`-th neighbor of tet `e` (i.e. the tet across from
    vertex `i`). If tet `e` does not have a neighbor across from `i` (i.e. face
    `i` is a boundary face), returns -1.
    @param e The index of the element.
    @param i The index of the neighbor
    @pre `e ∈ [0, mesh().num_elements())`.
    @pre `i ∈ [0, 3]`.
-  */
+   */
   int neighbor(int e, int i) const {
     DRAKE_DEMAND(0 <= e && e < ssize(tetrahedra_neighbors_));
     DRAKE_DEMAND(0 <= i && i < 4);
     return tetrahedra_neighbors_[e][i];
   }
 
+  /* Returns true if `topology` is bit-wise equal to `this` (i.e. member
+   containers are exact copies of each other). Note: this method is not intended
+   to distinguish whether two `VolumeMeshTopology` are *topologically*
+   equivalent to each other (i.e. whether they describe the same adjacency graph
+   up to isomorphism).
+   */
+  bool Equal(const VolumeMeshTopology& topology) const;
+
  private:
-  // Calculates the adjacency information for all tetrahedra in `elements`.
+  /* Calculates the adjacency information for all tetrahedra in `elements`. */
   void Initialize(const std::vector<VolumeElement>& elements);
 
-  // Stores the index of the neighboring tetrahedra of the element at index i.
-  // The index stored at index j is the neighbor across for vertex j, or in
-  // other terms the tet that shares face {0, 1, 2, 3} / {i}. -1 is used to
-  // represent the absence of a neighbor on a face (i.e. a boundary face).
+  /* Stores the index of the neighboring tetrahedra of the element at index i.
+   The index stored at index j is the neighbor across for vertex j, or in
+   other terms the tet that shares face {0, 1, 2, 3} / {i}. -1 is used to
+   represent the absence of a neighbor on a face (i.e. a boundary face). */
   std::vector<std::array<int, 4>> tetrahedra_neighbors_;
 };
 

geometry/proximity/volume_to_surface_mesh.cc

@@ -26,6 +26,8 @@ struct BoundaryFace {
 
 }  // namespace
 
+bool TetFace::operator==(const TetFace&) const = default;
+
 std::vector<std::array<int, 3>> IdentifyBoundaryFaces(
     const std::vector<VolumeElement>& tetrahedra,
     std::vector<TetFace>* element_indices) {