From ae294d5284352f26fb4310f1af3536d5a8b17521 Mon Sep 17 00:00:00 2001 From: Joseph Masterjohn Date: Wed, 23 Oct 2024 15:45:56 -0400 Subject: [PATCH] [geometry] Adds new compliant mesh intersection code --- geometry/proximity/BUILD.bazel | 4 + geometry/proximity/field_intersection.cc | 321 ++++++++++--- geometry/proximity/field_intersection.h | 224 ++++++---- geometry/proximity/hydroelastic_calculator.cc | 10 +- geometry/proximity/mesh_plane_intersection.cc | 94 +++- geometry/proximity/mesh_plane_intersection.h | 3 +- .../proximity/test/field_intersection_test.cc | 420 ++++++++++++------ .../test/hydroelastic_calculator_test.cc | 8 + 8 files changed, 799 insertions(+), 285 deletions(-) diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index b2e34818c048..d0bae61cf6fd 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -344,11 +344,15 @@ drake_cc_library( deps = [ ":bvh", ":contact_surface_utility", + ":hydroelastic_internal", ":mesh_field", ":mesh_intersection", ":mesh_plane_intersection", ":plane", ":posed_half_space", + ":triangle_surface_mesh", + ":volume_mesh_topology", + ":volume_to_surface_mesh", "//common:default_scalars", "//geometry/query_results:contact_surface", ], diff --git a/geometry/proximity/field_intersection.cc b/geometry/proximity/field_intersection.cc index 784208cc8218..4c1c0062cf42 100644 --- a/geometry/proximity/field_intersection.cc +++ b/geometry/proximity/field_intersection.cc @@ -1,8 +1,9 @@ #include "drake/geometry/proximity/field_intersection.h" -#include +#include +#include #include -#include +#include #include "drake/common/default_scalars.h" #include "drake/geometry/proximity/contact_surface_utility.h" @@ -16,6 +17,16 @@ namespace internal { using Eigen::Vector3d; +namespace { + +struct TetPairHasher final { + std::size_t operator()(const std::pair& p) const { + return std::hash()(p.first) ^ std::hash()(p.second); + } +}; + +} // namespace + template bool CalcEquilibriumPlane(int element0, const VolumeMeshFieldLinear& field0_M, @@ -24,13 +35,15 @@ bool CalcEquilibriumPlane(int element0, const math::RigidTransform& X_MN, Plane* plane_M) { const Vector3d grad_f0_M = field0_M.EvaluateGradient(element0); - const Vector3d p_MMo = Vector3d::Zero(); // Value of f₀ at the origin of frame M. - const double f0_Mo = field0_M.EvaluateCartesian(element0, p_MMo); + const double f0_Mo = field0_M.EvaluateAtMo(element0); const Vector3d grad_f1_N = field1_N.EvaluateGradient(element1); const Vector3 grad_f1_M = X_MN.rotation() * grad_f1_N.cast(); - const Vector3 p_NMo = X_MN.inverse() * p_MMo.cast(); + // p_NoMo_N = R_NM * p_NoMo_M = R_MN⁻¹ * (-p_MoNo_M) + const Vector3 p_NMo = + -(X_MN.rotation().matrix().transpose() * X_MN.translation()); + // Value of f₁ at the origin of frame M, which is the frame of f₀. const T f1_Mo = field1_N.EvaluateCartesian(element1, p_NMo); @@ -63,14 +76,14 @@ bool CalcEquilibriumPlane(int element0, // // p_MQ = -Δ * nhat_M // - const Vector3 p_MQ = -((f0_Mo - f1_Mo) / magnitude) * nhat_M; + const Vector3 p_MQ = ((f1_Mo - f0_Mo) / magnitude) * nhat_M; *plane_M = Plane(nhat_M, p_MQ, /*already_normalized = */ true); return true; } template -std::vector> IntersectTetrahedra( +std::pair>, std::vector> IntersectTetrahedra( int element0, const VolumeMesh& mesh0_M, int element1, const VolumeMesh& mesh1_N, const math::RigidTransform& X_MN, const Plane& equilibrium_plane_M) { @@ -81,6 +94,9 @@ std::vector> IntersectTetrahedra( // We use two alternating buffers to reduce heap allocations. std::vector> polygon_buffer[2]; + std::vector face_buffer[2]; + std::array distances; + // Each contact polygon has at most 8 vertices because it is the // intersection of the pressure-equilibrium plane and the two tetrahedra. // The plane intersects a tetrahedron into a convex polygon with at most four @@ -88,13 +104,18 @@ std::vector> IntersectTetrahedra( // more vertices. polygon_buffer[0].reserve(8); polygon_buffer[1].reserve(8); + face_buffer[0].reserve(8); + face_buffer[1].reserve(8); // Intersects the equilibrium plane with the tetrahedron element0. - std::vector>* polygon_M = &(polygon_buffer[0]); - SliceTetrahedronWithPlane(element0, mesh0_M, equilibrium_plane_M, polygon_M); - RemoveNearlyDuplicateVertices(polygon_M); - // Null polygon - if (polygon_M->size() < 3) return {}; + SliceTetrahedronWithPlane(element0, mesh0_M, equilibrium_plane_M, + &polygon_buffer[0], nullptr /* cut_edges */, + &face_buffer[0]); + + RemoveNearlyDuplicateVertices(&polygon_buffer[0]); + if (polygon_buffer[0].size() < 3) { + return {}; + } // Positions of vertices of tetrahedral element1 in mesh1_N expressed in // frame M. @@ -103,31 +124,77 @@ std::vector> IntersectTetrahedra( p_MVs[i] = X_MN * mesh1_N.vertex(mesh1_N.element(element1).vertex(i)).cast(); } - // Each tuple of three vertex indices are oriented so that their normal - // vector points outward from the tetrahedron. - constexpr int kFaceVertexLocalIndex[4][3] = { - {1, 2, 3}, {0, 3, 2}, {0, 1, 3}, {0, 2, 1}}; - std::vector>* in_M = polygon_M; - std::vector>* out_M = &(polygon_buffer[1]); + std::vector>* current_polygon = &polygon_buffer[0]; + std::vector>* clipped_polygon = &polygon_buffer[1]; + std::vector* current_faces = &face_buffer[0]; + std::vector* clipped_faces = &face_buffer[1]; + // Intersects the polygon with the four halfspaces of the four triangles // of the tetrahedral element1. - for (const auto& face_vertices : kFaceVertexLocalIndex) { - const Vector3& p_MA = p_MVs[face_vertices[0]]; - const Vector3& p_MB = p_MVs[face_vertices[1]]; - const Vector3& p_MC = p_MVs[face_vertices[2]]; + for (int face = 0; face < 4; ++face) { + clipped_polygon->clear(); + clipped_faces->clear(); + + // 'face' corresponds to the triangle formed by {0, 1, 2, 3} - {face} + // so any of (face+1)%4, (face+2)%4, (face+3)%4 are candidates for a + // point on the face's plane. We arbitrarily choose (face + 1) % 4. + const Vector3& p_MA = p_MVs[(face + 1) % 4]; const Vector3 triangle_outward_normal_M = - (p_MB - p_MA).cross(p_MC - p_MA); + X_MN.rotation() * -mesh1_N.inward_normal(element1, face).cast(); PosedHalfSpace half_space_M(triangle_outward_normal_M, p_MA); - ClipPolygonByHalfSpace(*in_M, half_space_M, out_M); - RemoveNearlyDuplicateVertices(out_M); - if (out_M->size() < 3) { - return {}; // Empty intersection; no contact here. + + const int size = ssize(*current_polygon); + + for (int i = 0; i < size; ++i) { + distances[i] = half_space_M.CalcSignedDistance((*current_polygon)[i]); + } + + // Walk the vertices checking for intersecting edges. + for (int i = 0; i < size; ++i) { + int j = (i + 1) % size; + // If the edge out of vertex i is at least partially inside, include + // vertex i and face i. + if (distances[i] <= 0) { + clipped_polygon->push_back((*current_polygon)[i]); + clipped_faces->push_back((*current_faces)[i]); + // If vertex j is outside, we've discovered an intersection. Add the + // intersection point as well as "face" to the list of faces. (For faces + // from element1 we insert face + 4, to indicate which element the edge + // came from). + if (distances[j] > 0) { + const T wa = distances[j] / (distances[j] - distances[i]); + const T wb = T(1.0) - wa; + clipped_polygon->push_back(wa * (*current_polygon)[i] + + wb * (*current_polygon)[j]); + clipped_faces->push_back(face + 4); + } + } else if (distances[j] <= 0) { + // Vertex i is outside, but vertex j is at least partially inside. We've + // discovered another intersection. Add the intersection point, this + // point is still on the edge created by current_faces[i], so include + // that face for the edge starting at the intersection point. + const T wa = distances[j] / (distances[j] - distances[i]); + const T wb = T(1.0) - wa; + clipped_polygon->push_back(wa * (*current_polygon)[i] + + wb * (*current_polygon)[j]); + clipped_faces->push_back((*current_faces)[i]); + } + } + std::swap(current_polygon, clipped_polygon); + std::swap(current_faces, clipped_faces); + + // If we've clipped off the entire polygon, return empty. + if (ssize(*current_polygon) == 0) { + return {}; } - std::swap(in_M, out_M); } - polygon_M = in_M; - return *polygon_M; + RemoveNearlyDuplicateVertices(current_polygon); + if (current_polygon->size() < 3) { + return {}; + } + + return std::make_pair(*current_polygon, *current_faces); } template @@ -170,6 +237,7 @@ void VolumeIntersector::IntersectFields( candidate_tetrahedra.emplace_back(tet0, tet1); return BvttCallbackResult::Continue; }; + bvh0_M.Collide(bvh1_N, convert_to_double(X_MN), callback); MeshBuilder builder_M; @@ -184,7 +252,130 @@ void VolumeIntersector::IntersectFields( } template -void VolumeIntersector::CalcContactPolygon( +void VolumeIntersector::IntersectFields( + const VolumeMeshFieldLinear& field0_M, + const Bvh>& bvh0_M, + const std::vector& tri_to_tet_M, + const VolumeMeshTopology& mesh_topology_M, + const VolumeMeshFieldLinear& field1_N, + const Bvh>& bvh1_N, + const std::vector& tri_to_tet_N, + const VolumeMeshTopology& mesh_topology_N, + const math::RigidTransform& X_MN, + std::unique_ptr* surface_01_M, + std::unique_ptr* e_01_M) { + DRAKE_DEMAND(surface_01_M != nullptr); + DRAKE_DEMAND(e_01_M != nullptr); + surface_01_M->reset(); + e_01_M->reset(); + tet0_of_contact_polygon_.clear(); + tet1_of_contact_polygon_.clear(); + + const math::RigidTransform X_MNd = convert_to_double(X_MN); + + // Keep track of pairs of tet indices that have already been inserted into the + // queue. + std::unordered_set, TetPairHasher> visited_pairs; + + std::queue> candidate_tetrahedra; + auto callback = [&candidate_tetrahedra, &visited_pairs, &tri_to_tet_M, + &tri_to_tet_N](int tri0, int tri1) -> BvttCallbackResult { + DRAKE_ASSERT(0 <= tri0 && tri0 < ssize(tri_to_tet_M)); + DRAKE_ASSERT(0 <= tri1 && tri1 < ssize(tri_to_tet_N)); + std::pair tet_pair(tri_to_tet_M[tri0].tet_index, + tri_to_tet_N[tri1].tet_index); + if (!visited_pairs.contains(tet_pair)) { + candidate_tetrahedra.push(tet_pair); + visited_pairs.insert(tet_pair); + } + return BvttCallbackResult::Continue; + }; + + // Collide the BVHs of the surface meshes and enqueue the candidate surface + // tetrahedra. The pressure range of surface tetrahedra are guaranteed to + // contain 0 (even if the surface vertex values are not 0 due to margin), + // thus if the tetrahedra overlap spatially they are guaranteed to produce + // a non-empty contact polygon. + bvh0_M.Collide(bvh1_N, X_MNd, callback); + + if (candidate_tetrahedra.empty()) return; + + MeshBuilder builder_M; + const math::RotationMatrix R_NM = X_MN.rotation().inverse(); + + // The intersected faces of tetM and tetN with the contact polygon. + std::vector faces; + + // Traverse all overlapping tet pairs and generate contact polygons. + while (!candidate_tetrahedra.empty()) { + const std::pair pair = candidate_tetrahedra.front(); + candidate_tetrahedra.pop(); + + const auto& [tetM, tetN] = pair; + // If a pair makes it into the queue, their pressure ranges intersect. But, + // they may or may not overlap. If they do not overlap and produce a contact + // surface, we do not have to process any neighbors. Note: the initial set + // of candidate tets are all boundary tets, so their pressure ranges + // necessarily intersect because they all contain 0 and are thus inserted + // without checking range intersection. + faces = CalcContactPolygon(field0_M, field1_N, X_MN, R_NM, tetM, tetN, + &builder_M); + + if (ssize(faces) == 0) { + continue; + } + + // Loop over all intersected faces. + for (int face : faces) { + // If face ∈ [0, 3], then it is an index of a face of tetM. + if (face < 4) { + // The index of the neighbor of tetM sharing the face with index face. + int neighborM = mesh_topology_M.neighbor(tetM, face); + // If there is no neighbor, continue. + if (neighborM < 0) continue; + + // neighbor and tetN's pressure ranges intersect necessarily (the equal + // pressure plane intersects a face of neighbor). They may overlap + // spatially as well, so try to insert them into the queue. + std::pair neighbor_pair(neighborM, tetN); + + // If this candidate pair has already been checked, continue. + if (visited_pairs.contains(neighbor_pair)) { + continue; + } + + candidate_tetrahedra.push(neighbor_pair); + visited_pairs.insert(neighbor_pair); + + } else { + // face ∈ [4, 7], thus face - 4 is a face of tetN. + int neighborN = mesh_topology_N.neighbor(tetN, face - 4); + // If there is no neighbor, continue. + if (neighborN < 0) continue; + + // neighbor and tetM's pressure ranges intersect necessarily (the equal + // pressure plane intersects a face of neighbor). They may overlap + // spatially as well, so try to insert them into the queue. + std::pair neighbor_pair(tetM, neighborN); + + // If this candidate pair has already been checked, continue. + if (visited_pairs.contains(neighbor_pair)) { + continue; + } + + candidate_tetrahedra.push(neighbor_pair); + visited_pairs.insert(neighbor_pair); + } + } + } + + if (builder_M.num_faces() == 0) return; + + std::tie(*surface_01_M, *e_01_M) = builder_M.MakeMeshAndField(); +} + +template +std::vector VolumeIntersector::CalcContactPolygon( const VolumeMeshFieldLinear& field0_M, const VolumeMeshFieldLinear& field1_N, const math::RigidTransform& X_MN, const math::RotationMatrix& R_NM, @@ -194,23 +385,23 @@ void VolumeIntersector::CalcContactPolygon( Plane equilibrium_plane_M{Vector3d::UnitZ(), Vector3d::Zero()}; if (!CalcEquilibriumPlane(tet0, field0_M, tet1, field1_N, X_MN, &equilibrium_plane_M)) { - return; + return {}; } // The normal points in the direction of increasing field_0 and decreasing // field_1. Vector3 polygon_nhat_M = equilibrium_plane_M.normal(); if (!IsPlaneNormalAlongPressureGradient(polygon_nhat_M, tet0, field0_M)) { - return; + return {}; } Vector3 reverse_polygon_nhat_N = R_NM * (-polygon_nhat_M); if (!IsPlaneNormalAlongPressureGradient(reverse_polygon_nhat_N, tet1, field1_N)) { - return; + return {}; } - const std::vector> polygon_vertices_M = IntersectTetrahedra( + const auto [polygon_vertices_M, faces] = IntersectTetrahedra( tet0, field0_M.mesh(), tet1, field1_N.mesh(), X_MN, equilibrium_plane_M); - if (polygon_vertices_M.size() < 3) return; + if (polygon_vertices_M.size() < 3) return {}; // Add the vertices to the builder_M (with corresponding pressure values) // and construct index-based polygon representation. @@ -232,25 +423,37 @@ void VolumeIntersector::CalcContactPolygon( tet0_of_contact_polygon_.push_back(tet0); tet1_of_contact_polygon_.push_back(tet1); } + + return faces; } template void HydroelasticVolumeIntersector::IntersectCompliantVolumes( - GeometryId id0, const VolumeMeshFieldLinear& field0_M, - const Bvh>& bvh0_M, - const math::RigidTransform& X_WM, GeometryId id1, - const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + GeometryId id_M, const hydroelastic::SoftMesh& compliant_M, + const math::RigidTransform& X_WM, GeometryId id_N, + const hydroelastic::SoftMesh& compliant_N, const math::RigidTransform& X_WN, - std::unique_ptr>* contact_surface_W) { + std::unique_ptr>* contact_surface_W, + const bool use_surfaces) { const math::RigidTransform X_MN = X_WM.InvertAndCompose(X_WN); // The computation will be in Frame M and then transformed to the world frame. std::unique_ptr surface01_M; std::unique_ptr field01_M; VolumeIntersector volume_intersector; - volume_intersector.IntersectFields(field0_M, bvh0_M, field1_N, bvh1_N, X_MN, - &surface01_M, &field01_M); + + if (use_surfaces) { + volume_intersector.IntersectFields( + compliant_M.pressure(), compliant_M.surface_mesh_bvh(), + compliant_M.tri_to_tet(), compliant_M.mesh_topology(), + compliant_N.pressure(), compliant_N.surface_mesh_bvh(), + compliant_N.tri_to_tet(), compliant_N.mesh_topology(), X_MN, + &surface01_M, &field01_M); + } else { + volume_intersector.IntersectFields( + compliant_M.pressure(), compliant_M.bvh(), compliant_N.pressure(), + compliant_N.bvh(), X_MN, &surface01_M, &field01_M); + } if (surface01_M == nullptr) return; @@ -268,15 +471,15 @@ void HydroelasticVolumeIntersector::IntersectCompliantVolumes( auto grad_field0_W = std::make_unique>>(); grad_field0_W->reserve(num_contact_polygons); for (int i = 0; i < num_contact_polygons; ++i) { - const Vector3& grad_field0_M = - field0_M.EvaluateGradient(volume_intersector.tet0_of_polygon(i)); + const Vector3& grad_field0_M = compliant_M.pressure().EvaluateGradient( + volume_intersector.tet0_of_polygon(i)); grad_field0_W->emplace_back(X_WM.rotation() * grad_field0_M); } auto grad_field1_W = std::make_unique>>(); grad_field1_W->reserve(num_contact_polygons); for (int i = 0; i < num_contact_polygons; ++i) { - const Vector3& grad_field1_N = - field1_N.EvaluateGradient(volume_intersector.tet1_of_polygon(i)); + const Vector3& grad_field1_N = compliant_N.pressure().EvaluateGradient( + volume_intersector.tet1_of_polygon(i)); grad_field1_W->emplace_back(X_WN.rotation() * grad_field1_N); } @@ -288,30 +491,30 @@ void HydroelasticVolumeIntersector::IntersectCompliantVolumes( // with normals in the direction of // - increasing pressure field0 (going *into* the first geometry) and // - decreasing pressure field1 (going *out* of the second geometry), - // so we create ContactSurface with the ids in the order of (id0, id1). + // so we create ContactSurface with the ids in the order of (id_M, id_N). *contact_surface_W = std::make_unique>( - id0, id1, std::move(surface01_M), std::move(field01_M), + id_M, id_N, std::move(surface01_M), std::move(field01_M), std::move(grad_field0_W), std::move(grad_field1_W)); } template std::unique_ptr> ComputeContactSurfaceFromCompliantVolumes( - GeometryId id0, const VolumeMeshFieldLinear& field0_M, - const Bvh>& bvh0_M, - const math::RigidTransform& X_WM, GeometryId id1, - const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + GeometryId id_M, const hydroelastic::SoftMesh& compliant_M, + const math::RigidTransform& X_WM, GeometryId id_N, + const hydroelastic::SoftMesh& compliant_N, const math::RigidTransform& X_WN, HydroelasticContactRepresentation representation) { std::unique_ptr> contact_surface_W; if (representation == HydroelasticContactRepresentation::kTriangle) { HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(id0, field0_M, bvh0_M, X_WM, id1, field1_N, - bvh1_N, X_WN, &contact_surface_W); + .IntersectCompliantVolumes(id_M, compliant_M, X_WM, id_N, compliant_N, + X_WN, &contact_surface_W, + false /* use_surfaces */); } else { HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(id0, field0_M, bvh0_M, X_WM, id1, field1_N, - bvh1_N, X_WN, &contact_surface_W); + .IntersectCompliantVolumes(id_M, compliant_M, X_WM, id_N, compliant_N, + X_WN, &contact_surface_W, + false /* use_surfaces */); } return contact_surface_W; } diff --git a/geometry/proximity/field_intersection.h b/geometry/proximity/field_intersection.h index f9539b8f4247..97d715ba8cf5 100644 --- a/geometry/proximity/field_intersection.h +++ b/geometry/proximity/field_intersection.h @@ -1,14 +1,19 @@ #pragma once #include +#include #include #include "drake/common/eigen_types.h" #include "drake/geometry/proximity/bvh.h" +#include "drake/geometry/proximity/hydroelastic_internal.h" #include "drake/geometry/proximity/obb.h" #include "drake/geometry/proximity/plane.h" +#include "drake/geometry/proximity/triangle_surface_mesh.h" #include "drake/geometry/proximity/volume_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/query_results/contact_surface.h" #include "drake/math/rigid_transform.h" @@ -63,15 +68,26 @@ bool CalcEquilibriumPlane(int element0, @param[in] plane_M The plane between the two tetrahedra, expressed in frame M. - @retval `polygon_M` A sequence of vertex positions of the intersecting - polygon expressed in frame M. Its unit normal vector - in CCW winding order convention is in the same direction - as the plane's normal vector. + @retval `` `polygon_M` is sequence of vertex positions of + the intersecting polygon expressed in frame M. Its unit normal vector in CCW + winding order convention is in the same direction as the plane's normal vector. + + `faces` is a sequence of local indices of faces of element0 and element1, thus + polygon_M.size() == faces.size. Each edge of the polygon is associated with one + triangular face of a tetrahedron because the polygon is the common intersection + of the equilibrium plane with the eight halfspaces from all faces of the two + tetrahedra. Some halfspaces are redundant and their corresponding face indices + do not show up in `faces`. faces[i] contains the index of the face that + produced the edge (polygon_M[i], polygon_M[i+1]). The values of `faces` are + encoded to identify whether the value corresponds to a face of element0 or + element1. If faces[i] ∈ [0, 3], then faces[i] is the index of a face of + element0. If faces[i] ∈ [4, 7], then faces[i] - 4 is the index of a face of + element1. @tparam T A valid Eigen scalar. */ template -std::vector> IntersectTetrahedra( +std::pair>, std::vector> IntersectTetrahedra( int element0, const VolumeMesh& mesh0_M, int element1, const VolumeMesh& mesh1_N, const math::RigidTransform& X_MN, const Plane& equilibrium_plane_M); @@ -87,7 +103,7 @@ std::vector> IntersectTetrahedra( @param nhat_M The normal to test against, expressed in the same frame M of the piecewise linear field. @param tetrahedron The index of the tetrahedron in the mesh of the field. - @param field_M The piecewise linear field expressed in frame M. + @param field0_M The piecewise linear field expressed in frame M. @pre `nhat_M` is unit length. @return `true` if the angle between `nhat_M` and the field gradient vector lies within a hard-coded tolerance. @@ -95,7 +111,7 @@ std::vector> IntersectTetrahedra( template bool IsPlaneNormalAlongPressureGradient( const Vector3& nhat_M, int tetrahedron, - const VolumeMeshFieldLinear& field_M); + const VolumeMeshFieldLinear& field0_M); /* %VolumeIntersector performs an intersection algorithm between two tetrahedral meshes with scalar fields to get a contact surface, on which @@ -124,39 +140,99 @@ class VolumeIntersector { VolumeIntersector() {} + /* @name Contact Surface Intersection Algorithms + + The following overloads of `IntersectFields` each provide a unique + algorithm for computing the contact surface of two pressure fields + represented by two VolumeMeshFieldLinear objects. The implementations + differ in their algorithmic details, but otherwise produce identical + contact surfaces (up to a permutation of element/vertex indices) with one + caveat: When the two geometries overlap but their surfaces do not intersect + (i.e. one geometry is completely enclosed in the interior of the other) the + algorithm that makes use of surface mesh BVHs will return an empty contact + surface, where the volume mesh BVH version might return a non-empty contact + surface. It is important to note that one geometry that is completely + enclosed in the other is a non-physical configuration and a contact surface + computed from this configuration is considered degenerate. + + Either of these functions can be used by Hydroelastics and deformables when + the scalar fields are pressure fields and signed distance fields, + respectively. + */ + /* @{ */ /* Creates the mesh and the scalar field on the contact surface between two - tetrahedral meshes with scalar fields. The output surface mesh is posed in - frame M of the first tetrahedral mesh. - - Hydroelastics and deformables use this function when the scalar fields - are pressure fields and signed distance fields, respectively. - - @param[in] field0_M The first geometry represented as a tetrahedral - mesh with scalar field, expressed in frame M. - @param[in] bvh0_M The bounding volume hierarchy built on the tetrahedral - mesh of `field0_M`. - @param[in] field1_N The second geometry represented as a - tetrahedral mesh with scalar field, expressed in - frame N. - @param[in] bvh1_N The bounding volume hierarchy built on the tetrahedral - mesh of `field1_N`. - @param[in] X_MN The pose of frame N in frame M. - @param[out] surface_01_M The output mesh of the contact surface between - the two geometries expressed in frame M. The surface - normal is in the direction of increasing the difference - field0 - field1. Usually it is the direction - of increasing field0 and decreasing field1. - @param[out] e_01_M The scalar field on the contact surface, expressed in - frame M. + tetrahedral meshes with scalar fields. This approach computes it by + intersecting tetrahedra directly (accelerated with a BVH build on the + tetrahedra). The output surface mesh is posed in frame M of the first + tetrahedral mesh. + + @param[in] field0_M The first geometry represented as a tetrahedral mesh + with scalar field, expressed in frame M. + @param[in] bvh_M The bounding volume hierarchy built on the tetrahedral + mesh of `field0_M`. + @param[in] field1_N The second geometry represented as a tetrahedral mesh + with scalar field, expressed in frame N. + @param[in] bvh_N The bounding volume hierarchy built on the tetrahedral + mesh of `field1_N`. + @param[in] X_MN The pose of frame N in frame M. + @param[out] surface_M The output mesh of the contact surface between + the two geometries expressed in frame M. The surface + normal is in the direction of increasing the + difference field0 - field1. Usually it is the + direction of increasing field0 and decreasing field1. + @param[out] e_M The scalar field on the contact surface, expressed in + frame M. + + @note The output surface mesh may have duplicate vertices. + */ + void IntersectFields(const VolumeMeshFieldLinear& field0_M, + const Bvh>& bvh_M, + const VolumeMeshFieldLinear& field1_N, + const Bvh>& bvh_N, + const math::RigidTransform& X_MN, + std::unique_ptr* surface_M, + std::unique_ptr* e_M); + + /* Creates the mesh and the scalar field on the contact surface between two + tetrahedral meshes with scalar fields. This approach computes it by finding + intersecting surface tetrahedra (accelerated by a BVH of the surface + triangles) and traversing intersecting neighbor tetrahedra in a breadth first + search fashion. The parameters are similar to the previous overload with the + following differences: + + - The BVH is built on the volume mesh's surface mesh. + - We include additional data relating surface mesh to volume mesh and mesh + topology. + + @param[in] bvh_M The bounding volume hierarchy built on the + surface mesh of `field_M`. + @param[in] tri_to_tet_M A mapping from surface triangle indices in the + surface mesh of M to their corresponding tet + element indices in the volume mesh of M. + @param[in] mesh_topology_M A representation of the topology of the volume + mesh M. Used to access neighbor adjacencies. + @param[in] bvh_N The bounding volume hierarchy built on the + surface mesh of `field_N`. + @param[in] tri_to_tet_N A mapping from surface triangle indices in the + surface mesh of N to their corresponding tet + element indices in the volume mesh of N. + @param[in] mesh_topology_N A representation of the topology of the volume + mesh N. Used to access neighbor adjacencies. + @note The output surface mesh may have duplicate vertices. */ void IntersectFields(const VolumeMeshFieldLinear& field0_M, - const Bvh>& bvh0_M, + const Bvh>& bvh_M, + const std::vector& tri_to_tet_M, + const VolumeMeshTopology& mesh_topology_M, const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + const Bvh>& bvh_N, + const std::vector& tri_to_tet_N, + const VolumeMeshTopology& mesh_topology_N, const math::RigidTransform& X_MN, - std::unique_ptr* surface_01_M, - std::unique_ptr* e_01_M); + std::unique_ptr* surface_M, + std::unique_ptr* e_M); + /* @} */ /* Returns the index of tetrahedron in the first mesh containing the i-th contact polygon. @@ -181,15 +257,21 @@ class VolumeIntersector { parameter for performance reason. @param[in] tet0 Index of the first tetrahedron in the first mesh. @param[in] tet1 Index of the second tetrahedron in the second mesh. - @param[in, out] builder_M If there is a non-empty intersection, the + @param[in, out] builder_M If there is a non-empty intersection, the intersecting polygon (contact polygon) and its field values are added into builder_M. The added - polygon is in frame M. */ - void CalcContactPolygon(const VolumeMeshFieldLinear& field0_M, - const VolumeMeshFieldLinear& field1_N, - const math::RigidTransform& X_MN, - const math::RotationMatrix& R_NM, int tet0, - int tet1, MeshBuilder* builder_M); + polygon is in frame M. + @returns A vector of indices of faces of either tet0 or tet1 corresponding + to the tet faces that intersect the calculated contact polygon. See + IntersectTetrahedra() for a more detailed explanation of the + encoding of the indices. If no contact polygon is produced, the + returned vector will be empty. + */ + std::vector CalcContactPolygon( + const VolumeMeshFieldLinear& field0_M, + const VolumeMeshFieldLinear& field1_N, + const math::RigidTransform& X_MN, const math::RotationMatrix& R_NM, + int tet0, int tet1, MeshBuilder* builder_M); // List of tetrahedron indices in the meshes of field0 and field1. One // index per contact polygon; @@ -216,48 +298,38 @@ class HydroelasticVolumeIntersector { geometries. The output contact surface is posed in World frame. It is a helper of ComputeContactSurfaceFromCompliantVolumes(). - @param[in] id0 ID of the first geometry. - @param[in] field0_M Pressure field on the tetrahedral mesh of the first - geometry, expressed in frame M. - @param[in] bvh0_M A bounding volume hierarchy built on the mesh of - `field0_M`. - @param[in] X_WM The pose of the first geometry in World. - @param[in] id1 ID of the second geometry. - @param[in] field1_N Pressure field on the tetrahedral mesh of the second - geometry, expressed in frame N. - @param[in] bvh1_N A bounding volume hierarchy built on the mesh of - `field1_N`. - @param[in] X_WN The pose of the second geometry in World. - @param[out] contact_surface_W The contact surface, whose type (e.g., + @param[in] id_M Id of geometry M. + @param[in] compliant_M SoftMesh of geometry M, expressed in frame M. + @param[in] X_WM The pose of the first geometry in World. + @param[in] id_N Id of geometry N. + @param[in] compliant_N SoftMesh of geometry N, expressed in frame N. + @param[in] X_WN The pose of the second geometry in World. + @param[in] use_surfaces If true, uses the version of + VolumeIntersector::IntersectFields() that makes use + of the topology of the geometries' surfaces. + @param[out] contact_surface_W The contact surface, whose type (e.g., triangles or polygons) depends on the type parameter MeshBuilder. It is expressed in World frame. If there is no contact, nullptr is returned. */ void IntersectCompliantVolumes( - GeometryId id0, const VolumeMeshFieldLinear& field0_M, - const Bvh>& bvh0_M, - const math::RigidTransform& X_WM, GeometryId id1, - const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + GeometryId id_M, const hydroelastic::SoftMesh& compliant_M, + const math::RigidTransform& X_WM, GeometryId id_N, + const hydroelastic::SoftMesh& compliant_N, const math::RigidTransform& X_WN, - std::unique_ptr>* contact_surface_W); + std::unique_ptr>* contact_surface_W, + const bool use_surfaces = true); }; /* Computes the contact surface between two compliant hydroelastic geometries with the requested representation. The output contact surface is posed in World frame. - @param[in] id0 Id of the first geometry. - @param[in] field0_M Pressure field on the tetrahedral mesh of the first - geometry, expressed in frame M. - @param[in] bvh0_M A bounding volume hierarchy built on the mesh of - `field0_M`. - @param[in] X_WM The pose of the first geometry in World. - @param[in] id1 Id of the second geometry. - @param[in] field1_N Pressure field on the tetrahedral mesh of the second - geometry, expressed in frame N. - @param[in] bvh1_N A bounding volume hierarchy built on the mesh of - `field1_N`. - @param[in] X_WN The pose of the second geometry in World. + @param[in] id_M Id of geometry M. + @param[in] compliant_M SoftMesh of geometry M, expressed in frame M. + @param[in] X_WM The pose of the first geometry in World. + @param[in] id_N Id of geometry N. + @param[in] compliant_N SoftMesh of geometry N, expressed in frame N. + @param[in] X_WN The pose of the second geometry in World. @param[in] representation The preferred representation of each contact polygon. @@ -274,11 +346,9 @@ class HydroelasticVolumeIntersector { */ template std::unique_ptr> ComputeContactSurfaceFromCompliantVolumes( - GeometryId id0, const VolumeMeshFieldLinear& field0_M, - const Bvh>& bvh0_M, - const math::RigidTransform& X_WM, GeometryId id1, - const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + GeometryId id_M, const hydroelastic::SoftMesh& compliant_M, + const math::RigidTransform& X_WM, GeometryId id_N, + const hydroelastic::SoftMesh& compliant_N, const math::RigidTransform& X_WN, HydroelasticContactRepresentation representation); diff --git a/geometry/proximity/hydroelastic_calculator.cc b/geometry/proximity/hydroelastic_calculator.cc index 97d6d557d364..60f64aed2b51 100644 --- a/geometry/proximity/hydroelastic_calculator.cc +++ b/geometry/proximity/hydroelastic_calculator.cc @@ -59,15 +59,9 @@ std::unique_ptr> CalcCompliantCompliant( HydroelasticContactRepresentation representation) { DRAKE_DEMAND(!compliant_F.is_half_space() && !compliant_G.is_half_space()); - const VolumeMeshFieldLinear& field_F = - compliant_F.pressure_field(); - const Bvh>& bvh_F = compliant_F.bvh(); - const VolumeMeshFieldLinear& field_G = - compliant_G.pressure_field(); - const Bvh>& bvh_G = compliant_G.bvh(); - return ComputeContactSurfaceFromCompliantVolumes( - id_F, field_F, bvh_F, X_WF, id_G, field_G, bvh_G, X_WG, representation); + id_F, compliant_F.soft_mesh(), X_WF, id_G, compliant_G.soft_mesh(), X_WG, + representation); } template diff --git a/geometry/proximity/mesh_plane_intersection.cc b/geometry/proximity/mesh_plane_intersection.cc index 5ecbeccdbe18..2df1d40b393d 100644 --- a/geometry/proximity/mesh_plane_intersection.cc +++ b/geometry/proximity/mesh_plane_intersection.cc @@ -22,14 +22,16 @@ constexpr std::array, 6> kTetEdges = { // pyramid with top at node 3. TetrahedronEdge{0, 3}, TetrahedronEdge{1, 3}, TetrahedronEdge{2, 3}}; -/* Marching tetrahedra table. Each entry in this table has an index value +/* Marching tetrahedra tables. Each entry in these tables have an index value based on a binary encoding of the signs of the plane's signed distance function evaluated at all tetrahedron vertices. Therefore, with four vertices and two possible signs, we have a total of 16 entries. We encode the table indexes in binary so that a "1" and "0" correspond to a vertex with positive or negative signed distance, respectively. The least significant bit (0) corresponds to vertex 0 in the tetrahedron, and the - most significant bit (3) is vertex 3. Each entry stores a vector of edges. + most significant bit (3) is vertex 3. */ + +/* Each entry of kMarchingTetsEdgeTable stores a vector of edges. Based on the signed distance values, these edges are the ones that intersect the plane. Edges are numbered according to the table kTetEdges. The edges have been ordered such that a polygon formed by visiting the @@ -41,7 +43,7 @@ constexpr std::array, 6> kTetEdges = { intersecting edges is equal to the index of the *first* -1 (with an implicit logical -1 at index 4). */ // clang-format off -constexpr std::array, 16> kMarchingTetsTable = { +constexpr std::array, 16> kMarchingTetsEdgeTable = { /* bits 3210 */ std::array{-1, -1, -1, -1}, /* 0000 */ std::array{0, 3, 2, -1}, /* 0001 */ @@ -59,16 +61,75 @@ constexpr std::array, 16> kMarchingTetsTable = { std::array{0, 4, 1, -1}, /* 1101 */ std::array{0, 2, 3, -1}, /* 1110 */ std::array{-1, -1, -1, -1} /* 1111 */}; + +/* Each entry of kMarchingTetsFaceTable stores a vector of face indices. + Based on the signed distance values, these faces are the ones that intersect + the plane. Faces are indexed in the following fashion: index i corresponds to + the face formed by vertices {0, 1, 2, 3} - {i}. For instance face 0 corresponds + to face {1, 2, 3} etc. + + The order of the indices is also constructed to follow the order of the edges + listed in the corresponding entry in kMarkingTetsEdgeTable. For instance: + + kMarchingTetsEdgeTable[0001] = {0, 3, 2, -1} + + Which maps to the edge list: + + {(0, 1), (0, 3), (2, 0)} + + The edge of the polygon connecting tet edge (0, 1) to (0, 3) lies on face + {0, 1, 3} thus the first element of kMarchingTetsFaceTable[0001] is face 2. + + The edge of the polygon connecting tet edge (0, 3) to (2, 0) lies on face + {0, 2, 3} thus the second element of kMarchingTetsFaceTable[0001] is face 1. + + The edge of the polygon connecting tet edge (2, 0) to (0, 1) lies on face + {0, 1, 2} thus the second element of kMarchingTetsFaceTable[0001] is face 3. + + Thus: + + kMarchingTetsFaceTable[0001] = {2, 1, 3, -1} + + A -1 is a sentinel value indicating no face encoding. The number of + intersecting face is equal to the index of the *first* -1 (with an implicit + logical -1 at index 4). + +*/ + +constexpr std::array, 16> kMarchingTetsFaceTable = { + /* bits 3210 */ + std::array{-1, -1, -1, -1}, /* 0000 */ + std::array{2, 1, 3, -1}, /* 0001 */ + std::array{3, 0, 2, -1}, /* 0010 */ + std::array{2, 1, 3, 0}, /* 0011 */ + std::array{3, 1, 0, -1}, /* 0100 */ + std::array{2, 1, 0, 3}, /* 0101 */ + std::array{3, 1, 0, 2}, /* 0110 */ + std::array{1, 0, 2, -1}, /* 0111 */ + std::array{2, 0, 1, -1}, /* 1000 */ + std::array{0, 1, 3, 2}, /* 1001 */ + std::array{0, 1, 2, 3}, /* 1010 */ + std::array{0, 1, 3, -1}, /* 1011 */ + std::array{3, 1, 2, 0}, /* 1100 */ + std::array{2, 0, 3, -1}, /* 1101 */ + std::array{3, 1, 2, -1}, /* 1110 */ + std::array{-1, -1, -1, -1} /* 1111 */}; // clang-format on + } // namespace template void SliceTetrahedronWithPlane(int tet_index, const VolumeMesh& mesh_M, const Plane& plane_M, std::vector>* polygon_vertices, - std::vector>* cut_edges) { + std::vector>* cut_edges, + std::vector* faces) { DRAKE_DEMAND(polygon_vertices != nullptr); + if (faces != nullptr) { + faces->clear(); + } + T distance[4]; // Bit encoding of the sign of signed-distance: v0, v1, v2, v3. int intersection_code = 0; @@ -78,15 +139,21 @@ void SliceTetrahedronWithPlane(int tet_index, const VolumeMesh& mesh_M, if (distance[i] > T(0)) intersection_code |= 1 << i; } - const std::array& intersected_edges = - kMarchingTetsTable[intersection_code]; - // No intersecting edges --> no intersection. - if (intersected_edges[0] == -1) return; + if (kMarchingTetsEdgeTable[intersection_code][0] == -1) return; + + // We have four candidate tet-edges and four candidate tet-faces, so we re-use + // `i` for indexing both. + for (int i = 0; i < 4; ++i) { + const int edge_index = kMarchingTetsEdgeTable[intersection_code][i]; + // When we encounter a -1 in the edge table, that marks the end of the edge + // list. There is always a corresponding -1 at the same index `i` in + // kMarchingTetsFaceTable. + if (edge_index == -1) { + DRAKE_ASSERT(kMarchingTetsFaceTable[intersection_code][i] == -1); + break; + } - for (int e = 0; e < 4; ++e) { - const int edge_index = intersected_edges[e]; - if (edge_index == -1) break; const TetrahedronEdge& tet_edge = kTetEdges[edge_index]; const int v0 = mesh_M.element(tet_index).vertex(tet_edge.first); const int v1 = mesh_M.element(tet_index).vertex(tet_edge.second); @@ -102,6 +169,9 @@ void SliceTetrahedronWithPlane(int tet_index, const VolumeMesh& mesh_M, const T t = d_v0 / (d_v0 - d_v1); const Vector3 p_MC = p_MV0 + t * (p_MV1 - p_MV0); polygon_vertices->push_back(p_MC); + if (faces != nullptr) { + faces->push_back(kMarchingTetsFaceTable[intersection_code][i]); + } if (cut_edges != nullptr) { const SortedPair mesh_edge{v0, v1}; cut_edges->push_back(mesh_edge); @@ -129,7 +199,7 @@ int SliceTetWithPlane( } const std::array& intersected_edges = - kMarchingTetsTable[intersection_code]; + kMarchingTetsEdgeTable[intersection_code]; // No intersecting edges --> no intersection. if (intersected_edges[0] == -1) return 0; diff --git a/geometry/proximity/mesh_plane_intersection.h b/geometry/proximity/mesh_plane_intersection.h index f61ccefb8b52..55bda5a8c351 100644 --- a/geometry/proximity/mesh_plane_intersection.h +++ b/geometry/proximity/mesh_plane_intersection.h @@ -49,7 +49,8 @@ template void SliceTetrahedronWithPlane( int tet_index, const VolumeMesh& mesh_M, const Plane& plane_M, std::vector>* polygon_vertices, - std::vector>* cut_edges = nullptr); + std::vector>* cut_edges = nullptr, + std::vector* faces = nullptr); /* Intersects a tetrahedron with a plane; the resulting polygon is passed into the provided MeshBuilder. diff --git a/geometry/proximity/test/field_intersection_test.cc b/geometry/proximity/test/field_intersection_test.cc index 2a2cd612fd4a..066ff9af8895 100644 --- a/geometry/proximity/test/field_intersection_test.cc +++ b/geometry/proximity/test/field_intersection_test.cc @@ -1,6 +1,7 @@ #include "drake/geometry/proximity/field_intersection.h" #include +#include #include #include @@ -19,6 +20,7 @@ namespace internal { namespace { using Eigen::Vector3d; +using hydroelastic::SoftMesh; using math::RigidTransformd; using math::RollPitchYawd; using math::RotationMatrixd; @@ -199,11 +201,12 @@ TEST_F(FieldIntersectionLowLevelTest, IntersectTetrahedra) { identity_X_MN, &plane_M); ASSERT_TRUE(success); - const std::vector polygon_M = IntersectTetrahedra( + const auto [polygon_M, faces] = IntersectTetrahedra( first_element_in_field0, field0_M_.mesh(), first_element_in_field1, field1_N_.mesh(), identity_X_MN, plane_M); ASSERT_EQ(polygon_M.size(), 8); + ASSERT_EQ(faces.size(), 8); // Use empirical tolerance 1e-14 meters. const double kEps = 1e-14; @@ -216,28 +219,41 @@ TEST_F(FieldIntersectionLowLevelTest, IntersectTetrahedra) { // ^ // | Pi = position of i-th vertex // | - // P7 1 P6 + // P5 1 P4 // | // | - // P0 0.5 P5 + // P6 0.5 P3 // | // | // +------+------0------+------+---------> +X // -1 -0.5 | 0.5 1 // | - // P1 -0.5 P4 + // P7 -0.5 P2 // | // | - // P2 -1 P3 - // - EXPECT_TRUE(CompareMatrices(polygon_M.at(0), Vector3d(-1, 0.5, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(1), Vector3d(-1, -0.5, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(2), Vector3d(-0.5, -1, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(3), Vector3d(0.5, -1, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(4), Vector3d(1, -0.5, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(5), Vector3d(1, 0.5, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(6), Vector3d(0.5, 1, 0), kEps)); - EXPECT_TRUE(CompareMatrices(polygon_M.at(7), Vector3d(-0.5, 1, 0), kEps)); + // P0 -1 P1 + + EXPECT_TRUE(CompareMatrices(polygon_M.at(0), Vector3d(-0.5, -1, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(1), Vector3d(0.5, -1, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(2), Vector3d(1, -0.5, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(3), Vector3d(1, 0.5, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(4), Vector3d(0.5, 1, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(5), Vector3d(-0.5, 1, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(6), Vector3d(-1, 0.5, 0), kEps)); + EXPECT_TRUE(CompareMatrices(polygon_M.at(7), Vector3d(-1, -0.5, 0), kEps)); + + // Verifies the expected intersected faces. The particular (cyclic) order of + // the faces is not strictly important. What is important is that all 4 faces + // of each tetrahedra appear, and that (by construction) the edges alternate + // intersection with a face of tet0 and a face of tet1. + EXPECT_EQ(faces.at(0), 2); // Face 2 of tet0 + EXPECT_EQ(faces.at(1), 6); // Face 2 of tet1 + EXPECT_EQ(faces.at(2), 1); // Face 1 of tet0 + EXPECT_EQ(faces.at(3), 5); // Face 1 of tet1 + EXPECT_EQ(faces.at(4), 3); // Face 3 of tet0 + EXPECT_EQ(faces.at(5), 7); // Face 3 of tet1 + EXPECT_EQ(faces.at(6), 0); // Face 0 of tet0 + EXPECT_EQ(faces.at(7), 4); // Face 0 of tet1 } // Move the equilibrium plane so far away that the above IntersectTetrahedra @@ -248,11 +264,12 @@ TEST_F(FieldIntersectionLowLevelTest, IntersectTetrahedra_NoIntersection) { // This plane is far away from the two tetrahedra. const Plane plane_M{Vector3d::UnitZ(), 5.0 * Vector3d::UnitZ()}; - const std::vector polygon_M = IntersectTetrahedra( + const auto [polygon_M, faces] = IntersectTetrahedra( first_element_in_mesh0, field0_M_.mesh(), first_element_in_mesh1, field1_N_.mesh(), RigidTransformd::Identity(), plane_M); EXPECT_EQ(polygon_M.size(), 0); + EXPECT_EQ(faces.size(), 0); } TEST_F(FieldIntersectionLowLevelTest, IsPlaneNormalAlongPressureGradient) { @@ -270,109 +287,241 @@ TEST_F(FieldIntersectionLowLevelTest, IsPlaneNormalAlongPressureGradient) { nhat_M_against_gradient, first_tetrahedron_in_field0, field0_M_)); } +// The tet-based and tri-based algorithms will generally produce the same +// results. The exception is if one mesh is completely within the other (such +// that their surfaces don't intersect). In that case, the tri-based algorithm +// may incorrectly report no contact. We say "may" because there is a +// possibility that some surface elements are sufficiently close to each other +// that the leaf node bounding volumes of their respective trees overlap. In +// this case the search algorithm of the tri-based algorithm *might* be seeded +// with a candidate pair that calculates a part of the contact surface (if the +// candidate tetrahedra actually overlap and the contact polygon is not culled +// based on the pressure gradients). Thus, only if the surfaces do not intersect +// AND all leaf nodes of the BVHs are disjoint, then no contact surface will be +// reported. This test quantifies that difference. +// Note: this is not a desirable behavior. +GTEST_TEST(VolumeIntersectionTest, FullyPenetrated) { + auto make_ball = [](double radius) { + const Sphere ball(radius); + auto mesh = + std::make_unique>(MakeSphereVolumeMesh( + ball, radius, TessellationStrategy::kSingleInteriorVertex)); + auto field = std::make_unique>( + MakeSpherePressureField(ball, mesh.get(), 1e5)); + return SoftMesh(std::move(mesh), std::move(field)); + }; + + SoftMesh big_sphere_M = make_ball(1.0); + SoftMesh small_sphere_N = make_ball(1.0 / 8.0); + + // Place the small sphere well within the big sphere, without surfaces + // touching. + const RigidTransformd X_MN(0.5 * Vector3d::UnitX()); + + std::array>, 2> surface_01_M; + std::array>, 2> + e_MN_M; + + VolumeIntersector, Obb>().IntersectFields( + big_sphere_M.pressure(), big_sphere_M.bvh(), small_sphere_N.pressure(), + small_sphere_N.bvh(), X_MN, &surface_01_M[0], &e_MN_M[0]); + + VolumeIntersector, Obb>().IntersectFields( + big_sphere_M.pressure(), big_sphere_M.surface_mesh_bvh(), + big_sphere_M.tri_to_tet(), big_sphere_M.mesh_topology(), + small_sphere_N.pressure(), small_sphere_N.surface_mesh_bvh(), + small_sphere_N.tri_to_tet(), small_sphere_N.mesh_topology(), X_MN, + &surface_01_M[1], &e_MN_M[1]); + + // Index 0 only uses tets. + EXPECT_NE(surface_01_M[0].get(), nullptr); + // Index 1 depends on intersection at the surface (and non-intersecting BVH + // leaf nodes). + EXPECT_EQ(surface_01_M[1].get(), nullptr); +} + class VolumeIntersectorTest : public ::testing::Test { public: - VolumeIntersectorTest() - : box_mesh0_M_(MakeBoxVolumeMeshWithMa(box_)), - box_field0_M_(MakeBoxPressureField(box_, &box_mesh0_M_, - kBoxElasitcModulus_)), - box_bvh0_M_(box_mesh0_M_), - // Get a mesh of an octahedron from a sphere specification by - // specifying very coarse resolution hint. - octahedron_mesh1_N_(MakeSphereVolumeMesh( + VolumeIntersectorTest() { + std::unique_ptr> mesh = + std::make_unique>( + MakeBoxVolumeMeshWithMa(box_)); + std::unique_ptr> field = + std::make_unique>( + MakeBoxPressureField(box_, mesh.get(), + kBoxElasitcModulus_)); + // Get a mesh of an octahedron from a sphere specification by + // specifying very coarse resolution hint. + std::unique_ptr> octahedron_mesh = + std::make_unique>(MakeSphereVolumeMesh( sphere_, 10 * sphere_.radius(), - TessellationStrategy::kSingleInteriorVertex)), - octahedron_field1_N_(MakeSpherePressureField( - sphere_, &octahedron_mesh1_N_, kOctahedronElasticModulus_)), - octahedron_bvh1_N_(octahedron_mesh1_N_) {} + TessellationStrategy::kSingleInteriorVertex)); + + std::unique_ptr> octahedron_field = + std::make_unique>( + MakeSpherePressureField(sphere_, octahedron_mesh.get(), + kOctahedronElasticModulus_)); + + box_M_ = SoftMesh(std::move(mesh), std::move(field)); + octahedron_N_ = + SoftMesh(std::move(octahedron_mesh), std::move(octahedron_field)); + } protected: void SetUp() override { - DRAKE_DEMAND(octahedron_mesh1_N_.num_elements() == 8); + DRAKE_DEMAND(octahedron_N_.mesh().num_elements() == 8); } // Geometry 0 and its field. const Box box_{0.06, 0.10, 0.14}; // 6cm-thick compliant pad. const double kBoxElasitcModulus_{1.0e5}; - const VolumeMesh box_mesh0_M_; - const VolumeMeshFieldLinear box_field0_M_; - const Bvh> box_bvh0_M_; + SoftMesh box_M_; // Geometry 1 and its field. const Sphere sphere_{0.03}; // 3cm-radius (6cm-diameter) finger tip. const double kOctahedronElasticModulus_{1.0e5}; - const VolumeMesh octahedron_mesh1_N_; - const VolumeMeshFieldLinear octahedron_field1_N_; - const Bvh> octahedron_bvh1_N_; + SoftMesh octahedron_N_; }; +// The two algorithms provided by VolumeIntersector only differ in the manner in +// which they find candidate tetrahedra pairs from the two MeshFieldLinear +// objects. Each end up calling CalcContactPolygon() with their candidate pairs. +// Rather than comparing contact surfaces directly -- which is cumbersome +// because we have to handle equivalent topology variants and floating point +// issues -- we infer equivalency by looking at the build history. The +// intersector stores the pair of tets which produce each element in the contact +// surface mesh. We'll simply confirm that the two sets tet-pairs match (and +// safely assume that they would produce the same surface). +template +bool ContactSurfacesAreEqual( + const VolumeIntersector& intersector_A, + const typename MeshBuilder::MeshType& surface_A, + const VolumeIntersector& intersector_B, + const typename MeshBuilder::MeshType& surface_B) { + if (surface_A.num_elements() != surface_B.num_elements()) { + return false; + } + + // When computing a triangulated contact surface, a single tet pair can + // produce several triangles, in which case the tet0/tet1 pair will be + // recorded multiple times. Thus we use a multiset to also check that the + // the number of surface elements produced by each tet pair visited in each + // algorithm also matches. + std::multiset> set_A, set_B; + for (int i = 0; i < surface_A.num_elements(); ++i) { + set_A.emplace(intersector_A.tet0_of_polygon(i), + intersector_A.tet1_of_polygon(i)); + set_B.emplace(intersector_B.tet0_of_polygon(i), + intersector_B.tet1_of_polygon(i)); + } + + return set_A == set_B; +} + TEST_F(VolumeIntersectorTest, IntersectFields) { const RigidTransformd X_MN = RigidTransformd(0.03 * Vector3d::UnitX()); + + // Compute the contact surface using both algorithms provided by + // VolumeIntersector and check that they produce identical contact surfaces. { SCOPED_TRACE("Use TriMeshBuilder."); - std::unique_ptr> surface_01_M; - std::unique_ptr> e_MN_M; - VolumeIntersector, Obb>().IntersectFields( - box_field0_M_, box_bvh0_M_, octahedron_field1_N_, octahedron_bvh1_N_, - X_MN, &surface_01_M, &e_MN_M); - - EXPECT_NE(surface_01_M.get(), nullptr); + std::array>, 2> surface_01_M; + std::array>, + 2> + e_MN_M; + std::array, Obb>, 2> intersector; + + intersector[0].IntersectFields( + box_M_.pressure(), box_M_.bvh(), octahedron_N_.pressure(), + octahedron_N_.bvh(), X_MN, &surface_01_M[0], &e_MN_M[0]); + + intersector[1].IntersectFields( + box_M_.pressure(), box_M_.surface_mesh_bvh(), box_M_.tri_to_tet(), + box_M_.mesh_topology(), octahedron_N_.pressure(), + octahedron_N_.surface_mesh_bvh(), octahedron_N_.tri_to_tet(), + octahedron_N_.mesh_topology(), X_MN, &surface_01_M[1], &e_MN_M[1]); + + ASSERT_NE(surface_01_M[0].get(), nullptr); + ASSERT_NE(surface_01_M[1].get(), nullptr); + EXPECT_TRUE(ContactSurfacesAreEqual(intersector[0], *surface_01_M[0], + intersector[1], *surface_01_M[1])); } { SCOPED_TRACE("Use PolyMeshBuilder."); - std::unique_ptr> surface_01_M; - std::unique_ptr> e_MN_M; - VolumeIntersector, Obb>().IntersectFields( - box_field0_M_, box_bvh0_M_, octahedron_field1_N_, octahedron_bvh1_N_, - X_MN, &surface_01_M, &e_MN_M); - - EXPECT_NE(surface_01_M.get(), nullptr); - - // Both types of MeshBuilder got the same pressure and gradient - // calculations, so we check only the PolyMeshBuilder. - - // TODO(DamrongGuoy) Add more rigorous check and documentation. Right now - // we do it empirically. If the order of tetrahedra in the input meshes - // change, the first vertex and the first polygon may not be the same as - // the ones we check below. Therefore, the expected pressure value and - // pressure gradients will change. - const double kRelativeTolerance = 1e-13; - // The hydroelastic modulus is in the order of 1e5 Pa. Empirically - // we determined that the first vertex of the contact surface has half of - // maximum pressure, i.e., 1e5 / 2 = 50 kPa = 5e4 Pa. - const double kExpectPressure = 5.0e4; - const double kPressureTolerance = kRelativeTolerance * kExpectPressure; - EXPECT_NEAR(e_MN_M->EvaluateAtVertex(0), kExpectPressure, - kPressureTolerance); + std::array>, 2> surface_01_M; + std::array>, + 2> + e_MN_M; + std::array, Obb>, 2> intersector; + + intersector[0].IntersectFields( + box_M_.pressure(), box_M_.bvh(), octahedron_N_.pressure(), + octahedron_N_.bvh(), X_MN, &surface_01_M[0], &e_MN_M[0]); + + intersector[1].IntersectFields( + box_M_.pressure(), box_M_.surface_mesh_bvh(), box_M_.tri_to_tet(), + box_M_.mesh_topology(), octahedron_N_.pressure(), + octahedron_N_.surface_mesh_bvh(), octahedron_N_.tri_to_tet(), + octahedron_N_.mesh_topology(), X_MN, &surface_01_M[1], &e_MN_M[1]); + + EXPECT_NE(surface_01_M[0].get(), nullptr); + EXPECT_NE(surface_01_M[1].get(), nullptr); + EXPECT_TRUE(ContactSurfacesAreEqual(intersector[0], *surface_01_M[0], + intersector[1], *surface_01_M[1])); } } // Smoke tests that AutoDiffXd can build. No checking on the values of -// derivatives. +// derivatives or whether the surfaces are equivalent. TEST_F(VolumeIntersectorTest, IntersectFieldsAutoDiffXd) { const math::RigidTransform X_MN(0.03 * Vector3::UnitX()); { SCOPED_TRACE("Use TriMeshBuilder."); - std::unique_ptr> surface_01_M; - std::unique_ptr> + std::array>, 2> + surface_01_M; + std::array< + std::unique_ptr>, + 2> e_MN_M; - VolumeIntersector, Obb>().IntersectFields( - box_field0_M_, box_bvh0_M_, octahedron_field1_N_, octahedron_bvh1_N_, - X_MN, &surface_01_M, &e_MN_M); + std::array, Obb>, 2> + intersector; + + intersector[0].IntersectFields( + box_M_.pressure(), box_M_.bvh(), octahedron_N_.pressure(), + octahedron_N_.bvh(), X_MN, &surface_01_M[0], &e_MN_M[0]); + + intersector[1].IntersectFields( + box_M_.pressure(), box_M_.surface_mesh_bvh(), box_M_.tri_to_tet(), + box_M_.mesh_topology(), octahedron_N_.pressure(), + octahedron_N_.surface_mesh_bvh(), octahedron_N_.tri_to_tet(), + octahedron_N_.mesh_topology(), X_MN, &surface_01_M[1], &e_MN_M[1]); - EXPECT_NE(surface_01_M.get(), nullptr); + EXPECT_NE(surface_01_M[0].get(), nullptr); + EXPECT_NE(surface_01_M[1].get(), nullptr); } { SCOPED_TRACE("Use PolyMeshBuilder."); - std::unique_ptr> surface_01_M; - std::unique_ptr> + std::array>, 2> surface_01_M; + std::array< + std::unique_ptr>, + 2> e_MN_M; - VolumeIntersector, Obb>().IntersectFields( - box_field0_M_, box_bvh0_M_, octahedron_field1_N_, octahedron_bvh1_N_, - X_MN, &surface_01_M, &e_MN_M); + std::array, Obb>, 2> + intersector; - EXPECT_NE(surface_01_M.get(), nullptr); + intersector[0].IntersectFields( + box_M_.pressure(), box_M_.bvh(), octahedron_N_.pressure(), + octahedron_N_.bvh(), X_MN, &surface_01_M[0], &e_MN_M[0]); + + intersector[1].IntersectFields( + box_M_.pressure(), box_M_.surface_mesh_bvh(), box_M_.tri_to_tet(), + box_M_.mesh_topology(), octahedron_N_.pressure(), + octahedron_N_.surface_mesh_bvh(), octahedron_N_.tri_to_tet(), + octahedron_N_.mesh_topology(), X_MN, &surface_01_M[1], &e_MN_M[1]); + + EXPECT_NE(surface_01_M[0].get(), nullptr); + EXPECT_NE(surface_01_M[1].get(), nullptr); } } @@ -380,15 +529,32 @@ TEST_F(VolumeIntersectorTest, IntersectFieldsAutoDiffXd) { // representative template argument. TEST_F(VolumeIntersectorTest, IntersectFieldsNoIntersection) { // 1 meter apart is well separated. - const RigidTransformd X_MN(Vector3d::UnitX()); - std::unique_ptr> surface_01_M; - std::unique_ptr> e_MN_M; - VolumeIntersector, Obb>().IntersectFields( - box_field0_M_, box_bvh0_M_, octahedron_field1_N_, octahedron_bvh1_N_, - X_MN, &surface_01_M, &e_MN_M); + { + SCOPED_TRACE("Use VolumeMesh BVH."); + const RigidTransformd X_MN(Vector3d::UnitX()); + std::unique_ptr> surface_01_M; + std::unique_ptr> e_MN_M; + VolumeIntersector, Obb>().IntersectFields( + box_M_.pressure(), box_M_.bvh(), octahedron_N_.pressure(), + octahedron_N_.bvh(), X_MN, &surface_01_M, &e_MN_M); + + EXPECT_EQ(surface_01_M.get(), nullptr); + EXPECT_EQ(e_MN_M.get(), nullptr); + } + { + SCOPED_TRACE("Use surface TriangleMesh BVH."); + const RigidTransformd X_MN(Vector3d::UnitX()); + std::unique_ptr> surface_01_M; + std::unique_ptr> e_MN_M; + VolumeIntersector, Obb>().IntersectFields( + box_M_.pressure(), box_M_.surface_mesh_bvh(), box_M_.tri_to_tet(), + box_M_.mesh_topology(), octahedron_N_.pressure(), + octahedron_N_.surface_mesh_bvh(), octahedron_N_.tri_to_tet(), + octahedron_N_.mesh_topology(), X_MN, &surface_01_M, &e_MN_M); - EXPECT_EQ(surface_01_M.get(), nullptr); - EXPECT_EQ(e_MN_M.get(), nullptr); + EXPECT_EQ(surface_01_M.get(), nullptr); + EXPECT_EQ(e_MN_M.get(), nullptr); + } } TEST_F(VolumeIntersectorTest, IntersectCompliantVolumes) { @@ -397,43 +563,48 @@ TEST_F(VolumeIntersectorTest, IntersectCompliantVolumes) { const RigidTransformd X_WM = RigidTransformd::Identity(); const RigidTransformd X_WN(0.03 * Vector3d::UnitX()); { - SCOPED_TRACE("Triangle contact surface."); + SCOPED_TRACE("Triangle contact surface with volume BVH."); std::unique_ptr> contact_patch_W; HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(first_id, box_field0_M_, box_bvh0_M_, X_WM, - second_id, octahedron_field1_N_, - octahedron_bvh1_N_, X_WN, &contact_patch_W); + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W, + false /* use_surfaces */); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), HydroelasticContactRepresentation::kTriangle); } { - SCOPED_TRACE("Polygon contact surface."); + SCOPED_TRACE("Triangle contact surface with surface BVH."); + std::unique_ptr> contact_patch_W; + HydroelasticVolumeIntersector>() + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W, + true /* use_surfaces */); + ASSERT_NE(contact_patch_W.get(), nullptr); + EXPECT_EQ(contact_patch_W->representation(), + HydroelasticContactRepresentation::kTriangle); + } + { + SCOPED_TRACE("Polygon contact surface with volume BVH."); std::unique_ptr> contact_patch_W; HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(first_id, box_field0_M_, box_bvh0_M_, X_WM, - second_id, octahedron_field1_N_, - octahedron_bvh1_N_, X_WN, &contact_patch_W); + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W, + false /* use_surfaces */); + ASSERT_NE(contact_patch_W.get(), nullptr); + EXPECT_EQ(contact_patch_W->representation(), + HydroelasticContactRepresentation::kPolygon); + } + { + SCOPED_TRACE("Polygon contact surface with surface BVH."); + std::unique_ptr> contact_patch_W; + HydroelasticVolumeIntersector>() + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W, + true /* use_surfaces */); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), HydroelasticContactRepresentation::kPolygon); - - // The hydroelastic modulus is in the order of 100 kPa, and the size of - // the geometries are in the order of centimeters. Therefore, the - // pressure gradient is in the order of 100 kPa / 1 cm = 10 MPa/meter - // = 1e7 Pa/m. - const double kExpectGradientOrderOfMagnitude = 1e7; - const double kRelativeTolerance = 1e-13; - const double kGradientTolerance = - kRelativeTolerance * kExpectGradientOrderOfMagnitude; - const Vector3d expect_grad_e0_M = - kExpectGradientOrderOfMagnitude * Vector3d(-1 / 3.0, 0, 0); - EXPECT_TRUE(CompareMatrices(contact_patch_W->EvaluateGradE_M_W(0), - expect_grad_e0_M, kGradientTolerance)); - const Vector3d expect_grad_e1_M = - kExpectGradientOrderOfMagnitude * Vector3d(1 / 3.0, -1 / 3.0, -1 / 3.0); - EXPECT_TRUE(CompareMatrices(contact_patch_W->EvaluateGradE_N_W(0), - expect_grad_e1_M, kGradientTolerance)); } } @@ -450,9 +621,8 @@ TEST_F(VolumeIntersectorTest, IntersectCompliantVolumesAutoDiffXd) { SCOPED_TRACE("Triangle contact surface."); std::unique_ptr> contact_patch_W; HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(first_id, box_field0_M_, box_bvh0_M_, X_WM, - second_id, octahedron_field1_N_, - octahedron_bvh1_N_, X_WN, &contact_patch_W); + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), HydroelasticContactRepresentation::kTriangle); @@ -461,9 +631,8 @@ TEST_F(VolumeIntersectorTest, IntersectCompliantVolumesAutoDiffXd) { SCOPED_TRACE("Polygon contact surface."); std::unique_ptr> contact_patch_W; HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(first_id, box_field0_M_, box_bvh0_M_, X_WM, - second_id, octahedron_field1_N_, - octahedron_bvh1_N_, X_WN, &contact_patch_W); + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), HydroelasticContactRepresentation::kPolygon); @@ -481,9 +650,8 @@ TEST_F(VolumeIntersectorTest, IntersectCompliantVolumesNoIntersection) { std::unique_ptr> contact_patch_W; HydroelasticVolumeIntersector>() - .IntersectCompliantVolumes(first_id, box_field0_M_, box_bvh0_M_, X_WM, - second_id, octahedron_field1_N_, - octahedron_bvh1_N_, X_WN, &contact_patch_W); + .IntersectCompliantVolumes(first_id, box_M_, X_WM, second_id, + octahedron_N_, X_WN, &contact_patch_W); EXPECT_EQ(contact_patch_W.get(), nullptr); } @@ -496,8 +664,7 @@ TEST_F(VolumeIntersectorTest, ComputeContactSurfaceFromCompliantVolumes) { SCOPED_TRACE("Request triangles."); std::unique_ptr> contact_patch_W = ComputeContactSurfaceFromCompliantVolumes( - first_id, box_field0_M_, box_bvh0_M_, X_WM, second_id, - octahedron_field1_N_, octahedron_bvh1_N_, X_WN, + first_id, box_M_, X_WM, second_id, octahedron_N_, X_WN, HydroelasticContactRepresentation::kTriangle); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), @@ -507,8 +674,7 @@ TEST_F(VolumeIntersectorTest, ComputeContactSurfaceFromCompliantVolumes) { SCOPED_TRACE("Request polygons."); std::unique_ptr> contact_patch_W = ComputeContactSurfaceFromCompliantVolumes( - first_id, box_field0_M_, box_bvh0_M_, X_WM, second_id, - octahedron_field1_N_, octahedron_bvh1_N_, X_WN, + first_id, box_M_, X_WM, second_id, octahedron_N_, X_WN, HydroelasticContactRepresentation::kPolygon); ASSERT_NE(contact_patch_W.get(), nullptr); EXPECT_EQ(contact_patch_W->representation(), @@ -530,16 +696,14 @@ TEST_F(VolumeIntersectorTest, SCOPED_TRACE("Request triangles."); std::unique_ptr> contact_patch_W = ComputeContactSurfaceFromCompliantVolumes( - first_id, box_field0_M_, box_bvh0_M_, X_WM, second_id, - octahedron_field1_N_, octahedron_bvh1_N_, X_WN, + first_id, box_M_, X_WM, second_id, octahedron_N_, X_WN, HydroelasticContactRepresentation::kTriangle); } { SCOPED_TRACE("Request polygons."); std::unique_ptr> contact_patch_W = ComputeContactSurfaceFromCompliantVolumes( - first_id, box_field0_M_, box_bvh0_M_, X_WM, second_id, - octahedron_field1_N_, octahedron_bvh1_N_, X_WN, + first_id, box_M_, X_WM, second_id, octahedron_N_, X_WN, HydroelasticContactRepresentation::kPolygon); } } diff --git a/geometry/proximity/test/hydroelastic_calculator_test.cc b/geometry/proximity/test/hydroelastic_calculator_test.cc index 32fff1b1c0fb..d32618499a1b 100644 --- a/geometry/proximity/test/hydroelastic_calculator_test.cc +++ b/geometry/proximity/test/hydroelastic_calculator_test.cc @@ -592,6 +592,14 @@ TYPED_TEST(MaybeMakeContactSurfaceTests, BothCompliantNonHalfSpace) { scene.calculator().MaybeMakeContactSurface(scene.id_A(), scene.id_B()); EXPECT_EQ(result, ContactSurfaceResult::kCalculated); EXPECT_NE(surface, nullptr); + + // Supplying ids in reversed order yields the same result, because the ids + // get put in order before constructing the surface. + auto [result_reversed, surface_reversed] = + scene.calculator().MaybeMakeContactSurface(scene.id_B(), scene.id_A()); + EXPECT_EQ(result_reversed, ContactSurfaceResult::kCalculated); + EXPECT_NE(surface_reversed, nullptr); + EXPECT_EQ(surface->id_M(), surface_reversed->id_M()); } TYPED_TEST(MaybeMakeContactSurfaceTests, BothHalfSpace) {