[proximity] Adds VolumeMeshTopology (#21746)

geometry/proximity/BUILD.bazel

@@ -63,6 +63,7 @@ drake_cc_package_library(
         ":triangle_surface_mesh",
         ":volume_mesh",
         ":volume_mesh_refiner",
+        ":volume_mesh_topology",
         ":volume_to_surface_mesh",
         ":vtk_to_volume_mesh",
     ],
@@ -924,6 +925,20 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "volume_mesh_topology",
+    srcs = [
+        "volume_mesh_topology.cc",
+    ],
+    hdrs = [
+        "volume_mesh_topology.h",
+    ],
+    deps = [
+        ":sorted_triplet",
+        ":volume_mesh",
+    ],
+)
+
 drake_cc_library(
     name = "volume_mesh_refiner",
     srcs = ["volume_mesh_refiner.cc"],
@@ -1680,6 +1695,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "volume_mesh_topology_test",
+    deps = [
+        ":volume_mesh_topology",
+    ],
+)
+
 drake_cc_googletest(
     name = "volume_to_surface_mesh_test",
     deps = [

geometry/proximity/test/volume_mesh_topology_test.cc

@@ -0,0 +1,125 @@
+#include "drake/geometry/proximity/volume_mesh_topology.h"
+
+#include <utility>
+
+#include <gtest/gtest.h>
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+namespace {
+
+// Test instantiation of VolumeMeshTopology of a geometry M and inspecting its
+// components.
+GTEST_TEST(VolumeMeshTopologyTest, TestVolumeMeshTopology) {
+  // A trivial volume mesh comprises of two tetrahedral elements with
+  // vertices on the coordinate axes and the origin like this:
+  //
+  //      +Z
+  //       |
+  //       v3
+  //       |
+  //       |
+  //     v0+------v2---+Y
+  //      /|
+  //     / |
+  //   v1  v4
+  //   /   |
+  // +X    |
+  //      -Z
+  //
+  // In the picture above, the positions are expressed in M's frame.
+  const int element_data[2][4] = {{0, 1, 2, 3}, {0, 2, 1, 4}};
+  std::vector<VolumeElement> elements;
+  for (int e = 0; e < 2; ++e) elements.emplace_back(element_data[e]);
+  const Vector3<double> vertex_data[5] = {
+      Vector3<double>::Zero(), Vector3<double>::UnitX(),
+      Vector3<double>::UnitY(), Vector3<double>::UnitZ(),
+      -Vector3<double>::UnitZ()};
+  std::vector<Vector3<double>> vertices_W;
+  for (int v = 0; v < 5; ++v) {
+    vertices_W.emplace_back(vertex_data[v]);
+  }
+  const VolumeMesh<double> volume_mesh_W(std::move(elements),
+                                         std::move(vertices_W));
+
+  const VolumeMeshTopology volume_mesh_topology(volume_mesh_W);
+  // The only shared face is (v0, v1, v2) across from v3 (index 3 face of
+  // element 0) and across from v4 (index 3 face of element 1). All other
+  // indices are -1 indicating boundary.
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 0), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 1), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 2), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 3), 1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 0), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 1), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 2), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 3), 0);
+}
+
+// Test instantiation of VolumeMeshTopology of a slightly more complicated
+// geometry M and inspecting its components.
+GTEST_TEST(VolumeMeshTopologyTest, TestVolumeMeshOctahedronTopology) {
+  // A volume mesh of an octahedron:
+  //      +Z
+  //       |
+  //       v5
+  //       |
+  //       |
+  //     v0+------v3---+Y
+  //      /|
+  //     / |
+  //   v1  |     v2
+  //   /   |
+  // +X    v4
+  //       |
+  //      -Z
+  //
+  const int element_data[4][4] = {
+      {0, 1, 3, 5}, {1, 2, 3, 5}, {0, 3, 1, 4}, {1, 3, 2, 4}};
+  std::vector<VolumeElement> elements;
+  for (int e = 0; e < 4; ++e) elements.emplace_back(element_data[e]);
+  const Vector3<double> vertex_data[6] = {
+      Vector3<double>::Zero(),
+      Vector3<double>::UnitX(),
+      Vector3<double>::UnitX() + Vector3<double>::UnitY(),
+      Vector3<double>::UnitY(),
+      -Vector3<double>::UnitZ(),
+      Vector3<double>::UnitZ()};
+  std::vector<Vector3<double>> vertices_W;
+  for (int v = 0; v < 6; ++v) {
+    vertices_W.emplace_back(vertex_data[v]);
+  }
+  const VolumeMesh<double> volume_mesh_W(std::move(elements),
+                                         std::move(vertices_W));
+
+  const VolumeMeshTopology volume_mesh_topology(volume_mesh_W);
+  // There are four shared faces:
+  // (v1, v3, v5) is shared by tet 0 at face 0 and tet 1 at face 1
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 0), 1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 1), 0);
+  // (v1, v2, v3) is shared by tet 1 at face 3 and tet 3 at face 3
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 3), 3);
+  EXPECT_EQ(volume_mesh_topology.neighbor(3, 3), 1);
+  // (v1, v3, v4) is shared by tet 2 at face 0 and tet 3 at face 2
+  EXPECT_EQ(volume_mesh_topology.neighbor(2, 0), 3);
+  EXPECT_EQ(volume_mesh_topology.neighbor(3, 2), 2);
+  // (v0, v1, v3) is shared by tet 0 at face 3 and tet 2 at face 3
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 3), 2);
+  EXPECT_EQ(volume_mesh_topology.neighbor(2, 3), 0);
+  // All other neighbors are -1, indicating boundary
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 1), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(0, 2), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 0), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(1, 2), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(2, 1), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(2, 2), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(3, 0), -1);
+  EXPECT_EQ(volume_mesh_topology.neighbor(3, 1), -1);
+}
+
+}  // namespace
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/volume_mesh_topology.cc

