diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index e5baf4cdc5f8..01d831fe95b1 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -62,6 +62,7 @@ drake_cc_package_library( ":triangle_surface_mesh", ":volume_mesh", ":volume_mesh_refiner", + ":volume_mesh_topology", ":volume_to_surface_mesh", ":vtk_to_volume_mesh", ], @@ -881,6 +882,22 @@ 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", + "//common:default_scalars", + "//common:reset_on_copy", + ], +) + drake_cc_library( name = "volume_mesh_refiner", srcs = ["volume_mesh_refiner.cc"], @@ -1617,6 +1634,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 = [ diff --git a/geometry/proximity/test/volume_mesh_topology_test.cc b/geometry/proximity/test/volume_mesh_topology_test.cc new file mode 100644 index 000000000000..ee7e724a714e --- /dev/null +++ b/geometry/proximity/test/volume_mesh_topology_test.cc @@ -0,0 +1,126 @@ +#include "drake/geometry/proximity/volume_mesh_topology.h" + +#include +#include + +#include + +namespace drake { +namespace geometry { + +namespace { + +using internal::VolumeMeshTopology; + +// 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 elements; + for (int e = 0; e < 2; ++e) elements.emplace_back(element_data[e]); + const Vector3 vertex_data[5] = { + Vector3::Zero(), Vector3::UnitX(), + Vector3::UnitY(), Vector3::UnitZ(), + -Vector3::UnitZ()}; + std::vector> vertices_W; + for (int v = 0; v < 5; ++v) { + vertices_W.emplace_back(vertex_data[v]); + } + const VolumeMesh 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 elements; + for (int e = 0; e < 4; ++e) elements.emplace_back(element_data[e]); + const Vector3 vertex_data[6] = { + Vector3::Zero(), + Vector3::UnitX(), + Vector3::UnitX() + Vector3::UnitY(), + Vector3::UnitY(), + -Vector3::UnitZ(), + Vector3::UnitZ()}; + std::vector> vertices_W; + for (int v = 0; v < 6; ++v) { + vertices_W.emplace_back(vertex_data[v]); + } + const VolumeMesh 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 geometry +} // namespace drake diff --git a/geometry/proximity/volume_mesh_topology.cc b/geometry/proximity/volume_mesh_topology.cc new file mode 100644 index 000000000000..10bd29e41934 --- /dev/null +++ b/geometry/proximity/volume_mesh_topology.cc @@ -0,0 +1,65 @@ +#include "drake/geometry/proximity/volume_mesh_topology.h" + +#include + +namespace drake { +namespace geometry { +namespace internal { + +void VolumeMeshTopology::CalcAdjacentTetrahedraNeighbors( + const std::vector& elements) { + // 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(a, b, c); + }; + + // Maps faces to the first tet we encountered containing that tet. + std::map, 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 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 diff --git a/geometry/proximity/volume_mesh_topology.h b/geometry/proximity/volume_mesh_topology.h new file mode 100644 index 000000000000..21ec0fe015c7 --- /dev/null +++ b/geometry/proximity/volume_mesh_topology.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#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 + explicit VolumeMeshTopology(const VolumeMesh& mesh) { + tetrahedra_neighbors_.resize(mesh.num_elements(), + std::array{-1, -1, -1, -1}); + + CalcAdjacentTetrahedraNeighbors(mesh.tetrahedra()); + } + + /** + 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: + // Calculate the adjacency information for all tetrahedra in `elements`. + void CalcAdjacentTetrahedraNeighbors( + const std::vector& 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> tetrahedra_neighbors_; +}; + +} // namespace internal +} // namespace geometry +} // namespace drake