[geometry] Add plane-tet intersection (RobotLocomotion#12802)

This is the atomic operation to support rigid half space vs soft volume
mesh intersection. It robustly intersects a single tet with a plane in
such a way that no redundant edge-plane intersection tests are performed
and (as a by-product) doesn't introduce duplicate vertices in the resulting
contact surface mesh.

geometry/proximity/BUILD.bazel

@@ -34,6 +34,7 @@ drake_cc_package_library(
         ":mesh_field",
         ":mesh_half_space_intersection",
         ":mesh_intersection",
+        ":mesh_plane_intersection",
         ":mesh_to_vtk",
         ":obj_to_surface_mesh",
         ":penetration_as_point_pair_callback",
@@ -328,6 +329,20 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "mesh_plane_intersection",
+    srcs = ["mesh_plane_intersection.cc"],
+    hdrs = ["mesh_plane_intersection.h"],
+    deps = [
+        ":contact_surface_utility",
+        ":mesh_field",
+        ":plane",
+        ":volume_mesh",
+        "//geometry/query_results:contact_surface",
+        "//math:geometric_transform",
+    ],
+)
+
 drake_cc_library(
     name = "mesh_to_vtk",
     srcs = ["mesh_to_vtk.cc"],
@@ -637,6 +652,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "mesh_plane_intersection_test",
+    deps = [
+        ":contact_surface_utility",
+        ":mesh_plane_intersection",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+    ],
+)
+
 drake_cc_googletest(
     name = "mesh_to_vtk_test",
     deps = [

geometry/proximity/mesh_plane_intersection.cc

@@ -0,0 +1,158 @@
+#include "drake/geometry/proximity/mesh_plane_intersection.h"
+
+#include <array>
+#include <utility>
+
+#include "drake/geometry/proximity/contact_surface_utility.h"
+#include "drake/geometry/proximity/volume_mesh.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+namespace {
+
+/* This table essentially assigns an index to each edge in the tetrahedron.
+ Each edge is represented by its pair of vertex indexes. */
+using TetrahedronEdge = std::pair<int, int>;
+constexpr std::array<std::pair<int, int>, 6> kTetEdges = {
+    // base formed by vertices 0, 1, 2.
+    TetrahedronEdge{0, 1}, TetrahedronEdge{1, 2}, TetrahedronEdge{2, 0},
+    // 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
+ 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.
+ 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
+ listed edge's intersection vertices in the array order has a right-handed
+ normal pointing in the direction of the plane's normal. The accompanying
+ unit tests verify this.
+
+ A -1 is a sentinel value indicating no edge encoding. The number of
+ intersecting edges is equal to the index of the *first* -1 (with an implicit
+ logical -1 at index 4).  */
+constexpr std::array<std::array<int, 4>, 16> kMarchingTetsTable = {
+                                /* bits    3210 */
+    std::array<int, 4>{-1, -1, -1, -1}, /* 0000 */
+    std::array<int, 4>{0, 3, 2, -1},    /* 0001 */
+    std::array<int, 4>{0, 1, 4, -1},    /* 0010 */
+    std::array<int, 4>{4, 3, 2, 1},     /* 0011 */
+    std::array<int, 4>{1, 2, 5, -1},    /* 0100 */
+    std::array<int, 4>{0, 3, 5, 1},     /* 0101 */
+    std::array<int, 4>{0, 2, 5, 4},     /* 0110 */
+    std::array<int, 4>{3, 5, 4, -1},    /* 0111 */
+    std::array<int, 4>{3, 4, 5, -1},    /* 1000 */
+    std::array<int, 4>{4, 5, 2, 0},     /* 1001 */
+    std::array<int, 4>{1, 5, 3, 0},     /* 1010 */
+    std::array<int, 4>{1, 5, 2, -1},    /* 1011 */
+    std::array<int, 4>{1, 2, 3, 4},     /* 1100 */
+    std::array<int, 4>{0, 4, 1, -1},    /* 1101 */
+    std::array<int, 4>{0, 2, 3, -1},    /* 1110 */
+    std::array<int, 4>{-1, -1, -1, -1}  /* 1111 */};
+
+}  // namespace
+
+template <typename T>
+void SliceTetWithPlane(
+    VolumeElementIndex tet_index,
+    const VolumeMeshField<double, double>& field_M, const Plane<T>& plane_M,
+    const math::RigidTransform<T>& X_WM, std::vector<SurfaceFace>* faces,
+    std::vector<SurfaceVertex<T>>* vertices_W, std::vector<T>* surface_e,
+    std::unordered_map<SortedPair<VolumeVertexIndex>, SurfaceVertexIndex>*
+    cut_edges) {
+  const VolumeMesh<double>& mesh_M = field_M.mesh();
+
+  T distance[4];
+  // Bit encoding of the sign of signed-distance: v0, v1, v2, v3.
+  int intersection_code = 0;
+  for (int i = 0; i < 4; ++i) {
+    const VolumeVertexIndex v = mesh_M.element(tet_index).vertex(i);
+    distance[i] = plane_M.CalcSignedDistance(mesh_M.vertex(v).r_MV());
+    if (distance[i] > T(0)) intersection_code |= 1 << i;
+  }
+
+  const std::array<int, 4>& intersected_edges =
+      kMarchingTetsTable[intersection_code];
+
+  // No intersecting edges --> no intersection.
+  if (intersected_edges[0] == -1) return;
+
+  // Indices of the new polygon's vertices in vertices_W. There can be, at
+  // most, four due to the intersection with the plane.
+  int num_intersections = 0;
+  SurfaceVertexIndex face_verts[4];
+  for (int e = 0; e < 4; ++e) {
+    const int edge_index = intersected_edges[e];
+    if (edge_index == -1) break;
+    ++num_intersections;
+    const TetrahedronEdge& tet_edge = kTetEdges[edge_index];
+    const VolumeVertexIndex v0 =
+        mesh_M.element(tet_index).vertex(tet_edge.first);
+    const VolumeVertexIndex v1 =
+        mesh_M.element(tet_index).vertex(tet_edge.second);
+    const SortedPair<VolumeVertexIndex> mesh_edge{v0, v1};
+
+    auto iter = cut_edges->find(mesh_edge);
+    if (iter != cut_edges->end()) {
+      // Result has already been computed.
+      face_verts[e] = iter->second;
+    } else {
+      // Need to compute the result; but we already know that this edge
+      // intersects the plane based on the signed distances of its two
+      // vertices.
+      SurfaceVertexIndex new_index{static_cast<int>(vertices_W->size())};
+      const Vector3<double>& p_MV0 = mesh_M.vertex(v0).r_MV();
+      const Vector3<double>& p_MV1 = mesh_M.vertex(v1).r_MV();
+      const T d_v0 = distance[tet_edge.first];
+      const T d_v1 = distance[tet_edge.second];
+      // Note: It should be impossible for the denominator to be zero. By
+      // definition, this is an edge that is split by the plane; they can't
+      // both have the same value. More particularly, one must be strictly
+      // positive the other must be strictly non-positive.
+      const T t = d_v0 / (d_v0 - d_v1);
+      const Vector3<double> p_MC = p_MV0 + t * (p_MV1 - p_MV0);
+      vertices_W->emplace_back(X_WM * p_MC);
+      const double e0 = field_M.EvaluateAtVertex(v0);
+      const double e1 = field_M.EvaluateAtVertex(v1);
+      surface_e->emplace_back(e0 + t * (e1 - e0));
+      (*cut_edges)[mesh_edge] = new_index;
+      face_verts[e] = new_index;
+    }
+  }
+
+  // TODO(SeanCurtis-TRI): Consider an overload for AddPolygonToMeshData that
+  //  does not require copying the indices.
+  const std::vector<SurfaceVertexIndex> polygon(
+      face_verts, face_verts + num_intersections);
+  // Because the vertices are measured and expressed in the world frame, we
+  // need to provide a normal expressed in the same.
+  const Vector3<T> nhat_W = X_WM.rotation() * plane_M.normal();
+  AddPolygonToMeshData(polygon, nhat_W, faces, vertices_W);
+  // TODO(SeanCurtis-TRI): This would be faster/cheaper if I used the same
+  //  area weights to compute the resulting pressure at the centroid as I do
+  //  to actually *compute* the centroid. Make an overload of
+  //  AddPolygonToMeshData that also takes the pressures.
+  const Vector3<T> p_MC = X_WM.inverse() * vertices_W->back().r_MV();
+  surface_e->emplace_back(field_M.EvaluateCartesian(tet_index, p_MC));
+}
+
+template void SliceTetWithPlane<double>(
+    VolumeElementIndex tet_index,
+    const VolumeMeshField<double, double>& field_M,
+    const Plane<double>& plane_M, const math::RigidTransformd& X_WM,
+    std::vector<SurfaceFace>* faces,
+    std::vector<SurfaceVertex<double>>* vertices_W,
+    std::vector<double>* surface_e,
+    std::unordered_map<SortedPair<VolumeVertexIndex>, SurfaceVertexIndex>*
+        cut_edges);
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/mesh_plane_intersection.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <unordered_map>
+#include <vector>
+
+#include "drake/common/sorted_pair.h"
+#include "drake/geometry/proximity/plane.h"
+#include "drake/geometry/proximity/volume_mesh_field.h"
+#include "drake/math/rigid_transform.h"
+
+namespace drake {
+namespace geometry {
+namespace internal {
+
+/* Intersects a tetrahedron with a plane, storing the result in the provided
+ collections.
+
+ This method constructs a mesh by a sequence of invocations. It guarantees
+ the output surface mesh has the same topological coherency as the input mesh.
+ For example, if the plane cuts through a tetrahedron edge e at vertex v, then
+ each of the tetrahedra incident to that edge will be cut into a polygon which
+ includes the same vertex v; no duplicate vertices will be introduced.
+
+ This is accomplished by storing the edges that have already been evaluated.
+ Call after call, the cached set of intersected edges grows and subsequent
+ calls look up edges in the cache to see if the plane-edge intersection has
+ already been accounted for.
+
+ The unique vertices are written to `vertices_W`, the unique triangles are
+ written to `faces`, and the pressure values at each intersection vertex are
+ written to `surface_e`.
+
+ The face vertices are ordered such that the normal implied by their winding
+ points in the direction of the plane's normal.
+
+ After each call the following invariants are maintained:
+
+   - `vertices_W.size() == surface_e.size()`
+   - `i ∈ [0, N) ∀ i` referenced in `faces` (and `N = vertices_W.size()`).
+   - The iᵗʰ entry in `surface_e` is the pressure at the position for the iᵗʰ
+     vertex in `vertices_W`.
+   - Every vertex position in `vertices_W` that represents an intersection of
+     edge and plane can be found in `cut_edges`. All remaining vertex
+     positions are the centroids of the intersected polygon.
+   - No intersection guarantees no changes to _any_ of the out parameters.
+
+ @param[in] tet_index       The index of the tetrahedron to attempt to
+                            intersect.
+ @param[in] field_M         The volume mesh field (and mesh) containing the
+                            tetrahedra to intersect. The vertex positions are
+                            all measured and expressed in Frame M.
+ @param[in] plane_M         The definition of a plane measured and expressed
+                            in Frame M.
+ @param[in] X_WM            The relative pose between the mesh frame M and the
+                            world frame W.
+ @param[in,out] faces       The triangles (defined by triples of vertex
+                            indices) forming the intersection mesh so far.
+ @param[in,out] vertices_W  The vertex positions for the intersecting mesh,
+                            measured and expressed in Frame W.
+ @param[in,out] surface_e   The per-vertex field values. Upon returning,
+                            surface_e.size() == vertices_W.size() is true.
+ @param[in,out] cut_edges   The cache of volume mesh edges that have already
+                            been cut and the surface mesh vertex associated
+                            with it.
+ @pre `tet_index` lies in the range `[0, field_M.mesh().num_elements())`.
+ */
+template <typename T>
+void SliceTetWithPlane(
+    VolumeElementIndex tet_index,
+    const VolumeMeshField<double, double>& field_M, const Plane<T>& plane_M,
+    const math::RigidTransform<T>& X_WM, std::vector<SurfaceFace>* faces,
+    std::vector<SurfaceVertex<T>>* vertices_W, std::vector<T>* surface_e,
+    std::unordered_map<SortedPair<VolumeVertexIndex>, SurfaceVertexIndex>*
+        cut_edges);
+
+}  // namespace internal
+}  // namespace geometry
+}  // namespace drake

geometry/proximity/plane.h

@@ -26,7 +26,8 @@ class Plane {
        expressed in frame F (the `n̂` in the implicit equation).
    @param displacement
        The signed distance from F's origin to the half space boundary (the `d`
-       term in the implicit equation).
+       term in the implicit equation). E.g., for a point p_FP on the plane,
+       `displacement = nhat_F.dot(p_FP)`.
    @pre
        ‖nhat_F‖₂ = 1.
    */