Mesh/half space public interface (RobotLocomotion#12029)

geometry/proximity/BUILD.bazel

@@ -114,6 +114,7 @@ drake_cc_library(
         "mesh_half_space_intersection.h",
     ],
     deps = [
+        ":mesh_intersection",
         ":surface_mesh",
         "//common",
     ],

geometry/proximity/mesh_half_space_intersection.h

@@ -4,10 +4,12 @@
 #include <cmath>
 #include <limits>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 #include "drake/common/eigen_types.h"
 #include "drake/common/sorted_pair.h"
+#include "drake/geometry/proximity/mesh_intersection.h"
 #include "drake/geometry/proximity/surface_mesh.h"
 
 namespace drake {
@@ -111,11 +113,7 @@ SurfaceVertexIndex GetVertexAddIfNeeded(
  @param vertices_F the vertices from the input mesh as position vectors measured
         and expressed in Frame F.
  @param triangle a single face from the input mesh.
- @param half_space_normal_F the outward facing surface normal to the half space,
-        expressed in Frame F.
- @param half_space_constant the half space constant d, defined as
-        nᵀx = d for any point x that lies on the boundary of the half space and
-        given the surface normal to the half space n.
+ @param half_space_F the half space with normal to surface expressed in Frame F.
  @param[in,out] new_vertices_F the accumulator for all of the vertices in the
                 intersecting mesh. It should be empty to start and will
                 gradually accumulate all of the vertices with each subsequent
@@ -146,8 +144,9 @@ SurfaceVertexIndex GetVertexAddIfNeeded(
 template <typename T>
 void ConstructTriangleHalfspaceIntersectionPolygon(
     const std::vector<SurfaceVertex<T>>& vertices_F,
-    const SurfaceFace& triangle, const Vector3<T>& half_space_normal_F,
-    const T& half_space_constant, std::vector<SurfaceVertex<T>>* new_vertices_F,
+    const SurfaceFace& triangle,
+    const mesh_intersection::HalfSpace<T>& half_space_F,
+    std::vector<SurfaceVertex<T>>* new_vertices_F,
     std::vector<SurfaceFace>* new_faces,
     std::unordered_map<SurfaceVertexIndex, SurfaceVertexIndex>*
         vertices_to_newly_created_vertices,
@@ -176,8 +175,8 @@ void ConstructTriangleHalfspaceIntersectionPolygon(
   T s[3];
   int num_positive = 0;
   for (int i = 0; i < 3; ++i) {
-    s[i] = half_space_normal_F.dot(vertices_F[triangle.vertex(i)].r_MV()) -
-           half_space_constant;
+    s[i] =
+        half_space_F.CalcSignedDistance(vertices_F[triangle.vertex(i)].r_MV());
     if (s[i] > 0) ++num_positive;
   }
 
@@ -285,5 +284,36 @@ void ConstructTriangleHalfspaceIntersectionPolygon(
 }
 
 }  // namespace internal
+
+/**
+ Constructs the SurfaceMesh that results from intersecting a triangle mesh with
+ a half space.
+ @param input_mesh_F the mesh with vertices measured and expressed in some
+        frame, F.
+ @param half_space_F the half space, expressed in Frame F.
+ @returns the SurfaceMesh corresponding to the intersection, the vertices of
+          which will be measured and expressed in Frame F.
+ */
+template <typename T>
+SurfaceMesh<T> ConstructSurfaceMeshFromMeshHalfspaceIntersection(
+    const SurfaceMesh<T>& input_mesh_F,
+    const mesh_intersection::HalfSpace<T>& half_space_F) {
+  std::vector<SurfaceVertex<T>> new_vertices_F;
+  std::vector<SurfaceFace> new_faces;
+  std::unordered_map<SurfaceVertexIndex, SurfaceVertexIndex>
+      vertices_to_newly_created_vertices;
+  std::unordered_map<SortedPair<SurfaceVertexIndex>, SurfaceVertexIndex>
+      edges_to_newly_created_vertices;
+
+  for (const SurfaceFace& face : input_mesh_F.faces()) {
+    internal::ConstructTriangleHalfspaceIntersectionPolygon(
+        input_mesh_F.vertices(), face, half_space_F, &new_vertices_F,
+        &new_faces, &vertices_to_newly_created_vertices,
+        &edges_to_newly_created_vertices);
+  }
+
+  return SurfaceMesh<T>(std::move(new_faces), std::move(new_vertices_F));
+}
+
 }  // namespace geometry
 }  // namespace drake

geometry/proximity/mesh_intersection.h

@@ -18,6 +18,8 @@
 
 namespace drake {
 namespace geometry {
+
+// TODO(sean-curtis) Move this to a more common namespace.
 namespace mesh_intersection {
 
 #ifndef DRAKE_DOXYGEN_CXX  // Hide from Doxygen for now.
@@ -50,6 +52,8 @@ namespace mesh_intersection {
 template <typename T>
 class HalfSpace {
  public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(HalfSpace)
+
   /** Constructs a HalfSpace in frame F.
    @param nhat_F
        A unit-length vector perpendicular to the half space's planar boundary
@@ -77,17 +81,23 @@ class HalfSpace {
    frame F). The point is strictly inside, on the boundary, or outside based on
    the return value being negative, zero, or positive, respectively.
    */
-  T signed_distance(const Vector3<T>& p_FQ) const {
+  T CalcSignedDistance(const Vector3<T>& p_FQ) const {
     return nhat_F_.dot(p_FQ) - displacement_;
   }
 
   /** Reports true if the point Q (measured and expressed in frame F),
    strictly lies outside this half space.
    */
-  bool point_is_outside(const Vector3<T>& p_FQ) const {
-    return signed_distance(p_FQ) > 0;
+  bool PointIsOutside(const Vector3<T>& p_FQ) const {
+    return CalcSignedDistance(p_FQ) > 0;
   }
 
+  /** Gets the normal expressed in frame F. */
+  const Vector3<T> nhat_F() const { return nhat_F_; }
+
+  /** Gets the displacement constant. */
+  const T& displacement() const { return displacement_; }
+
  private:
   Vector3<T> nhat_F_;
   T displacement_{};
@@ -115,8 +125,8 @@ template <typename T>
 Vector3<T> CalcIntersection(const Vector3<T>& p_FA,
                             const Vector3<T>& p_FB,
                             const HalfSpace<T>& H_F) {
-  const T a = H_F.signed_distance(p_FA);
-  const T b = H_F.signed_distance(p_FB);
+  const T a = H_F.CalcSignedDistance(p_FA);
+  const T b = H_F.CalcSignedDistance(p_FB);
   // We require that A and B classify in opposite directions (one inside and one
   // outside). Outside has a strictly positive distance, inside is non-positive.
   // We confirm that their product is non-positive and that at least one of the
@@ -133,7 +143,7 @@ Vector3<T> CalcIntersection(const Vector3<T>& p_FA,
   //  if it turns out we need to perform this test at other sites.
   // Verify that the intersection point is on the plane of the half space.
   using std::abs;
-  DRAKE_DEMAND(abs(H_F.signed_distance(intersection)) < kEps);
+  DRAKE_DEMAND(abs(H_F.CalcSignedDistance(intersection)) < kEps);
   return intersection;
   // Justification.
   // 1. We set up the weights wa and wb such that wa + wb = 1, which
@@ -204,8 +214,8 @@ std::vector<Vector3<T>> ClipPolygonByHalfSpace(
   for (int i = 0; i < size; ++i) {
     const Vector3<T>& current = polygon_vertices_F[i];
     const Vector3<T>& previous = polygon_vertices_F[(i - 1 + size) % size];
-    const bool current_contained = !H_F.point_is_outside(current);
-    const bool previous_contained = !H_F.point_is_outside(previous);
+    const bool current_contained = !H_F.PointIsOutside(current);
+    const bool previous_contained = !H_F.PointIsOutside(previous);
     if (current_contained) {
       if (!previous_contained) {
         // Current is inside and previous is outside. Compute the point where

geometry/proximity/test/mesh_half_space_intersection_test.cc

@@ -9,15 +9,13 @@ namespace drake {
 namespace geometry {
 namespace {
 
+using mesh_intersection::HalfSpace;
+
 template <typename T>
 class MeshHalfspaceIntersectionTest : public ::testing::Test {
  public:
-  /// Returns the normal to the half space.
-  const Vector3<T>& normal_H() const { return normal_H_; }
-
-  /// Returns the constant d to the half space such that normal_Hᵀx = d,
-  /// where x is an arbitrary point on the surface of the half space.
-  const T& half_space_constant() const { return d_; }
+  /// Returns the half space with normal expressed in Frame H.
+  const HalfSpace<T> half_space_H() const { return *half_space_H_; }
 
   // Accessors for data structures used repeatedly.
   std::vector<SurfaceVertex<T>>& new_vertices() { return new_vertices_; }
@@ -126,8 +124,7 @@ class MeshHalfspaceIntersectionTest : public ::testing::Test {
   }
 
   // Calls the triangle-half space intersection routine using this object's
-  // half space normal and halfspace constant as well as the given mesh and
-  // construction data structures.
+  // half space as well as the given mesh and construction data structures.
   void ConstructTriangleHalfspaceIntersectionPolygon(
       const SurfaceMesh<T>& mesh,
       std::vector<SurfaceVertex<T>>* new_vertices_F_in,
@@ -138,7 +135,7 @@ class MeshHalfspaceIntersectionTest : public ::testing::Test {
           edges_to_newly_created_vertices_in) {
     for (const SurfaceFace& face : mesh.faces()) {
       internal::ConstructTriangleHalfspaceIntersectionPolygon(
-          mesh.vertices(), face, this->normal_H(), this->half_space_constant(),
+          mesh.vertices(), face, this->half_space_H(),
           new_vertices_F_in, new_faces_in,
           vertices_to_newly_created_vertices_in,
           edges_to_newly_created_vertices_in);
@@ -166,9 +163,10 @@ class MeshHalfspaceIntersectionTest : public ::testing::Test {
   void SetUp() {
     // The tests all use half space normal [0 0 1], with point [0 0 2]
     // lying on the half space.
-    normal_H_ = Vector3<T>(0, 0, 1);
+    Vector3<T> normal_H(0, 0, 1);
     const Vector3<T> point_H(0, 0, 2);
-    d_ = normal_H_.dot(point_H);
+    const T displacement = normal_H.dot(point_H);
+    half_space_H_ = std::make_unique<HalfSpace<T>>(normal_H, displacement);
   }
 
   std::vector<SurfaceVertex<T>> new_vertices_;
@@ -177,8 +175,7 @@ class MeshHalfspaceIntersectionTest : public ::testing::Test {
       vertices_to_newly_created_vertices_;
   std::unordered_map<SortedPair<SurfaceVertexIndex>, SurfaceVertexIndex>
       edges_to_newly_created_vertices_;
-  Vector3<T> normal_H_;
-  T d_;
+  std::unique_ptr<HalfSpace<T>> half_space_H_;
 };  // namespace
 TYPED_TEST_CASE_P(MeshHalfspaceIntersectionTest);
 
@@ -492,11 +489,89 @@ TYPED_TEST_P(MeshHalfspaceIntersectionTest, OneInsideTwoOutside) {
   this->VerifyMeshesEquivalent(expected_mesh, actual_mesh);
 }
 
+// Tests that a mesh of a box bisected by the half space produces the expected
+// number of faces and vertices (other unit tests in this file assess the
+// correctness of various kinds of triangle/half space intersections).
+TYPED_TEST_P(MeshHalfspaceIntersectionTest, BoxMesh) {
+  using T = TypeParam;
+
+  // Set the box vertices according to the following diagram. The box is
+  // centered at the origin of Frame F. Introducing this frame makes it less
+  // likely that all of the underlying tests might succeed because of some
+  // fortuitous initial values. The halfspace normal points along F's +z axis.
+  // Set the box vertices according to the following diagram. The box is
+  // centered at the origin.
+  //               v₁      +Z         v₃
+  //               ●───────┆─────────●
+  //              /│       ┆        /│
+  //             / │       ┆     v₇/ │
+  //         v₅ ●─────────────────●  │
+  //            │  │       ┆      │  │
+  //            │  │       o┄┄┄┄┄┄│┄┄┄┄┄ +Y
+  //            │  │      /       │  │
+  //            │  ●─────/────────│──● v₂
+  //            │ /v₀   /         │ /
+  //            │/     /          │/
+  //         v₄ ●─────────────────● v₆
+  //                 /
+  //                +X
+
+  // Set F to an arbitrarily chosen pose relative to the world frame.
+  math::RigidTransform<T> X_WF(math::RollPitchYaw<T>(M_PI_4, M_PI_4, M_PI_4),
+                               Vector3<T>(1.0, 2.0, 3.0));
+
+  std::vector<SurfaceVertex<T>> vertices_W;
+  vertices_W.emplace_back(X_WF * Vector3<T>(-1, -1, -1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(-1, -1, 1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(-1, 1, -1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(-1, 1, 1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(1, -1, -1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(1, -1, 1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(1, 1, -1));
+  vertices_W.emplace_back(X_WF * Vector3<T>(1, 1, 1));
+
+  // Set the twelve box faces using a counter-clockwise winding.
+  typedef SurfaceVertexIndex Index;
+  std::vector<SurfaceFace> faces;
+  faces.emplace_back(Index(4), Index(6), Index(7));  // +X face
+  faces.emplace_back(Index(7), Index(5), Index(4));  // +X face
+  faces.emplace_back(Index(1), Index(3), Index(2));  // -X face
+  faces.emplace_back(Index(2), Index(0), Index(1));  // -X face
+  faces.emplace_back(Index(2), Index(3), Index(7));  // +Y face
+  faces.emplace_back(Index(7), Index(6), Index(2));  // +Y face
+  faces.emplace_back(Index(4), Index(5), Index(1));  // -Y face
+  faces.emplace_back(Index(1), Index(0), Index(4));  // -Y face
+  faces.emplace_back(Index(7), Index(3), Index(1));  // +Z face
+  faces.emplace_back(Index(1), Index(5), Index(7));  // +Z face
+  faces.emplace_back(Index(4), Index(0), Index(2));  // -Z face
+  faces.emplace_back(Index(2), Index(6), Index(4));  // -Z face
+
+  // Construct the mesh.
+  const SurfaceMesh<T> mesh(std::move(faces), std::move(vertices_W));
+
+  // Construct the half-space.
+  const Vector3<T> half_space_normal_W = X_WF.rotation() * Vector3<T>(0, 0, 1);
+  const T half_space_displacement = half_space_normal_W.dot(X_WF.translation());
+  const HalfSpace<T> half_space(half_space_normal_W, half_space_displacement);
+
+  // Compute the intersection.
+  const SurfaceMesh<T> intersection_mesh =
+      ConstructSurfaceMeshFromMeshHalfspaceIntersection(
+          mesh, half_space);
+  ASSERT_EQ(intersection_mesh.num_vertices(), 8);
+
+  // Note: The sides of the box facing the +/- X and +/- Y directions are each
+  // split into three triangles. The side of the box facing -Z is fully enclosed
+  // (two more triangles) yielding 4 x 3 + 2 = 14 triangles. (The side facing +Z
+  // is outside the half space.)
+  ASSERT_EQ(intersection_mesh.num_faces(), 14);
+}
+
 REGISTER_TYPED_TEST_CASE_P(MeshHalfspaceIntersectionTest, NoIntersection,
                            InsideOrOnIntersection,
                            VertexOnHalfspaceIntersection,
                            EdgeOnHalfspaceIntersection, QuadrilateralResults,
-                           OutsideInsideOn, OneInsideTwoOutside);
+                           OutsideInsideOn, OneInsideTwoOutside, BoxMesh);
 
 typedef ::testing::Types<double, AutoDiffXd> MyTypes;
 INSTANTIATE_TYPED_TEST_CASE_P(My, MeshHalfspaceIntersectionTest, MyTypes);