@@ -0,0 +1,70 @@
+#include "drake/geometry/proximity/volume_mesh_topology.h"
+
+#include <map>
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+VolumeMeshTopology::~VolumeMeshTopology() = default;
+
+void VolumeMeshTopology::Initialize(
+    const std::vector<VolumeElement>& elements) {
+  tetrahedra_neighbors_.resize(ssize(elements),
+                               std::array<int, 4>{-1, -1, -1, -1});
+
+  // Get the cannonical representation of face f of element e.
+  auto get_face = [](const VolumeElement& e, int f) {
+    int a = e.vertex((f + 1) % 4);
+    int b = e.vertex((f + 2) % 4);
+    int c = e.vertex((f + 3) % 4);
+
+    return SortedTriplet<int>(a, b, c);
+  };
+
+  // Maps faces to the first tet we encountered containing that tet.
+  std::map<SortedTriplet<int>, int> face_to_tet;
+
+  // Establish neighbors for all tets.
+  for (int tet_index = 0; tet_index < ssize(elements); ++tet_index) {
+    const VolumeElement& e = elements.at(tet_index);
+
+    for (int face_index = 0; face_index < 4; ++face_index) {
+      SortedTriplet<int> face = get_face(e, face_index);
+
+      // If we've seen this face before, we now know the two tets that
+      // neighbor on `face`.
+      if (face_to_tet.contains(face)) {
+        int neighbor_tet_index = face_to_tet.at(face);
+
+        // Set the neighbor of the current tet at `face`'s index to the
+        // neighbor tet index.
+        tetrahedra_neighbors_[tet_index][face_index] = neighbor_tet_index;
+
+        // Find the index of `face` in the neighbor tet.
+        int matching_face_index = -1;
+        for (int neighbor_face_index = 0; neighbor_face_index < 4;
+             ++neighbor_face_index) {
+          if (get_face(elements.at(neighbor_tet_index), neighbor_face_index) ==
+              face) {
+            matching_face_index = neighbor_face_index;
+            break;
+          }
+        }
+        DRAKE_ASSERT(matching_face_index != -1);
+        // Set the neighbor of the neighboring tet at `face`'s matching index
+        // to the current tet index.
+        tetrahedra_neighbors_[neighbor_tet_index][matching_face_index] =
+            tet_index;
+
+      } else {
+        // We haven't seen `face` before, so just map it to the current tet.
+        face_to_tet[face] = tet_index;
+      }
+    }
+  }
+}
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/volume_mesh_topology.h

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <array>
+#include <vector>
+
+#include "drake/geometry/proximity/sorted_triplet.h"
+#include "drake/geometry/proximity/volume_mesh.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+/* %VolumeMeshTopology represents the topology of a tetrahedral volume mesh.
+ */
+class VolumeMeshTopology {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(VolumeMeshTopology);
+
+  template <typename T>
+  explicit VolumeMeshTopology(const VolumeMesh<T>& mesh) {
+    Initialize(mesh.tetrahedra());
+  }
+
+  ~VolumeMeshTopology();
+
+  /*
+   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];
+  }
+
+ private:
+  // 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).
+  std::vector<std::array<int, 4>> tetrahedra_neighbors_;
+};
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake