From aef3590e7f36f1a780ff5ee595b9a4a860cf57e9 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Mon, 28 Sep 2020 15:11:28 -0700 Subject: [PATCH] [geometry] Remove the non-BVH interface for rigid-soft mesh intersection (#14110) For historical reasons, computing intersection between a soft volume mesh and a rigid surface mesh had two separate APIs. One used broadphase acceleration with a bounding volume hierarchy (BVH) one did not. Both APIs were propagated as a basis for measuring improvement. The need for both APIs is long gone. So, we'll simplify the API and eliminate the slow version. Going forward, improvements should be against strictly the best possible results. This change had several implications: - mesh_intersection_benchmark now only considers one case -- it reports the performance of queries with the *current* BVH implementation. - Unit tests in mesh_intersection_test would exploit the BVH-free API to shorten the tests. They needed to be expanded to use the BVH API. - One test in particular would produce a smoke test confirming that the test could be used with AutoDiffXd-valued meshes. That used the old BVH-free API. In removing that API, we need to be able to build a BVH for an AutoDiffXd-valued mesh. - Bvh and obb (and tests) have been updated to allow constructing a BVH for an AutoDiffXd-valued mesh. - Incidentally, cleaned up names of meshes to match the quantity_F notation. --- geometry/benchmarking/README.md | 8 +- .../mesh_intersection_benchmark.cc | 144 +++---------- geometry/proximity/bvh.cc | 14 +- geometry/proximity/bvh.h | 18 +- geometry/proximity/mesh_intersection.cc | 185 +--------------- geometry/proximity/mesh_intersection.h | 54 +++-- geometry/proximity/obb.cc | 13 +- geometry/proximity/obb.h | 3 +- geometry/proximity/test/bvh_test.cc | 13 ++ .../proximity/test/mesh_intersection_test.cc | 201 ++++-------------- geometry/proximity/test/mesh_to_vtk_test.cc | 15 +- 11 files changed, 174 insertions(+), 494 deletions(-) diff --git a/geometry/benchmarking/README.md b/geometry/benchmarking/README.md index fb231653d2de..2dad818cbabf 100644 --- a/geometry/benchmarking/README.md +++ b/geometry/benchmarking/README.md @@ -33,7 +33,7 @@ designed so users can assess the relative cost of the renderers on their own hardware configuration, aiding in design decisions for understanding the cost of renderer choice. * [mesh_intersection_benchmark.cc](./mesh_intersection_benchmark.cc): -Benchmark program to compare different mesh intersection optimizations with -varying mesh attributes and overlaps. It is targeted toward developers during -the process of optimizing the performance of hydroelastic contact and may be -removed once sufficient work has been done in that effort. +Benchmark program to evaluate bounding volume hierarchy impact on mesh-mesh +intersections across varying mesh attributes and overlaps. It is targeted toward +developers during the process of optimizing the performance of hydroelastic +contact and may be removed once sufficient work has been done in that effort. diff --git a/geometry/benchmarking/mesh_intersection_benchmark.cc b/geometry/benchmarking/mesh_intersection_benchmark.cc index bef062c778ad..e80a717d2b57 100644 --- a/geometry/benchmarking/mesh_intersection_benchmark.cc +++ b/geometry/benchmarking/mesh_intersection_benchmark.cc @@ -14,12 +14,11 @@ namespace internal { /* @defgroup mesh_intersection_benchmarks Mesh Intersection Benchmarks @ingroup proximity_queries - The benchmark compares mesh intersection techniques between soft and rigid - meshes. + The benchmark evaluates mesh intersection between soft and rigid meshes. It computes the contact surface formed from the intersection of an ellipsoid - and a sphere both using and not using broad-phase culling (via a bounding - volume hierarchy). Arguments include: + and a sphere using broad-phase culling (via a bounding volume hierarchy). + Arguments include: - __resolution__: An enumeration in the integer range from 0 to 3 that guides the level of mesh refinement, where 0 produces the coarsest meshes and 3 produces the finest meshes. @@ -64,59 +63,33 @@ Load Average: 21.52, 41.04, 26.99 ---------------------------------------------------------------------------------------------------- // NOLINT(*) Benchmark Time CPU Iterations Line Number // NOLINT(*) ---------------------------------------------------------------------------------------------------- // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/0/4/0/min_time:2.000 0.903 ms 0.903 ms 3297 [1] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/1/4/0/min_time:2.000 5.74 ms 5.74 ms 456 [2] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/4/0/min_time:2.000 19.1 ms 19.1 ms 129 [3] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/3/4/0/min_time:2.000 530 ms 530 ms 5 [4] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/0/0/min_time:2.000 15.1 ms 15.1 ms 189 [5] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/1/0/min_time:2.000 15.4 ms 15.4 ms 178 [6] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/2/0/min_time:2.000 15.9 ms 15.9 ms 176 [7] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/3/0/min_time:2.000 18.3 ms 18.3 ms 150 [8] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/4/1/min_time:2.000 19.1 ms 19.1 ms 147 [9] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/4/2/min_time:2.000 19.0 ms 19.0 ms 147 [10] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/4/3/min_time:2.000 19.2 ms 19.2 ms 145 [11] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/3/1/min_time:2.000 17.9 ms 17.9 ms 156 [12] // NOLINT(*) -MeshIntersectionBenchmark/WithoutBVH/2/2/2/min_time:2.000 15.6 ms 15.6 ms 180 [13] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/0/4/0/min_time:2.000 0.629 ms 0.629 ms 4408 [14] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/1/4/0/min_time:2.000 2.15 ms 2.15 ms 1328 [15] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/4/0/min_time:2.000 3.14 ms 3.14 ms 893 [16] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/3/4/0/min_time:2.000 14.3 ms 14.3 ms 192 [17] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/0/0/min_time:2.000 0.000 ms 0.000 ms 54932081 [18] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/1/0/min_time:2.000 0.023 ms 0.023 ms 119450 [19] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/2/0/min_time:2.000 0.119 ms 0.119 ms 23142 [20] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/3/0/min_time:2.000 1.87 ms 1.87 ms 1486 [21] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/4/1/min_time:2.000 2.95 ms 2.95 ms 944 [22] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/4/2/min_time:2.000 3.15 ms 3.15 ms 894 [23] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/4/3/min_time:2.000 3.21 ms 3.21 ms 871 [24] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/3/1/min_time:2.000 1.94 ms 1.94 ms 1448 [25] // NOLINT(*) -MeshIntersectionBenchmark/___WithBVH/2/2/2/min_time:2.000 0.127 ms 0.127 ms 22007 [26] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/0/4/0/min_time:2.000 0.629 ms 0.629 ms 4408 [14] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/1/4/0/min_time:2.000 2.15 ms 2.15 ms 1328 [15] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/4/0/min_time:2.000 3.14 ms 3.14 ms 893 [16] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/3/4/0/min_time:2.000 14.3 ms 14.3 ms 192 [17] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/0/0/min_time:2.000 0.000 ms 0.000 ms 54932081 [18] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/1/0/min_time:2.000 0.023 ms 0.023 ms 119450 [19] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/2/0/min_time:2.000 0.119 ms 0.119 ms 23142 [20] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/3/0/min_time:2.000 1.87 ms 1.87 ms 1486 [21] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/4/1/min_time:2.000 2.95 ms 2.95 ms 944 [22] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/4/2/min_time:2.000 3.15 ms 3.15 ms 894 [23] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/4/3/min_time:2.000 3.21 ms 3.21 ms 871 [24] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/3/1/min_time:2.000 1.94 ms 1.94 ms 1448 [25] // NOLINT(*) +MeshIntersectionBenchmark/RigidSoftMesh/2/2/2/min_time:2.000 0.127 ms 0.127 ms 22007 [26] // NOLINT(*) Resulting contact surface sizes: - - WithoutBVH/0/4/0: 93.76 m^2, 448 triangles - - WithoutBVH/1/4/0: 93.76 m^2, 1592 triangles - - WithoutBVH/2/4/0: 107.59 m^2, 2848 triangles - - WithoutBVH/3/4/0: 111.67 m^2, 14336 triangles - - WithoutBVH/2/0/0: 0.00 m^2, 0 triangles - - WithoutBVH/2/1/0: 0.00 m^2, 0 triangles - - WithoutBVH/2/2/0: 1.50 m^2, 58 triangles - - WithoutBVH/2/3/0: 48.21 m^2, 2226 triangles - - WithoutBVH/2/4/1: 106.41 m^2, 3804 triangles - - WithoutBVH/2/4/2: 106.89 m^2, 3848 triangles - - WithoutBVH/2/4/3: 107.44 m^2, 3808 triangles - - WithoutBVH/2/3/1: 48.26 m^2, 2190 triangles - - WithoutBVH/2/2/2: 1.52 m^2, 62 triangles - - ___WithBVH/0/4/0: 93.76 m^2, 448 triangles - - ___WithBVH/1/4/0: 93.76 m^2, 1592 triangles - - ___WithBVH/2/4/0: 107.59 m^2, 2848 triangles - - ___WithBVH/3/4/0: 111.67 m^2, 14336 triangles - - ___WithBVH/2/0/0: 0.00 m^2, 0 triangles - - ___WithBVH/2/1/0: 0.00 m^2, 0 triangles - - ___WithBVH/2/2/0: 1.50 m^2, 58 triangles - - ___WithBVH/2/3/0: 48.21 m^2, 2226 triangles - - ___WithBVH/2/4/1: 106.41 m^2, 3804 triangles - - ___WithBVH/2/4/2: 106.89 m^2, 3848 triangles - - ___WithBVH/2/4/3: 107.44 m^2, 3808 triangles - - ___WithBVH/2/3/1: 48.26 m^2, 2190 triangles - - ___WithBVH/2/2/2: 1.52 m^2, 62 triangles + - RigidSoftMesh/0/4/0: 93.76 m^2, 448 triangles + - RigidSoftMesh/1/4/0: 93.76 m^2, 1592 triangles + - RigidSoftMesh/2/4/0: 107.59 m^2, 2848 triangles + - RigidSoftMesh/3/4/0: 111.67 m^2, 14336 triangles + - RigidSoftMesh/2/0/0: 0.00 m^2, 0 triangles + - RigidSoftMesh/2/1/0: 0.00 m^2, 0 triangles + - RigidSoftMesh/2/2/0: 1.50 m^2, 58 triangles + - RigidSoftMesh/2/3/0: 48.21 m^2, 2226 triangles + - RigidSoftMesh/2/4/1: 106.41 m^2, 3804 triangles + - RigidSoftMesh/2/4/2: 106.89 m^2, 3848 triangles + - RigidSoftMesh/2/4/3: 107.44 m^2, 3808 triangles + - RigidSoftMesh/2/3/1: 48.26 m^2, 2190 triangles + - RigidSoftMesh/2/2/2: 1.52 m^2, 62 triangles ``` @@ -129,9 +102,7 @@ Resulting contact surface sizes: MeshIntersectionBenchmark/TestName/resolution/contact_overlap/rotation_factor/min_time ``` - - __TestName__: One of - - __WithoutBVH__: Computes the intersection without broad-phase culling. - - __WithBVH__: Computes the intersection with broad-phase culling. + - __TestName__: RigidSoftMesh - __resolution__: Affects the resolution of the ellipsoid and sphere meshes. Valid values must be one of [0, 1, 2, 3], where 0 produces the coarsest meshes and 3 produces the finest meshes. This is converted behind @@ -163,22 +134,6 @@ Resulting contact surface sizes: compute the intersection. The `Iterations` indicates how often the action was performed to compute the average value. For more information see the [google benchmark documentation](https://github.com/google/benchmark). - - Now we can analyze the example output and draw some example inferences (not a - complete set of valid inferences): - - - The BVH is increasingly effective as mesh resolution increases. - - For a resolution parameter of 2, broad-phase culling improves performance - by a factor of ~6X (see [3] vs [16]), while at a resolution parameter of - 3 (a finer mesh), it improves by a factor of ~37X (see [4] vs [17]). - - As expected, the BVH is increasingly effective as contact overlap is - reduced. - - At a contact overlap parameter of 4, i.e. a maximal contact surface, - broad-phase culling improves performance by a factor of ~6X (see [3] vs - [16]), while at a contact overlap parameter of 2, i.e. a minimal contact - surface, it improves performance by a factor of ~133X (see [7] vs [20]). - - The offset in axis alignment has an apparently negligible impact on - performance. */ using Eigen::AngleAxis; @@ -272,40 +227,7 @@ std::set MeshIntersectionBenchmark::contact_surface_result_keys; std::vector MeshIntersectionBenchmark::contact_surface_result_output; -BENCHMARK_DEFINE_F(MeshIntersectionBenchmark, WithoutBVH) -// NOLINTNEXTLINE(runtime/references) -(benchmark::State& state) { - SetupMeshes(state); - std::unique_ptr> surface_SR; - std::unique_ptr> e_SR; - std::vector> grad_eM_Ms; - for (auto _ : state) { - SurfaceVolumeIntersector().SampleVolumeFieldOnSurface( - field_S_, mesh_R_, X_SR_, &surface_SR, &e_SR, &grad_eM_Ms); - } - RecordContactSurfaceResult(surface_SR.get(), "WithoutBVH", state); -} -BENCHMARK_REGISTER_F(MeshIntersectionBenchmark, WithoutBVH) - ->Unit(benchmark::kMillisecond) - ->MinTime(2) - ->Args({0, 4, 0}) // 0 resolution, 4 contact overlap, 0 rotation factor. - ->Args({1, 4, 0}) // 1 resolution, 4 contact overlap, 0 rotation factor. - ->Args({2, 4, 0}) // 2 resolution, 4 contact overlap, 0 rotation factor. - ->Args({3, 4, 0}) // 3 resolution, 4 contact overlap, 0 rotation factor. - ->Args({2, 0, 0}) // 2 resolution, 0 contact overlap, 0 rotation factor. - ->Args({2, 1, 0}) // 2 resolution, 1 contact overlap, 0 rotation factor. - ->Args({2, 2, 0}) // 2 resolution, 2 contact overlap, 0 rotation factor. - ->Args({2, 3, 0}) // 2 resolution, 3 contact overlap, 0 rotation factor. - ->Args({2, 4, 1}) // 2 resolution, 4 contact overlap, 1 rotation factor. - ->Args({2, 4, 2}) // 2 resolution, 4 contact overlap, 2 rotation factor. - ->Args({2, 4, 3}) // 2 resolution, 4 contact overlap, 3 rotation factor. - ->Args({2, 3, 1}) // 2 resolution, 3 contact overlap, 1 rotation factor. - ->Args({2, 2, 2}); // 2 resolution, 2 contact overlap, 2 rotation factor. - -// This test name is prefixed with underscores so that the total number of -// characters matches that of the first test, "WithoutBVH". This causes the -// final output to line up and makes it much easier to read. -BENCHMARK_DEFINE_F(MeshIntersectionBenchmark, ___WithBVH) +BENCHMARK_DEFINE_F(MeshIntersectionBenchmark, RigidSoftMesh) // NOLINTNEXTLINE(runtime/references) (benchmark::State& state) { SetupMeshes(state); @@ -319,9 +241,9 @@ BENCHMARK_DEFINE_F(MeshIntersectionBenchmark, ___WithBVH) field_S_, bvh_S, mesh_R_, bvh_R, X_SR_, &surface_SR, &e_SR, &grad_eM_Ms); } - RecordContactSurfaceResult(surface_SR.get(), "___WithBVH", state); + RecordContactSurfaceResult(surface_SR.get(), "RigidSoftMesh", state); } -BENCHMARK_REGISTER_F(MeshIntersectionBenchmark, ___WithBVH) +BENCHMARK_REGISTER_F(MeshIntersectionBenchmark, RigidSoftMesh) ->Unit(benchmark::kMillisecond) ->MinTime(2) ->Args({0, 4, 0}) // 0 resolution, 4 contact overlap, 0 rotation factor. diff --git a/geometry/proximity/bvh.cc b/geometry/proximity/bvh.cc index cb1aa47da2b4..4b265baeafcf 100644 --- a/geometry/proximity/bvh.cc +++ b/geometry/proximity/bvh.cc @@ -5,6 +5,8 @@ #include #include +#include "drake/geometry/utilities.h" + namespace drake { namespace geometry { namespace internal { @@ -114,7 +116,8 @@ Vector3d Bvh::ComputeCentroid(const MeshType& mesh, const auto& element = mesh.element(i); // Calculate average from all vertices. for (int v = 0; v < kElementVertexCount; ++v) { - const auto& vertex = mesh.vertex(element.vertex(v)).r_MV(); + const Vector3d& vertex = + convert_to_double(mesh.vertex(element.vertex(v)).r_MV()); centroid += vertex; } centroid /= kElementVertexCount; @@ -149,3 +152,12 @@ template class drake::geometry::internal::Bvh< drake::geometry::SurfaceMesh>; template class drake::geometry::internal::Bvh< drake::geometry::VolumeMesh>; + +// TODO(SeanCurtis-tri) These are here to allow creating a BVH for an +// AutoDiffXd-valued mesh. Currently, the code doesn't strictly disallow this +// although projected uses are only double-valued. If we choose to definitively +// close the door on autodiff-valued meshes, we can remove these. +template class drake::geometry::internal::Bvh< + drake::geometry::SurfaceMesh>; +template class drake::geometry::internal::Bvh< + drake::geometry::VolumeMesh>; diff --git a/geometry/proximity/bvh.h b/geometry/proximity/bvh.h index 79218f2a837c..c4a1c84fc45d 100644 --- a/geometry/proximity/bvh.h +++ b/geometry/proximity/bvh.h @@ -23,13 +23,13 @@ namespace internal { template struct MeshTraits; -template <> -struct MeshTraits> { +template +struct MeshTraits> { static constexpr int kMaxElementPerBvhLeaf = 3; }; -template <> -struct MeshTraits> { +template +struct MeshTraits> { static constexpr int kMaxElementPerBvhLeaf = 1; }; @@ -350,3 +350,13 @@ extern template class drake::geometry::internal::Bvh< drake::geometry::SurfaceMesh>; extern template class drake::geometry::internal::Bvh< drake::geometry::VolumeMesh>; + +// TODO(SeanCurtis-TRI): Remove support for building a Bvh on an AutoDiff-valued +// mesh after we've cleaned up the scalar types in hydroelastics. Specifically, +// this is here to support the unit tests in mesh_intersection_test.cc. Also +// the calls to convert_to_double should be removed. +// See issue #14136. +extern template class drake::geometry::internal::Bvh< + drake::geometry::SurfaceMesh>; +extern template class drake::geometry::internal::Bvh< + drake::geometry::VolumeMesh>; diff --git a/geometry/proximity/mesh_intersection.cc b/geometry/proximity/mesh_intersection.cc index 27b6f71c9cb8..bb37ecec2406 100644 --- a/geometry/proximity/mesh_intersection.cc +++ b/geometry/proximity/mesh_intersection.cc @@ -278,104 +278,6 @@ bool SurfaceVolumeIntersector::IsFaceNormalAlongPressureGradient( tri_index, X_MN.rotation()); } -template -void SurfaceVolumeIntersector::SampleVolumeFieldOnSurface( - const VolumeMeshField& volume_field_M, - const SurfaceMesh& surface_N, - const math::RigidTransform& X_MN, - std::unique_ptr>* surface_MN_M, - std::unique_ptr>* e_MN, - std::vector>* grad_eM_Ms) { - DRAKE_DEMAND(surface_MN_M); - DRAKE_DEMAND(e_MN); - DRAKE_DEMAND(grad_eM_Ms); - grad_eM_Ms->clear(); - - // In order to get the gradient, we need the concrete linear field underneath. - const VolumeMeshFieldLinear& volume_linear_field_M = - dynamic_cast&>(volume_field_M); - std::vector surface_faces; - std::vector> surface_vertices_M; - std::vector surface_e; - const auto& mesh_M = volume_field_M.mesh(); - // We know that each contact polygon has at most 7 vertices because - // each surface triangle is clipped by four half-spaces of the four - // triangular faces of a tetrahedron. - std::vector contact_polygon; - contact_polygon.reserve(7); - - for (VolumeElementIndex tet_index(0); tet_index < mesh_M.num_elements(); - ++tet_index) { - for (SurfaceFaceIndex tri_index(0); tri_index < surface_N.num_faces(); - ++tri_index) { - if (!IsFaceNormalAlongPressureGradient(volume_field_M, surface_N, X_MN, - tet_index, tri_index)) { - continue; - } - - // TODO(SeanCurtis-TRI): This redundantly transforms surface mesh vertex - // positions. Specifically, each vertex will be transformed M times (once - // per tetrahedron. Even with broadphase culling, this vertex will get - // transformed once for each tet-tri pair where the tri is incidental - // to the vertex and the tet-tri pair can't be conservatively culled. - // This is O(mn), where m is the number of faces incident to the vertex - // and n is the number of tet BVs that overlap this triangle BV. However, - // if the broadphase culling determines the surface and volume are - // disjoint regions, *no* vertices will be transformed. Unclear what the - // best balance for best average performance. - const std::vector>& polygon_vertices_M = - ClipTriangleByTetrahedron(tet_index, mesh_M, tri_index, surface_N, - X_MN); - - const int poly_vertex_count = - static_cast(polygon_vertices_M.size()); - if (poly_vertex_count < 3) continue; - - const int num_previous_vertices = surface_vertices_M.size(); - // Add the new polygon vertices to the mesh vertices and construct a - // polygon from the vertex indices. - contact_polygon.clear(); - for (int i = 0; i < poly_vertex_count; ++i) { - contact_polygon.emplace_back(surface_vertices_M.size()); - surface_vertices_M.emplace_back(polygon_vertices_M[i]); - } - const Vector3& nhat_M = - X_MN.rotation() * surface_N.face_normal(tri_index); - - size_t old_count = surface_faces.size(); - AddPolygonToMeshData(contact_polygon, nhat_M, &surface_faces, - &surface_vertices_M); - // TODO(SeanCurtis-TRI) Consider rolling this operation into - // AddPolygonToMeshData to eliminate the extra pass through triangles. - const Vector3& grad_eMi_M = - volume_linear_field_M.EvaluateGradient(tet_index); - for (size_t i = old_count; i < surface_faces.size(); ++i) { - grad_eM_Ms->push_back(grad_eMi_M); - } - - const int num_current_vertices = surface_vertices_M.size(); - // Calculate values of the pressure field and the normal field at the - // new vertices. - for (int v = num_previous_vertices; v < num_current_vertices; ++v) { - const Vector3& r_MV = surface_vertices_M[v].r_MV(); - const T pressure = volume_field_M.EvaluateCartesian(tet_index, r_MV); - surface_e.push_back(pressure); - } - } - } - - DRAKE_DEMAND(surface_vertices_M.size() == surface_e.size()); - if (surface_faces.empty()) return; - - *surface_MN_M = std::make_unique>( - std::move(surface_faces), std::move(surface_vertices_M)); - const bool calculate_gradient = false; - *e_MN = std::make_unique>( - "e", std::move(surface_e), surface_MN_M->get(), calculate_gradient); -} - -/* A variant of SampleVolumeFieldOnSurface but with broad-phase culling to - reduce the number of element-pairs evaluated. */ template void SurfaceVolumeIntersector::SampleVolumeFieldOnSurface( const VolumeMeshField& volume_field_M, @@ -463,7 +365,7 @@ void SurfaceVolumeIntersector::SampleVolumeFieldOnSurface( } return BvttCallbackResult::Continue; }; - bvh_M.Collide(bvh_N, X_MN, callback); + bvh_M.Collide(bvh_N, convert_to_double(X_MN), callback); DRAKE_DEMAND(surface_vertices_M.size() == surface_e.size()); if (surface_faces.empty()) return; @@ -475,65 +377,6 @@ void SurfaceVolumeIntersector::SampleVolumeFieldOnSurface( "e", std::move(surface_e), surface_MN_M->get(), calculate_gradient); } -template -std::unique_ptr> -ComputeContactSurfaceFromSoftVolumeRigidSurface( - const GeometryId id_S, const VolumeMeshField& field_S, - const math::RigidTransform& X_WS, const GeometryId id_R, - const SurfaceMesh& mesh_R, const math::RigidTransform& X_WR) { - // TODO(SeanCurtis-TRI): This function is insufficiently templated. - // Generally, - // there are three types of scalars: the pose scalar, the mesh field - // *value* scalar, and the mesh vertex-position scalar. However, short - // term, it is probably unnecessary to distinguish all three here. If we - // assume that this function is *only* called to find the contact surface - // between two declared geometries, then the volume mesh values and vertex - // positions will always be in double. However, even dividing between pose - // and mesh scalars has *huge* downstream implications on how the meshes - // and mesh fields are defined. We're deferring this work by simply - // disallowing invocation of this method on AutoDiffXd (see below). It - // compiles, but throws. - - // Compute the transformation from the rigid frame to the soft frame. - const math::RigidTransform X_SR = X_WS.inverse() * X_WR; - - // The mesh will be computed in Frame S and then transformed to the world - // frame. - std::unique_ptr> surface_SR; - std::unique_ptr> e_SR; - std::vector> grad_eS_S; - - SurfaceVolumeIntersector().SampleVolumeFieldOnSurface( - field_S, mesh_R, X_SR, &surface_SR, &e_SR, &grad_eS_S); - - if (surface_SR == nullptr) return nullptr; - - // TODO(DamrongGuoy): Compute the mesh and field with the quantities - // expressed in World frame by construction so that we can delete these two - // transforming methods. - - // Transform the mesh from the S frame to the world frame. This entails: - // 1. Transforming the surface's vertices. - // 2. Allowing the LinearField e_SR a chance to re-express its cached - // gradient values. - // 3. Re-expressing the gradients of the soft-mesh's pressure field - // (grad_eS_S) in the world frame (grad_eS_W). - surface_SR->TransformVertices(X_WS); - e_SR->TransformGradients(X_WS); - std::unique_ptr>> grad_eS_W; - grad_eS_W = std::make_unique>>(); - grad_eS_W->reserve(grad_eS_S.size()); - for (const auto& grad_eSi_S : grad_eS_S) { - grad_eS_W->emplace_back(X_WS.rotation() * grad_eSi_S); - } - - return std::make_unique>(id_S, id_R, std::move(surface_SR), - std::move(e_SR), - std::move(grad_eS_W), nullptr); -} - -/* A variant of ComputeContactSurfaceFromSoftVolumeRigidSurface but with - broad-phase culling to reduce the number of element-pairs evaluated. */ template std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( @@ -551,6 +394,7 @@ ComputeContactSurfaceFromSoftVolumeRigidSurface( // *huge* downstream implications on how the meshes and mesh fields are // defined. We're deferring this work by simply disallowing invocation of // this method on AutoDiffXd (see below). It compiles, but throws. + // See issue #14136. // Compute the transformation from the rigid frame to the soft frame. const math::RigidTransform X_SR = X_WS.inverse() * X_WR; @@ -595,18 +439,7 @@ ComputeContactSurfaceFromSoftVolumeRigidSurface( } template class SurfaceVolumeIntersector; -// This template instantiation: -// template class SurfaceVolumeIntersector; -// triggers compile error because: -// Bvh>& bvh_M -// does not support VolumeMesh. - -template std::unique_ptr> -ComputeContactSurfaceFromSoftVolumeRigidSurface( - const GeometryId id_S, const VolumeMeshField& field_S, - const math::RigidTransform& X_WS, const GeometryId id_R, - const SurfaceMesh& mesh_R, - const math::RigidTransform& X_WR); +template class SurfaceVolumeIntersector; template std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( @@ -620,8 +453,10 @@ template std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( const GeometryId id_S, const VolumeMeshField& field_S, + const Bvh>& bvh_S, const math::RigidTransform& X_WS, const GeometryId id_R, const SurfaceMesh& mesh_R, + const Bvh>& bvh_R, const math::RigidTransform& X_WR); // NOTE: This is a short-term hack to allow ProximityEngine to compile when @@ -629,16 +464,6 @@ ComputeContactSurfaceFromSoftVolumeRigidSurface( // doing contact surface computation with AutoDiffXd. This curtails those // issues for now by short-circuiting the functionality. (See the note on the // templated version of this function.) -std::unique_ptr> -ComputeContactSurfaceFromSoftVolumeRigidSurface( - const GeometryId, const VolumeMeshField&, - const math::RigidTransform&, const GeometryId, - const SurfaceMesh&, const math::RigidTransform&) { - throw std::logic_error( - "AutoDiff-valued ContactSurface calculation between meshes is not" - "currently supported"); -} - std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( const GeometryId, const VolumeMeshField&, diff --git a/geometry/proximity/mesh_intersection.h b/geometry/proximity/mesh_intersection.h index d4c2f1c94a30..ec3bdb793da6 100644 --- a/geometry/proximity/mesh_intersection.h +++ b/geometry/proximity/mesh_intersection.h @@ -54,9 +54,14 @@ class SurfaceVolumeIntersector { defines its domain. The vertex positions of the mesh are measured and expressed in frame M. And the field can be evaluated at positions likewise measured and expressed in frame M. + @param[in] bvh_M + A bounding volume hierarchy built on the geometry contained in + `volume_field_M`. @param[in] surface_N The surface mesh intersected with the volume mesh to define the sample domain. Its vertex positions are measured and expressed in frame N. + @param[in] bvh_N + A bounding volume hierarchy built on the geometry `surface_N`. @param[in] X_MN The pose of frame N in frame M. @param[out] surface_MN_M @@ -73,15 +78,6 @@ class SurfaceVolumeIntersector { @note The output surface mesh may have duplicate vertices. */ - void SampleVolumeFieldOnSurface( - const VolumeMeshField& volume_field_M, - const SurfaceMesh& surface_N, const math::RigidTransform& X_MN, - std::unique_ptr>* surface_MN_M, - std::unique_ptr>* e_MN, - std::vector>* grad_eM_Ms); - - /* A variant of SampleVolumeFieldOnSurface but with broad-phase culling to - reduce the number of element-pairs evaluated. */ void SampleVolumeFieldOnSurface( const VolumeMeshField& volume_field_M, const Bvh>& bvh_M, const SurfaceMesh& surface_N, @@ -316,9 +312,11 @@ class SurfaceVolumeIntersector { @param[in] field_S A scalar field defined on the soft volume mesh S. Mesh S's vertices are defined in S's frame. The scalar field is likewise defined in frame S - (that is, it can only be evaluated on points which have been measured and + (that is, it can only be evaluated on points which have been measured and expressed in frame S). For hydroelastic contact, the scalar field is a "pressure" field. + @param[in] bvh_S + A bounding volume hierarchy built on the geometry contained in `field_S`. @param[in] X_WS The pose of the rigid frame S in the world frame W. @param[in] id_R @@ -327,15 +325,17 @@ class SurfaceVolumeIntersector { The rigid geometry R is represented as a surface mesh, whose vertex positions are in R's frame. We assume that triangles are oriented outward. + @param[in] bvh_R + A bounding volume hierarchy built on the geometry contained in `mesh_R`. @param[in] X_WR The pose of the rigid frame R in the world frame W. @return The contact surface between M and N. Geometries S and R map to M and N - with a consistent mapping (as documented in ContactSurface) but without any + with a consistent mapping (as documented in ContactSurface) but without any guarantee as to what that mapping is. Positions of vertex coordinates are expressed in the world frame. The pressure distribution comes from the - soft geometry S. The normal vector field, expressed in the world frame frame, - comes from the rigid geometry R. + soft geometry S. The normal vector field, expressed in the world frame + frame, comes from the rigid geometry R. ooo soft S o o @@ -353,32 +353,30 @@ class SurfaceVolumeIntersector { */ template std::unique_ptr> -ComputeContactSurfaceFromSoftVolumeRigidSurface( - const GeometryId id_S, const VolumeMeshField& field_S, - const math::RigidTransform& X_WS, const GeometryId id_R, - const SurfaceMesh& mesh_R, const math::RigidTransform& X_WR); - -/* A variant of ComputeContactSurfaceFromSoftVolumeRigidSurface but with - broad-phase culling to reduce the number of element-pairs evaluated. */ -template -std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( const GeometryId id_S, const VolumeMeshField& field_S, const Bvh>& bvh_S, const math::RigidTransform& X_WS, const GeometryId id_R, const SurfaceMesh& mesh_R, const Bvh>& bvh_R, const math::RigidTransform& X_WR); +// TODO(SeanCurtis-TRI): ComputeContactSurfaceFromSoftVolumeRigidSurface is +// incorrectly templated. We currently have no reasonable expectation that the +// meshes provided as inputs will be AutoDiffXd-valued. Currently, all meshes +// we construct are strictly double-valued. So, the fact that the meshes are +// described as T-valued is probably inappropriate and needs to have careful +// reasoning applied. +// +// One of the proofs of the incorrectness of the templating is the declaration +// given below. ProximityEngine calls this method with double-valued meshes +// and AutoDiffXd-valued transforms. Thus, the function below is *not* a +// specialization of the templated function but an overload. +// See issue #14136. + // NOTE: This is a short-term hack to allow ProximityEngine to compile when // invoking this method. There are currently a host of issues preventing us from // doing contact surface computation with AutoDiffXd. This curtails those // issues for now by short-circuiting the functionality. (See the note on the // templated version of this function.) -std::unique_ptr> -ComputeContactSurfaceFromSoftVolumeRigidSurface( - const GeometryId, const VolumeMeshField&, - const math::RigidTransform&, const GeometryId, - const SurfaceMesh&, const math::RigidTransform&); - std::unique_ptr> ComputeContactSurfaceFromSoftVolumeRigidSurface( const GeometryId, const VolumeMeshField&, diff --git a/geometry/proximity/obb.cc b/geometry/proximity/obb.cc index 8d1d3095da05..7840f61c45ae 100644 --- a/geometry/proximity/obb.cc +++ b/geometry/proximity/obb.cc @@ -203,13 +203,13 @@ RotationMatrixd ObbMaker::CalcOrientationByPca() const { // C is for centroid. Vector3d p_MC = Vector3d::Zero(); for (typename MeshType::VertexIndex v : vertices_) { - p_MC += mesh_M_.vertex(v).r_MV(); + p_MC += convert_to_double(mesh_M_.vertex(v).r_MV()); } p_MC *= one_over_n; Matrix3d covariance_M = Matrix3d::Zero(); for (typename MeshType::VertexIndex v : vertices_) { - const Vector3d& p_MV = mesh_M_.vertex(v).r_MV(); + const Vector3d& p_MV = convert_to_double(mesh_M_.vertex(v).r_MV()); const Vector3d p_CV_M = p_MV - p_MC; // covariance_M is a symmetric matrix because it's a sum of the // 3x3 symmetric matrices V*Váµ€ of column vectors V. @@ -303,7 +303,7 @@ Obb ObbMaker::CalcOrientedBox(const RotationMatrixd& R_MB) const { for (typename MeshType::VertexIndex v : vertices_) { // Since frame F is a rotation of frame M with the same origin, we can use // the rotation R_FM for the transform X_FM. - const Vector3d p_FV = R_FM * mesh_M_.vertex(v).r_MV(); + const Vector3d p_FV = R_FM * convert_to_double(mesh_M_.vertex(v).r_MV()); p_FL = p_FL.cwiseMin(p_FV); p_FU = p_FU.cwiseMax(p_FV); } @@ -411,6 +411,13 @@ Obb ObbMaker::Compute() const { template class ObbMaker>; template class ObbMaker>; +// TODO(SeanCurtis-TRI): Remove support for building a Bvh on an AutoDiff-valued +// mesh after we've cleaned up the scalar types in hydroelastics. Specifically, +// this is here to support the unit tests in mesh_intersection_test.cc. Also +// the calls to convert_to_double should be removed. +template class ObbMaker>; +template class ObbMaker>; + } // namespace internal } // namespace geometry } // namespace drake diff --git a/geometry/proximity/obb.h b/geometry/proximity/obb.h index 4e0ad4f61e39..ff0f5d4c1cbb 100644 --- a/geometry/proximity/obb.h +++ b/geometry/proximity/obb.h @@ -148,7 +148,8 @@ template class ObbMakerTester; /* %ObbMaker performs an algorithm to create an oriented bounding box that fits a specified set of vertices in a mesh. - @tparam MeshType is either SurfaceMesh or VolumeMesh. */ + @tparam MeshType is either SurfaceMesh or VolumeMesh, where T is + double or AutoDiffXd. */ template class ObbMaker { public: diff --git a/geometry/proximity/test/bvh_test.cc b/geometry/proximity/test/bvh_test.cc index ebbb4a9d0ddd..0ea28b6801cf 100644 --- a/geometry/proximity/test/bvh_test.cc +++ b/geometry/proximity/test/bvh_test.cc @@ -560,6 +560,19 @@ GTEST_TEST(BoundingVolumeHierarchyTest, TestEqual) { EXPECT_TRUE(bvh_ellipsoid.Equal(bvh_ellipsoid)); } +// Simply confirms that an Obb can be built from an autodiff mesh. We apply a +// limited smoke test to indicate success -- the bounding volume of the root +// node is the same as if the mesh were double-valued. +GTEST_TEST(BoundingVolumeHierarchyTest, BvhFromAutodiffmesh) { + SurfaceMesh mesh_ad = + MakeSphereSurfaceMesh(Sphere(1.5), 3); + Bvh> bvh_ad(mesh_ad); + SurfaceMesh mesh_d = + MakeSphereSurfaceMesh(Sphere(1.5), 3); + Bvh> bvh_d(mesh_d); + EXPECT_TRUE(bvh_ad.root_node().bv().Equal(bvh_d.root_node().bv())); +} + } // namespace } // namespace internal } // namespace geometry diff --git a/geometry/proximity/test/mesh_intersection_test.cc b/geometry/proximity/test/mesh_intersection_test.cc index 28f7a314e5a5..2381b24cacfc 100644 --- a/geometry/proximity/test/mesh_intersection_test.cc +++ b/geometry/proximity/test/mesh_intersection_test.cc @@ -791,10 +791,10 @@ VolumeElementIndex GetTetForTriangle(const SurfaceMesh& surface_S, GTEST_TEST(MeshIntersectionTest, SampleVolumeFieldOnSurface) { auto volume_M = TrivialVolumeMesh(); - const Bvh> bvh_M(*volume_M); + const Bvh> bvh_volume_M(*volume_M); auto volume_field_M = TrivialVolumeMeshField(volume_M.get()); auto rigid_N = TrivialSurfaceMesh(); - const Bvh> bvh_N(*rigid_N); + const Bvh> bvh_rigid_N(*rigid_N); // Transform the surface (single triangle) so that it intersects with *both* // tets in the volume mesh. The surface lies on the y = 0.75 plane. // Each tet gets intersected into a isosceles right triangle with a leg @@ -809,8 +809,8 @@ GTEST_TEST(MeshIntersectionTest, SampleVolumeFieldOnSurface) { unique_ptr> surface_M; unique_ptr> e_field; SurfaceVolumeIntersector().SampleVolumeFieldOnSurface( - *volume_field_M, bvh_M, *rigid_N, bvh_N, X_MN, &surface_M, &e_field, - &grad_eM_M); + *volume_field_M, bvh_volume_M, *rigid_N, bvh_rigid_N, X_MN, &surface_M, + &e_field, &grad_eM_M); const double kEps = std::numeric_limits::epsilon(); EXPECT_EQ(6, surface_M->num_faces()); @@ -1007,9 +1007,12 @@ void TestComputeContactSurfaceSoftRigid() { auto id_A = GeometryId::get_new_id(); auto id_B = GeometryId::get_new_id(); EXPECT_LT(id_A, id_B); - auto mesh_S = OctahedronVolume(); - auto field_S = OctahedronPressureField(mesh_S.get()); - auto surface_R = PyramidSurface(); + unique_ptr> mesh_S = OctahedronVolume(); + unique_ptr> field_S = + OctahedronPressureField(mesh_S.get()); + const Bvh> bvh_mesh_S(*mesh_S); + unique_ptr> surface_R = PyramidSurface(); + const Bvh> bvh_surface_R(*surface_R); // Move the rigid pyramid up, so only its square base intersects the top // part of the soft octahedron. const auto X_SR = RigidTransform(Vector3(0, 0, 0.5)); @@ -1026,7 +1029,7 @@ void TestComputeContactSurfaceSoftRigid() { // In this case, we assign id_A to soft and we already know that id_A < id_B. // Confirm order auto contact_SR = ComputeContactSurfaceFromSoftVolumeRigidSurface( - id_A, *field_S, X_WS, id_B, *surface_R, X_WR); + id_A, *field_S, bvh_mesh_S, X_WS, id_B, *surface_R, bvh_surface_R, X_WR); EXPECT_EQ(contact_SR->id_M(), id_A); EXPECT_EQ(contact_SR->id_N(), id_B); EXPECT_TRUE(contact_SR->HasGradE_M()); @@ -1036,7 +1039,7 @@ void TestComputeContactSurfaceSoftRigid() { // is less than id_B, but we should further satisfy various invariants // (listed below). auto contact_RS = ComputeContactSurfaceFromSoftVolumeRigidSurface( - id_B, *field_S, X_WS, id_A, *surface_R, X_WR); + id_B, *field_S, bvh_mesh_S, X_WS, id_A, *surface_R, bvh_surface_R, X_WR); EXPECT_EQ(contact_RS->id_M(), id_A); EXPECT_EQ(contact_RS->id_N(), id_B); EXPECT_FALSE(contact_RS->HasGradE_M()); @@ -1154,11 +1157,14 @@ bool FindElement(Vector3d p_MQ, const VolumeMesh& volume_M, GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) { // Soft octahedron volume S with pressure field. auto s_id = GeometryId::get_new_id(); - auto s_mesh_S = OctahedronVolume(); - auto s_pressure_S = OctahedronPressureField(s_mesh_S.get()); + unique_ptr> volume_S = OctahedronVolume(); + const Bvh> bvh_volume_S(*volume_S); + unique_ptr> pressure_S = + OctahedronPressureField(volume_S.get()); // Rigid pyramid surface R. auto r_id = GeometryId::get_new_id(); - auto r_mesh_R = PyramidSurface(); + unique_ptr> surface_R = PyramidSurface(); + const Bvh> bvh_surface_R(*surface_R); // We use 1e-14 instead of std::numeric_limits::epsilon() to // compensate for the rounding due to general rigid transform. @@ -1177,27 +1183,28 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) { const auto X_WR = X_WS * X_SR; // Contact surface C is expressed in World frame. const auto contact = ComputeContactSurfaceFromSoftVolumeRigidSurface( - s_id, *s_pressure_S, X_WS, r_id, *r_mesh_R, X_WR); + s_id, *pressure_S, bvh_volume_S, X_WS, r_id, *surface_R, bvh_surface_R, + X_WR); // TODO(DamrongGuoy): More comprehensive checks on the mesh of the contact // surface. Here we only check the number of triangles. EXPECT_EQ(12, contact->mesh_W().num_faces()); - // Point Q is at the center vertex of the soft mesh s_mesh_S. Check that - // the contact surface C also has a vertex coincident with Q with the same - // pressure value. + // Point Q is coincident with the center vertex of the soft mesh volume_S. + // The point C on the contact surface is coincident with Q. Check that the + // contact surface reports the same pressure at C as the volume does at Q. { const Vector3d p_SQ = Vector3d::Zero(); const Vector3d p_WQ = X_WS * p_SQ; // Index of contact surface C's vertex coincident with Q. - SurfaceVertexIndex c_vertex; - ASSERT_TRUE(FindVertex(p_WQ, contact->mesh_W(), &c_vertex)); - const double c_pressure = contact->EvaluateE_MN(c_vertex); - // Index of soft octahedron S's vertex coincident with Q. - VolumeVertexIndex s_vertex; - ASSERT_TRUE(FindVertex(p_SQ, s_pressure_S->mesh(), &s_vertex)); - const double s_pressure = s_pressure_S->EvaluateAtVertex(s_vertex); - - EXPECT_NEAR(c_pressure, s_pressure, kEps * s_pressure); + SurfaceVertexIndex index_C; + ASSERT_TRUE(FindVertex(p_WQ, contact->mesh_W(), &index_C)); + const double pressure_at_C = contact->EvaluateE_MN(index_C); + // Index of Q in the volume mesh. + VolumeVertexIndex index_Q; + ASSERT_TRUE(FindVertex(p_SQ, pressure_S->mesh(), &index_Q)); + const double pressure_at_Q = pressure_S->EvaluateAtVertex(index_Q); + + EXPECT_NEAR(pressure_at_C, pressure_at_Q, kEps * pressure_at_Q); } } @@ -1230,7 +1237,8 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) { RigidTransformd(RollPitchYawd(M_PI / 2., 0., 0.), Vector3d{0, -0.5, 0}); const auto X_WR = X_WS * X_SR; auto contact = ComputeContactSurfaceFromSoftVolumeRigidSurface( - s_id, *s_pressure_S, X_WS, r_id, *r_mesh_R, X_WR); + s_id, *pressure_S, bvh_volume_S, X_WS, r_id, *surface_R, bvh_surface_R, + X_WR); // TODO(DamrongGuoy): More comprehensive checks on the mesh of the contact // surface. Here we only check the number of triangles. EXPECT_EQ(12, contact->mesh_W().num_faces()); @@ -1245,8 +1253,8 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) { // Find the tetrahedral element of S containing Q. VolumeElementIndex tetrahedron; VolumeMesh::Barycentric b_Q; - ASSERT_TRUE(FindElement(p_SQ, s_pressure_S->mesh(), &tetrahedron, &b_Q)); - const double s_pressure = s_pressure_S->Evaluate(tetrahedron, b_Q); + ASSERT_TRUE(FindElement(p_SQ, pressure_S->mesh(), &tetrahedron, &b_Q)); + const double s_pressure = pressure_S->Evaluate(tetrahedron, b_Q); EXPECT_NEAR(c_pressure, s_pressure, kEps * s_pressure); } @@ -1258,142 +1266,23 @@ GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidMoving) { // this spelling. It supports compilation but throws a runtime exception. // This confirms the exception. GTEST_TEST(MeshIntersectionTest, DoubleAutoDiffMixed) { - unique_ptr> soft_mesh = OctahedronVolume(); - unique_ptr> soft_field = - OctahedronPressureField(soft_mesh.get()); - unique_ptr> rigid_mesh = PyramidSurface(); + unique_ptr> volume_S = OctahedronVolume(); + const Bvh> bvh_volume_S(*volume_S); + unique_ptr> field_S = + OctahedronPressureField(volume_S.get()); + unique_ptr> surface_R = PyramidSurface(); + const Bvh> bvh_surface_R(*surface_R); DRAKE_EXPECT_THROWS_MESSAGE( ComputeContactSurfaceFromSoftVolumeRigidSurface( - GeometryId::get_new_id(), *soft_field, RigidTransform(), - GeometryId::get_new_id(), *rigid_mesh, RigidTransform()), + GeometryId::get_new_id(), *field_S, bvh_volume_S, + RigidTransform(), GeometryId::get_new_id(), *surface_R, + bvh_surface_R, RigidTransform()), std::logic_error, "AutoDiff-valued ContactSurface calculation between meshes is not" "currently supported"); } -// Checks if two surfaces meshes are equivalent. To be equivalent, there must be -// a bijective mapping between the two sets of triangles such that each mapped -// pair of triangles spans the same domain with the same winding (i.e., normal -// direction). Note that equivalence is a generous relationship. For example, a -// "triangle-soup" mesh and a watertight topology which represent the same -// manifold would be considered equivalent. Similarly, a mesh with extraneous, -// unreferenced vertices would also be considered equivalent. Those details are -// irrelevant for this test. -bool IsEquivalent(const SurfaceMesh& mesh_a, - const SurfaceMesh& mesh_b) { - // The number of triangles should be equal. - if (mesh_a.num_elements() != mesh_b.num_elements()) { - return false; - } - - // Check that the triangles are equivalent in that they consist of vertices in - // the same domain. We do this by representing each triangle by a vector of - // its 3 vertices, sorting them in a consistent manner, and comparing them - // across both meshes. We also keep metadata of the face index so that we - // can use the original 3 vertices to test face winding order later. - using SurfaceFacePair = std::pair>; - std::vector tri_vertices_a; - std::vector tri_vertices_b; - // Comparison function that defines a lexical ordering for Vector3d. - auto vector_comp = [](const Vector3d& vector_a, - const Vector3d& vector_b) -> bool { - // Sort based on x, then y, then z values. - if (vector_a[0] < vector_b[0]) { - return true; - } else if (vector_a[0] > vector_b[0]) { - return false; - } else if (vector_a[1] < vector_b[1]) { - return true; - } else if (vector_a[1] > vector_b[1]) { - return false; - } else { - return vector_a[2] < vector_b[2]; - } - }; - for (SurfaceFaceIndex f(0); f < mesh_a.num_elements(); ++f) { - std::array vectors_a; - std::array vectors_b; - for (int v = 0; v < 3; ++v) { - vectors_a[v] = mesh_a.vertex(mesh_a.element(f).vertex(v)).r_MV(); - vectors_b[v] = mesh_b.vertex(mesh_b.element(f).vertex(v)).r_MV(); - } - std::sort(vectors_a.begin(), vectors_a.end(), vector_comp); - std::sort(vectors_b.begin(), vectors_b.end(), vector_comp); - tri_vertices_a.emplace_back(f, vectors_a); - tri_vertices_b.emplace_back(f, vectors_b); - } - // Comparison function that defines a lexical ordering for SurfaceFacePair. - auto comp = [&vector_comp](const SurfaceFacePair& pair_a, - const SurfaceFacePair& pair_b) -> bool { - // Sort according to the first vertex, followed by the second and so on if - // there is a tie. - if (!CompareMatrices(pair_a.second[0], pair_b.second[0])) { - return vector_comp(pair_a.second[0], pair_b.second[0]); - } else if (!CompareMatrices(pair_a.second[1], pair_b.second[1])) { - return vector_comp(pair_a.second[1], pair_b.second[1]); - } else { - return vector_comp(pair_a.second[2], pair_b.second[2]); - } - }; - std::sort(tri_vertices_a.begin(), tri_vertices_a.end(), comp); - std::sort(tri_vertices_b.begin(), tri_vertices_b.end(), comp); - - // Now we can compare matching triangles across the two meshes. - const double kEps = std::numeric_limits::epsilon(); - for (std::vector::size_type i = 0; i < tri_vertices_a.size(); - ++i) { - // Compare that the vertices span the same domain. - for (int v = 0; v < 3; ++v) { - if (!CompareMatrices(tri_vertices_a[i].second[v], - tri_vertices_b[i].second[v])) { - return false; - } - } - // Since we kept track of the face index metadata, we can retrieve the - // original triangles and and check on face winding order by comparing the - // cross product (normal) between the two. - const Vector3d& face_normal_a = mesh_a.face_normal(tri_vertices_a[i].first); - const Vector3d& face_normal_b = mesh_b.face_normal(tri_vertices_b[i].first); - // Since they've been normalised to unit vectors we can check that the dot - // product should be almost equal to 1, bar an epsilon tolerance. - if (face_normal_a.dot(face_normal_b) < (1 - kEps)) { - return false; - } - } - - return true; -} - -// Tests ComputeContactSurfaceFromSoftVolumeRigidSurface using BVH broadphase -// culling produces an equivalent contact surface. -GTEST_TEST(MeshIntersectionTest, ComputeContactSurfaceSoftRigidBVH) { - auto id_S = GeometryId::get_new_id(); - auto id_R = GeometryId::get_new_id(); - auto mesh_S = OctahedronVolume(); - auto field_S = OctahedronPressureField(mesh_S.get()); - auto surface_R = PyramidSurface(); - // Move the rigid pyramid up, so only its square base intersects the top - // part of the soft octahedron. - const auto X_SR = RigidTransformd(Vector3d(0, 0, 0.5)); - - // The relationship between the frames for the soft body and the - // world frame is irrelevant for this test. - const auto X_WS = RigidTransformd::Identity(); - const auto X_WR = X_WS * X_SR; - - auto contact_SR = ComputeContactSurfaceFromSoftVolumeRigidSurface( - id_S, *field_S, X_WS, id_R, *surface_R, X_WR); - - // Compute the contact surface using the BVHs. - auto bvh_S = Bvh>(*mesh_S); - auto bvh_R = Bvh>(*surface_R); - auto bvh_contact_SR = ComputeContactSurfaceFromSoftVolumeRigidSurface( - id_S, *field_S, bvh_S, X_WS, id_R, *surface_R, bvh_R, X_WR); - - EXPECT_TRUE(IsEquivalent(contact_SR->mesh_W(), bvh_contact_SR->mesh_W())); -} - } // namespace } // namespace internal } // namespace geometry diff --git a/geometry/proximity/test/mesh_to_vtk_test.cc b/geometry/proximity/test/mesh_to_vtk_test.cc index 8b75165aa2fd..b27b86847b2f 100644 --- a/geometry/proximity/test/mesh_to_vtk_test.cc +++ b/geometry/proximity/test/mesh_to_vtk_test.cc @@ -60,23 +60,26 @@ GTEST_TEST(MeshToVtkTest, BoxTetrahedraPressureField) { unique_ptr> BoxContactSurface() { const Box soft_box(4., 4., 2.); // resolution_hint 0.5 is enough to have vertices on the medial axis. - const auto soft_mesh = MakeBoxVolumeMesh(soft_box, 0.5); + const VolumeMesh volume_S = MakeBoxVolumeMesh(soft_box, 0.5); const double kElasticModulus = 1.0e+5; - const auto soft_pressure = - MakeBoxPressureField(soft_box, &soft_mesh, kElasticModulus); + const VolumeMeshFieldLinear field_S = + MakeBoxPressureField(soft_box, &volume_S, kElasticModulus); + const Bvh> bvh_volume_S(volume_S); // The soft box is at the center of World. RigidTransformd X_WS = RigidTransformd::Identity(); const Box rigid_box(4, 4, 2); // Very coarse resolution_hint 4.0 should give the coarsest mesh. - const auto rigid_mesh = MakeBoxSurfaceMesh(rigid_box, 4.0); + const SurfaceMesh surface_R = + MakeBoxSurfaceMesh(rigid_box, 4.0); + const Bvh> bvh_surface_R(surface_R); // The rigid box intersects the soft box in a unit cube at the corner // (2.0, 2.0, 1.0). RigidTransformd X_WR(Vector3d{3., 3., 1.}); return ComputeContactSurfaceFromSoftVolumeRigidSurface( - GeometryId::get_new_id(), soft_pressure, X_WS, - GeometryId::get_new_id(), rigid_mesh, X_WR); + GeometryId::get_new_id(), field_S, bvh_volume_S, X_WS, + GeometryId::get_new_id(), surface_R, bvh_surface_R, X_WR); } GTEST_TEST(MeshToVtkTest, BoxContactSurfacePressure) {