From 4825f134fd77c108def9b03903181f846b1e89ad 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 | 307 ++++++++++-- geometry/proximity/field_intersection.h | 207 +++++--- geometry/proximity/hydroelastic_calculator.cc | 10 +- geometry/proximity/mesh_plane_intersection.cc | 81 ++- geometry/proximity/mesh_plane_intersection.h | 3 +- .../proximity/test/field_intersection_test.cc | 461 +++++++++++++----- .../test/hydroelastic_calculator_test.cc | 8 + 8 files changed, 803 insertions(+), 278 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..bb1997ecae39 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" @@ -24,13 +25,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 +66,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 +84,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 +94,21 @@ 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); + std::vector* faces = &(face_buffer[0]); + polygon_M->clear(); + faces->clear(); + SliceTetrahedronWithPlane(element0, mesh0_M, equilibrium_plane_M, polygon_M, + nullptr /* cut_edges */, faces); + RemoveNearlyDuplicateVertices(polygon_M); - // Null polygon - if (polygon_M->size() < 3) return {}; + if (polygon_M->size() < 3) { + return {}; + } // Positions of vertices of tetrahedral element1 in mesh1_N expressed in // frame M. @@ -103,31 +117,78 @@ 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* faces_in = faces; + std::vector* faces_out = &(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) { + out_M->clear(); + faces_out->clear(); + + // 'face' corresponds the 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(*in_M); + + for (int i = 0; i < size; ++i) { + distances[i] = half_space_M.CalcSignedDistance((*in_M)[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) { + out_M->push_back((*in_M)[i]); + faces_out->push_back((*faces_in)[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; + out_M->push_back(wa * (*in_M)[i] + wb * (*in_M)[j]); + faces_out->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 faces_in[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; + out_M->push_back(wa * (*in_M)[i] + wb * (*in_M)[j]); + faces_out->push_back((*faces_in)[i]); + } } std::swap(in_M, out_M); + std::swap(faces_in, faces_out); + + // If we've clipped off the entire polygon, return empty. + if (ssize(*in_M) == 0) { + return {}; + } } + polygon_M = in_M; + faces = faces_in; + + RemoveNearlyDuplicateVertices(polygon_M); + if (polygon_M->size() < 3) { + return {}; + } - return *polygon_M; + return std::make_pair(*polygon_M, *faces); } template @@ -170,6 +231,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 +246,134 @@ 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. + auto pair_hash = [](const std::pair& p) -> std::size_t { + return std::hash()(p.first) ^ std::hash()(p.second); + }; + std::unordered_set, decltype(pair_hash)> checked_pairs( + 8, pair_hash); + + std::queue> candidate_tetrahedra; + auto callback = [&candidate_tetrahedra, &checked_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 (!checked_pairs.contains(tet_pair)) { + candidate_tetrahedra.push(tet_pair); + checked_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 (checked_pairs.contains(neighbor_pair)) { + continue; + } + + candidate_tetrahedra.push(neighbor_pair); + checked_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 (checked_pairs.contains(neighbor_pair)) { + continue; + } + + candidate_tetrahedra.push(neighbor_pair); + checked_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 +383,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 +421,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_topology) { 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_topology) { + 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 +469,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 +489,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, + true /* use_topology */); } 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, + true /* use_topology */); } return contact_surface_W; } diff --git a/geometry/proximity/field_intersection.h b/geometry/proximity/field_intersection.h index f9539b8f4247..b14c2433fa89 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,84 @@ 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). + + Hydroelastics and deformables use these functions 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 + tetrahedral meshes with scalar fields using a BVH of the volume elements of + the meshes. 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>& bvh0_M, + const Bvh>& bvh_M, const VolumeMeshFieldLinear& field1_N, - const Bvh>& bvh1_N, + const Bvh>& bvh_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); + + /* Overload of the above version using a BVH of the surface triangles of + the volume meshes and neighbor traversal. It takes the same parameters + as the above version except that the BVHs are associated with the surface + triangles, and it requests these extra parameters to accelerate + computation: + + @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>& bvh_M, + const std::vector& tri_to_tet_M, + const VolumeMeshTopology& mesh_topology_M, + const VolumeMeshFieldLinear& field1_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_M, + std::unique_ptr* e_M); + /* @} */ /* Returns the index of tetrahedron in the first mesh containing the i-th contact polygon. @@ -181,15 +242,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 +283,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_topology If true, uses the version of + VolumeIntersector::IntersectFields() that makes use + of the topology of the geometry's 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_topology = 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 +331,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..e2db1a920b1c 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 kMarchingTetsEdgeTable 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 to 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,14 +139,11 @@ 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; for (int e = 0; e < 4; ++e) { - const int edge_index = intersected_edges[e]; + const int edge_index = kMarchingTetsEdgeTable[intersection_code][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); @@ -102,6 +160,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][e]); + } if (cut_edges != nullptr) { const SortedPair mesh_edge{v0, v1}; cut_edges->push_back(mesh_edge); @@ -129,7 +190,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..caf5a22ba853 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,80 +287,220 @@ TEST_F(FieldIntersectionLowLevelTest, IsPlaneNormalAlongPressureGradient) { nhat_M_against_gradient, first_tetrahedron_in_field0, field0_M_)); } +// Special cases that we may or may not care. +// +// This unit test suite shows that in special cases, the two versions of +// IntersectFields() give different results. One version can sense the +// contact, but the other version misses the contact. +// +// They are special cases that may or may not affect users depending on their +// applications. +class VolumeIntersectorTestSpecialCases : public ::testing::Test { + public: + VolumeIntersectorTestSpecialCases() { + std::unique_ptr> big_mesh = + std::make_unique>(MakeSphereVolumeMesh( + big_sphere_, big_sphere_.radius(), + TessellationStrategy::kSingleInteriorVertex)); + std::unique_ptr> big_field = + std::make_unique>( + MakeSpherePressureField(big_sphere_, big_mesh.get(), + kElasticModulus_)); + + std::unique_ptr> small_mesh = + std::make_unique>(MakeSphereVolumeMesh( + small_sphere_, small_sphere_.radius(), + TessellationStrategy::kSingleInteriorVertex)); + + std::unique_ptr> small_field = + std::make_unique>( + MakeSpherePressureField(small_sphere_, small_mesh.get(), + kElasticModulus_)); + + big_sphere_M_ = SoftMesh(std::move(big_mesh), std::move(big_field)); + small_sphere_N_ = SoftMesh(std::move(small_mesh), std::move(small_field)); + } + + protected: + void SetUp() override { + // The tessellation needs to be fine enough that the BVH leaf node + // (three triangles for TriangleSurfaceMesh and one tetrahedron for + // VolumeMesh) can do what we want, i.e., BVH(TriangleSurfaceMesh) and + // BVH(VolumeMesh) report empty and non-empty overlaps, respectively. + ASSERT_EQ(small_sphere_N_.mesh().num_elements(), 32); + } + + // Geometry 0 and its field. + const Sphere big_sphere_{0.20}; + SoftMesh big_sphere_M_; + + // Geometry 1 and its field. + const Sphere small_sphere_{0.03}; + SoftMesh small_sphere_N_; + + const double kElasticModulus_{1.0e5}; +}; + +TEST_F(VolumeIntersectorTestSpecialCases, IntersectFields) { + // The small sphere is completely inside the big sphere. It may or may not + // be an interesting case for users. + // + // The penetration is deep enough that the two BVHs of the two surface + // meshes miss the contact. + const RigidTransformd X_MN = RigidTransformd(0.10 * Vector3d::UnitX()); + + // Compute the contact surface using both algorithms provided by + // VolumeIntersector and confirm that they produce different contact + // surfaces: an empty one and a non-empty one. + { + SCOPED_TRACE("Use PolyMeshBuilder."); + std::array>, 2> surface_01_M; + std::array>, + 2> + e_MN_M; + std::array, Obb>, 2> intersector; + + intersector[0].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]); + + intersector[1].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]); + + // The first version of IntersectFields() can sense the contact. + EXPECT_NE(surface_01_M[0].get(), nullptr); + // The second version of IntersectFields() cannot. + 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. +// Therefore, if we can verify that each algorithm makes the same set of +// calls to CalcContactPolygon(), then the computed surfaces must be equivalent +// (up to a shuffling of indexes). CalcContactPolygon() records the tet pairs +// for each polygon it computes in tet0/1_of_polygon so we verify by checking +// that the multiset of tet pairs recorded is identical. +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; + } + + 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])); } } @@ -354,25 +511,54 @@ TEST_F(VolumeIntersectorTest, IntersectFieldsAutoDiffXd) { 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); - - EXPECT_NE(surface_01_M.get(), nullptr); + 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])); } { 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); - - EXPECT_NE(surface_01_M.get(), nullptr); + 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])); } } @@ -380,15 +566,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); - - EXPECT_EQ(surface_01_M.get(), nullptr); - EXPECT_EQ(e_MN_M.get(), nullptr); + { + 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); + } } TEST_F(VolumeIntersectorTest, IntersectCompliantVolumes) { @@ -397,43 +600,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_topology */); 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_topology */); + 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_topology */); + 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_topology */); 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 +658,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 +668,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 +687,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 +701,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 +711,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 +733,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) {