[geometry] ComputeSignedDistanceToPoint from meshes (#21471)

geometry/proximity/BUILD.bazel

@@ -155,24 +155,6 @@ drake_cc_library(
     ],
 )
 
-drake_cc_library(
-    name = "calc_signed_distance_to_surface_mesh",
-    srcs = ["calc_signed_distance_to_surface_mesh.cc"],
-    hdrs = ["calc_signed_distance_to_surface_mesh.h"],
-    internal = True,
-    visibility = ["//geometry:__pkg__"],
-    deps = [
-        ":bv",
-        ":bvh",
-        ":triangle_surface_mesh",
-        "//common:essential",
-    ],
-    implementation_deps = [
-        ":distance_to_point_callback",
-        "//math:geometric_transform",
-    ],
-)
-
 drake_cc_library(
     name = "collision_filter",
     srcs = ["collision_filter.cc"],
@@ -288,14 +270,27 @@ drake_cc_library(
 
 drake_cc_library(
     name = "distance_to_point_callback",
-    srcs = ["distance_to_point_callback.cc"],
-    hdrs = ["distance_to_point_callback.h"],
+    srcs = [
+        "calc_signed_distance_to_surface_mesh.cc",
+        "distance_to_point_callback.cc",
+        "mesh_distance_boundary.cc",
+    ],
+    hdrs = [
+        "calc_signed_distance_to_surface_mesh.h",
+        "distance_to_point_callback.h",
+        "mesh_distance_boundary.h",
+    ],
     internal = True,
     visibility = [
         "//geometry:__pkg__",
     ],
     deps = [
+        ":bv",
+        ":bvh",
         ":proximity_utilities",
+        ":triangle_surface_mesh",
+        ":volume_mesh",
+        ":volume_to_surface_mesh",
         "//common:default_scalars",
         "//common:drake_export",
         "//common:essential",
@@ -1102,7 +1097,7 @@ drake_cc_googletest(
 drake_cc_googletest(
     name = "calc_signed_distance_to_surface_mesh_test",
     deps = [
-        ":calc_signed_distance_to_surface_mesh",
+        ":distance_to_point_callback",
         ":volume_mesh",
         ":volume_to_surface_mesh",
         "//common/test_utilities:eigen_matrix_compare",
@@ -1250,6 +1245,7 @@ drake_cc_googletest(
     deps = [
         ":characterization_utilities",
         ":distance_to_point_callback",
+        ":make_box_mesh",
     ],
 )
 
@@ -1529,6 +1525,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "mesh_distance_boundary_test",
+    deps = [
+        "distance_to_point_callback",
+    ],
+)
+
 drake_cc_googletest(
     name = "mesh_field_linear_test",
     deps = [

geometry/proximity/calc_signed_distance_to_surface_mesh.cc

@@ -90,8 +90,11 @@ class BvhVisitor {
 
 }  // namespace
 
-FeatureNormalSet::FeatureNormalSet(const TriangleSurfaceMesh<double>& mesh_M)
-    : vertex_normals_(mesh_M.num_vertices(), Vector3d::Zero()) {
+std::variant<FeatureNormalSet, std::string> FeatureNormalSet::MaybeCreate(
+    const TriangleSurfaceMesh<double>& mesh_M) {
+  std::vector<Vector3<double>> vertex_normals(mesh_M.num_vertices(),
+                                              Vector3d::Zero());
+  std::unordered_map<SortedPair<int>, Vector3<double>> edge_normals;
   const std::vector<Vector3d>& vertices = mesh_M.vertices();
   // We can compute a tolerance for a knife edge based on a minimum dihedral
   // angle θ between adjacent faces using the following formula:
@@ -118,33 +121,33 @@ FeatureNormalSet::FeatureNormalSet(const TriangleSurfaceMesh<double>& mesh_M)
     for (int i = 0; i < 3; ++i) {
       const double angle =
           std::acos(unit_edge_vector[i].dot(-unit_edge_vector[(i + 2) % 3]));
-      vertex_normals_[v[i]] += angle * face_normal;
+      vertex_normals[v[i]] += angle * face_normal;
     }
     // Accumulate normal for each edge of the triangle.
     for (int i = 0; i < 3; ++i) {
       const auto edge = MakeSortedPair(v[i], v[(i + 1) % 3]);
-      auto it = edge_normals_.find(edge);
-      if (it == edge_normals_.end()) {
-        edge_normals_[edge] = face_normal;
+      auto it = edge_normals.find(edge);
+      if (it == edge_normals.end()) {
+        edge_normals[edge] = face_normal;
       } else {
         it->second += face_normal;
         if (it->second.squaredNorm() < kToleranceSquaredNorm) {
-          throw std::runtime_error(
-              "FeatureNormalSet: Cannot compute an edge normal because "
-              "the two triangles sharing the edge make a very sharp edge.");
+          return "FeatureNormalSet: Cannot compute an edge normal because "
+                 "the two triangles sharing the edge make a very sharp edge.";
         }
         it->second.normalize();
       }
     }
   }
-  for (auto& v_normal : vertex_normals_) {
+  for (auto& v_normal : vertex_normals) {
     if (v_normal.squaredNorm() < kToleranceSquaredNorm) {
-      throw std::runtime_error(
-          "FeatureNormalSet: Cannot compute a vertex normal because "
-          "the triangles sharing the vertex form a very pointy needle.");
+      return "FeatureNormalSet: Cannot compute a vertex normal because "
+             "the triangles sharing the vertex form a very pointy needle.";
     }
     v_normal.normalize();
   }
+
+  return FeatureNormalSet(std::move(vertex_normals), std::move(edge_normals));
 }
 
 SquaredDistanceToTriangle CalcSquaredDistanceToTriangle(

geometry/proximity/calc_signed_distance_to_surface_mesh.h

@@ -1,7 +1,9 @@
 #pragma once
 
 #include <limits>
+#include <string>
 #include <unordered_map>
+#include <utility>
 #include <variant>
 #include <vector>
 
@@ -35,31 +37,32 @@ namespace internal {
 
  J.A. Baerentzen; H. Aanaes. Signed distance computation using the angle
  weighted pseudonormal. IEEE Transactions on Visualization and Computer
- Graphics (Volume: 11, Issue: 3, May-June 2005).
-
- @throws std::exception if the mesh has very sharp features.
- They make calculation of the vertex normals and edge normals too sensitive
- to numerical rounding.
-     Think of the simplest possible knife model of two adjacent triangles
- sharing a single edge. The two triangles would have a small angle between
- them, creating a "sharp edge".  If the edge is too sharp, the mesh will be
- rejected.  We have a generous tolerance -- the angle can be less than a
- degree -- but it would be better to stay well away from impractically sharp
- edges.
-     There is an analogous situation with vertices, where a vertex
- creates a pointy, needle-like region on the mesh. The skinny "needles"
- have the same problem as the thin knife edges. Presence of these
- "needles" may lead to the mesh being rejected.  */
+ Graphics (Volume: 11, Issue: 3, May-June 2005).  */
 class FeatureNormalSet {
  public:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FeatureNormalSet);
 
-  // Computes and stores the normals at the vertices and the edges of the given
-  // surface mesh expressed in frame M.
+  // Computes the normals at the vertices and the edges of the given surface
+  // mesh expressed in frame M.
   //
   // @pre  The mesh is watertight and a closed manifold. Otherwise, the
   //       computed normals are incorrect.
-  explicit FeatureNormalSet(const TriangleSurfaceMesh<double>& mesh_M);
+  //
+  // @returns the computed feature normals or an error message if the mesh has
+  // extremely sharp features. They make calculation of the vertex normals and
+  // edge normals too sensitive to numerical rounding.
+  //     Think of the simplest possible knife model of two adjacent triangles
+  // sharing a single edge. The two triangles would have a small angle between
+  // them, creating a "sharp edge".  If the edge is too sharp, the mesh will be
+  // rejected.  We have a generous tolerance -- the angle can be less than a
+  // degree -- but it would be better to stay well away from impractically sharp
+  // edges.
+  //     There is an analogous situation with vertices, where a vertex
+  // creates a pointy, needle-like region on the mesh. The skinny "needles"
+  // have the same problem as the thin knife edges. Presence of these
+  // "needles" may lead to the mesh being rejected.
+  static std::variant<FeatureNormalSet, std::string> MaybeCreate(
+      const TriangleSurfaceMesh<double>& mesh_M);
 
   // Returns the normal at a vertex `v` as the angle-weighted average of face
   // normals of triangles sharing the vertex. The weight of a triangle is the
@@ -91,6 +94,12 @@ class FeatureNormalSet {
   }
 
  private:
+  FeatureNormalSet(
+      std::vector<Vector3<double>>&& vertex_normals,
+      std::unordered_map<SortedPair<int>, Vector3<double>>&& edge_normals)
+      : vertex_normals_(std::move(vertex_normals)),
+        edge_normals_(std::move(edge_normals)) {}
+
   std::vector<Vector3<double>> vertex_normals_{};
   std::unordered_map<SortedPair<int>, Vector3<double>> edge_normals_{};
 };

geometry/proximity/distance_to_point_callback.cc

@@ -1,7 +1,11 @@
 #include "drake/geometry/proximity/distance_to_point_callback.h"
 
+#include <string>
+#include <variant>
+
 #include "drake/common/default_scalars.h"
 #include "drake/common/drake_bool.h"
+#include "drake/geometry/proximity/calc_signed_distance_to_surface_mesh.h"
 
 namespace drake {
 namespace geometry {
@@ -154,22 +158,6 @@ void ComputeDistanceToPrimitive(const fcl::Capsuled& capsule,
   }
 }
 
-// clang-format off
-#define INSTANTIATE_DISTANCE_TO_PRIMITIVE(Shape, S)                         \
-template void ComputeDistanceToPrimitive<S>(                                \
-    const fcl::Shape&, const math::RigidTransform<S>&, const Vector3<S>&,   \
-    Vector3<S>*, S*, Vector3<S>*)
-// clang-format on
-
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Sphered, double);
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Sphered, AutoDiffXd);
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Halfspaced, double);
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Halfspaced, AutoDiffXd);
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Capsuled, double);
-// INSTANTIATE_DISTANCE_TO_PRIMITIVE(Capsuled, AutoDiffXd);
-
-#undef INSTANTIATE_DISTANCE_TO_PRIMITIVE
-
 template <typename T>
 SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(const fcl::Boxd& box) {
   // Express the given query point Q in the frame of the box geometry G.
@@ -380,6 +368,22 @@ SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
   return SignedDistanceToPoint<T>{geometry_id_, p_GN_G, distance, grad_W};
 }
 
+template <typename T>
+SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
+    const MeshDistanceBoundary& mesh_G) {
+  const Vector3<double> p_GQ = ExtractDoubleOrThrow(X_WG_.inverse() * p_WQ_);
+  if (!std::holds_alternative<FeatureNormalSet>(mesh_G.feature_normal())) {
+    throw std::runtime_error("DistanceToPoint from meshes: " +
+                             std::get<std::string>(mesh_G.feature_normal()));
+  }
+  const SignedDistanceToSurfaceMesh d = CalcSignedDistanceToSurfaceMesh(
+      p_GQ, mesh_G.tri_mesh(), mesh_G.tri_bvh(),
+      std::get<FeatureNormalSet>(mesh_G.feature_normal()));
+  return SignedDistanceToPoint<T>{geometry_id_, d.nearest_point,
+                                  d.signed_distance,
+                                  X_WG_.rotation() * Vector3<T>(d.gradient)};
+}
+
 template <typename T>
 template <int dim, typename U>
 int DistanceToPoint<T>::ExtremalAxis(const Vector<U, dim>& p,
@@ -489,30 +493,16 @@ DistanceToPoint<T>::ComputeDistanceToBox(const Vector<double, dim>& h,
   return std::make_tuple(p_GN_G, grad_G, is_Q_on_edge_or_vertex);
 }
 
-bool ScalarSupport<double>::is_supported(fcl::NODE_TYPE node_type) {
-  switch (node_type) {
-    case fcl::GEOM_BOX:
-    case fcl::GEOM_CAPSULE:
-    case fcl::GEOM_CYLINDER:
-    case fcl::GEOM_ELLIPSOID:
-    case fcl::GEOM_HALFSPACE:
-    case fcl::GEOM_SPHERE:
-      return true;
-    default:
-      return false;
-  }
-}
-
 template <typename T>
 bool Callback(fcl::CollisionObjectd* object_A_ptr,
               fcl::CollisionObjectd* object_B_ptr,
               // NOLINTNEXTLINE
-              void* callback_data, double& threshold) {
+              void* callback_data, double& threshold_out) {
   auto& data = *static_cast<CallbackData<T>*>(callback_data);
 
   // Three things:
-  //   1. We repeatedly set max_distance in each call to the callback because we
-  //   can't initialize it. The cost is negligible but maximizes any culling
+  //   1. We repeatedly set `threshold_out` in each call to the callback because
+  //   we can't initialize it. The cost is negligible but maximizes any culling
   //   benefit.
   //   2. Due to how FCL is implemented, passing a value <= 0 will cause results
   //   to be omitted because the bounding box test only considers *separating*
@@ -524,7 +514,7 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
   //   bounding box test in which this is used doesn't produce a code via
   //   calculation; it is a perfect, hard-coded zero.
   const double kEps = std::numeric_limits<double>::epsilon() / 10;
-  threshold = std::max(data.threshold, kEps);
+  threshold_out = std::max(data.threshold, kEps);
 
   // We use `const` to prevent modification of the collision objects.
   const fcl::CollisionObjectd* geometry_object =
@@ -549,6 +539,16 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
         distance = distance_to_point(
             *static_cast<const fcl::Capsuled*>(collision_geometry));
         break;
+      // Both drake::geometry::Mesh and Convex use fcl::GEOM_CONVEX.
+      case fcl::GEOM_CONVEX:
+        if (data.mesh_boundaries.contains(geometry_id)) {
+          distance = distance_to_point(data.mesh_boundaries.at(geometry_id));
+        } else {
+          // Unsupported mesh types. Returning false tells fcl to continue
+          // to other objects.
+          return false;
+        }
+        break;
       case fcl::GEOM_CYLINDER:
         distance = distance_to_point(
             *static_cast<const fcl::Cylinderd*>(collision_geometry));
@@ -566,7 +566,8 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
             *static_cast<const fcl::Sphered*>(collision_geometry));
         break;
       default:
-        // Returning false tells fcl to continue to other objects.
+        // Unsupported shapes.  Returning false tells fcl to continue to
+        // other objects.
         return false;
     }