From 0cd5158cf2295bf4cb9906dedf64b3f8ced30010 Mon Sep 17 00:00:00 2001 From: Joseph Masterjohn Date: Wed, 23 Oct 2024 15:45:12 -0400 Subject: [PATCH] [geometry] Adds surface mesh, bvh, and volume mesh topology to SoftMesh. --- geometry/proximity/BUILD.bazel | 2 + geometry/proximity/hydroelastic_internal.cc | 23 +++++++- geometry/proximity/hydroelastic_internal.h | 50 ++++++++++++++--- .../test/hydroelastic_internal_test.cc | 54 ++++++++++++++++++- geometry/proximity/volume_mesh_topology.cc | 4 ++ geometry/proximity/volume_mesh_topology.h | 23 +++++--- geometry/proximity/volume_to_surface_mesh.h | 2 + 7 files changed, 141 insertions(+), 17 deletions(-) diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index 0db4725970cd..ec7a2da6928f 100644 --- a/geometry/proximity/BUILD.bazel +++ b/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", diff --git a/geometry/proximity/hydroelastic_internal.cc b/geometry/proximity/hydroelastic_internal.cc index 035cbf47f681..f059248a10cf 100644 --- a/geometry/proximity/hydroelastic_internal.cc +++ b/geometry/proximity/hydroelastic_internal.cc @@ -90,6 +90,22 @@ bool is_primitive(const Shape& shape) { using std::make_unique; +SoftMesh::SoftMesh( + std::unique_ptr> mesh, + std::unique_ptr> pressure) + : mesh_(std::move(mesh)), + pressure_(std::move(pressure)), + bvh_(std::make_unique>>(*mesh_)) { + DRAKE_ASSERT(mesh_.get() == &pressure_->mesh()); + tri_to_tet_ = std::make_unique>(); + surface_mesh_ = std::make_unique>( + ConvertVolumeToSurfaceMeshWithBoundaryVertices(*mesh_, nullptr, + tri_to_tet_.get())); + surface_mesh_bvh_ = + std::make_unique>>(*surface_mesh_); + mesh_topology_ = std::make_unique(*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>>(s.bvh()); - + surface_mesh_ = + std::make_unique>(s.surface_mesh()); + tri_to_tet_ = std::make_unique>(s.tri_to_tet()); + surface_mesh_bvh_ = std::make_unique>>( + s.surface_mesh_bvh()); + mesh_topology_ = std::make_unique(s.mesh_topology()); return *this; } diff --git a/geometry/proximity/hydroelastic_internal.h b/geometry/proximity/hydroelastic_internal.h index a67f062fcd85..4adb0801d9ca 100644 --- a/geometry/proximity/hydroelastic_internal.h +++ b/geometry/proximity/hydroelastic_internal.h @@ -6,6 +6,7 @@ #include #include #include +#include #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" @@ -34,12 +37,7 @@ class SoftMesh { SoftMesh() = default; SoftMesh(std::unique_ptr> mesh, - std::unique_ptr> pressure) - : mesh_(std::move(mesh)), - pressure_(std::move(pressure)), - bvh_(std::make_unique>>(*mesh_)) { - DRAKE_ASSERT(mesh_.get() == &pressure_->mesh()); - } + std::unique_ptr> pressure); SoftMesh(const SoftMesh& s) { *this = s; } SoftMesh& operator=(const SoftMesh& s); @@ -50,6 +48,10 @@ class SoftMesh { DRAKE_DEMAND(mesh_ != nullptr); return *mesh_; } + const TriangleSurfaceMesh& surface_mesh() const { + DRAKE_DEMAND(surface_mesh_ != nullptr); + return *surface_mesh_; + } const VolumeMeshFieldLinear& pressure() const { DRAKE_DEMAND(pressure_ != nullptr); return *pressure_; @@ -58,11 +60,25 @@ class SoftMesh { DRAKE_DEMAND(bvh_ != nullptr); return *bvh_; } + const Bvh>& surface_mesh_bvh() const { + DRAKE_DEMAND(surface_mesh_bvh_ != nullptr); + return *surface_mesh_bvh_; + } + const std::vector& tri_to_tet() const { return *tri_to_tet_; } + const VolumeMeshTopology& mesh_topology() const { + DRAKE_DEMAND(mesh_topology_ != nullptr); + return *mesh_topology_; + } private: std::unique_ptr> mesh_; std::unique_ptr> pressure_; std::unique_ptr>> bvh_; + + std::unique_ptr> surface_mesh_; + std::unique_ptr>> surface_mesh_bvh_; + std::unique_ptr mesh_topology_; + std::unique_ptr> tri_to_tet_; }; /* Defines a soft half space. The half space is defined such that the half @@ -125,12 +141,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(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(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(geometry_); + } + /* Returns a reference to the volume mesh -- calling this will throw if is_half_space() returns `true`. */ const VolumeMesh& mesh() const { diff --git a/geometry/proximity/test/hydroelastic_internal_test.cc b/geometry/proximity/test/hydroelastic_internal_test.cc index 68dd8322fd56..63d0f4e3ff03 100644 --- a/geometry/proximity/test/hydroelastic_internal_test.cc +++ b/geometry/proximity/test/hydroelastic_internal_test.cc @@ -49,6 +49,10 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) { EXPECT_NE(&original.mesh(), ©.mesh()); EXPECT_NE(&original.pressure(), ©.pressure()); EXPECT_NE(&original.bvh(), ©.bvh()); + EXPECT_NE(&original.surface_mesh(), ©.surface_mesh()); + EXPECT_NE(&original.surface_mesh_bvh(), ©.surface_mesh_bvh()); + EXPECT_NE(&original.tri_to_tet(), ©.tri_to_tet()); + EXPECT_NE(&original.mesh_topology(), ©.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(), ©.mesh()); EXPECT_NE(&original.pressure(), ©.pressure()); EXPECT_NE(&original.bvh(), ©.bvh()); + EXPECT_NE(&original.surface_mesh(), ©.surface_mesh()); + EXPECT_NE(&original.surface_mesh_bvh(), ©.surface_mesh_bvh()); + EXPECT_NE(&original.tri_to_tet(), ©.tri_to_tet()); + EXPECT_NE(&original.mesh_topology(), ©.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,12 +116,22 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) { const VolumeMeshFieldLinear* const pressure_ptr = &start.pressure(); const Bvh>* const bvh_ptr = &start.bvh(); + const TriangleSurfaceMesh* const surface_mesh_ptr = + &start.surface_mesh(); + const Bvh>* const surface_mesh_bvh_ptr = + &start.surface_mesh_bvh(); + const std::vector* const tri_to_tetptr = &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_tetptr); + EXPECT_EQ(&move_constructed.mesh_topology(), mesh_topology_ptr); // Test move-assignment operator. SoftMesh move_assigned; @@ -111,6 +139,10 @@ GTEST_TEST(SoftMeshTest, TestCopyMoveAssignConstruct) { 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_tetptr); + 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(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); diff --git a/geometry/proximity/volume_mesh_topology.cc b/geometry/proximity/volume_mesh_topology.cc index 23446ba33f7a..abef773257c8 100644 --- a/geometry/proximity/volume_mesh_topology.cc +++ b/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 diff --git a/geometry/proximity/volume_mesh_topology.h b/geometry/proximity/volume_mesh_topology.h index 349c69a60bbd..6adc6cfce7ec 100644 --- a/geometry/proximity/volume_mesh_topology.h +++ b/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& 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> tetrahedra_neighbors_; }; diff --git a/geometry/proximity/volume_to_surface_mesh.h b/geometry/proximity/volume_to_surface_mesh.h index 2080305b0c4b..70cc89b7a885 100644 --- a/geometry/proximity/volume_to_surface_mesh.h +++ b/geometry/proximity/volume_to_surface_mesh.h @@ -27,6 +27,8 @@ struct TetFace { | 2 | 1, 3, 0 | | 3 | 2, 1, 0 | */ int face_index{}; + + auto operator<=>(const TetFace&) const = default; }; /* Identifies the triangular boundary faces of a tetrahedral volume mesh.