Convex (and Mesh) support for QueryObject::ComputeSignedDistanceToPoint

[SeanCurtis-TRI]

QueryObject::ComputeSignedDistanceToPoint() has a sparsely populated support matrix. With recent changes in support for the convex hull for Mesh and Convex shapes, it would be good if we could fill the support matrix to include Convex, explicitly, and Mesh (implicitly as its convex hull).

This function has historically been unique as we needed to make sure that it produced good gradients and could successfully be evaluated w.r.t. AutoDiffXd. As such, the shapes that are supported resulted from a bunch of bespoke Drake code. The gaps are simply those specific point-queries that Drake hasn't implemented yet. We should do so for a convex hull.

Describe the solution you'd like We need to implement a specific point-convex-hull query.

We are guaranteed a convex hull for Mesh and Convex types. But we need to perform signed distance to a point. This essentially means we need to implement our own version of GJK-EPA tailored to this particular problem (which makes it much simpler). We'll need:

1. Convert the convex hull representation (PolygonSurfaceMesh) into a data structure that will allow us to walk along the convex hull i a particular direction.
2. Given the query point, walk along the boundary of the polytope to find the closest point (inside or outside).
3. Do so in such a way that the gradient is well defined even with T = AutoDiffXd (i.e., no sqrt of displacement vector).

Describe alternatives you've considered None; either we support it or we don't.

[jwnimmer-tri]

When choosing the class design and algorithms here, planning to support both double and AutoDiffXd makes sense.

However, in terms of incremental implementation milestones, landing support for double first and AutoDiffXd in a second pass would benefit our most immediate downstream needs (where we only need double in the near-term).

[DamrongGuoy]

Is this a relevant issue?

• 3220

At the end of that issue, @xuchenhan-tri and I made these two points:

1. For triangle meshes, we can compute unsigned distance using geometry/proximity/calc_distance_to_surface_mesh.h

   double CalcDistanceToSurfaceMesh(
   const Vector3<double>& p_WQ, const TriangleSurfaceMesh<double>& mesh_W,
   const Bvh<Obb, TriangleSurfaceMesh<double>>& bvh_W);

2. If the triangle meshes are water-tight, I believe we can assign the correct sign using the angle-weighted pseudonormals in this paper, but I have not tried it. 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)

Additionally, if the mesh is tetrahedral, we can extract its water-tight triangle surface mesh (ConvertVolumeToSurfaceMesh()) and then use the same code above.

I am more concerned about the return grad_W in the struct SignedDistanceToPoint. I hope we can use the pseudonormal as a gradient. In general, there are multiple nearest points p_GN and multiple gradients grad_W when the query point is on the medial axis.

[jwnimmer-tri]

Is this a relevant issue?

Somewhat. It is useful to cross-mention #3220 since it's related (so thank you for that), but that issue is about signed distance to non-convex meshes and this issue is about signed distance to (surely watertight) convex hulls.

[jwnimmer-tri]

To make sure we're on the same page, possibly a good first step here would be to open a small Draft PR that contains the documentation changes for this new feature, and then after checking that over maybe we add a quick pseudo-code (as comments) for the new test case(s) that will demonstrate it working.

[chen-tianjian]

Just a note from user perspective: we are interested in using the ComputeSignedDistanceToPoint function against deformable meshes. I would assume this effort for generic meshes should make that happen, but let's make a note to keep these ideas connected.

[SeanCurtis-TRI]

In retrospect, the relationship to deformable meshes may not properly homed in this issue nor in #3220. While deformable meshes are intrinisically non-convex, they are guaranteed to be watertight by construction. So, they sit somewhere in between. Still, good to note it somewhere. If necessary, we'll elevate it to its own issue. Perhaps an issue more generally addressing QueryObject's methods an deformable geometries, perhaps? @xuchenhan-tri what do you think?