[geometry] Add Shape::Visit (#21181)

Code that needs to operate on concrete Shape subclasses can now do so
without needing to implement an entire Reifier (though that remains a
fine and well-supported option when suitable).

Author: jwnimmer-tri

geometry/BUILD.bazel

@@ -397,6 +397,7 @@ drake_cc_library(
     ],
     deps = [
         "//common:nice_type_name",
+        "//common:overloaded",
         "//geometry/proximity:make_convex_hull_mesh_impl",
         "//geometry/proximity:meshing_utilities",
         "//geometry/proximity:obj_to_surface_mesh",
@@ -1014,6 +1015,7 @@ drake_cc_googletest(
     deps = [
         ":shape_specification",
         "//common:find_resource",
+        "//common:overloaded",
         "//common/test_utilities",
     ],
 )

geometry/shape_specification.cc

@@ -9,6 +9,7 @@
 
 #include "drake/common/drake_throw.h"
 #include "drake/common/nice_type_name.h"
+#include "drake/common/overloaded.h"
 #include "drake/geometry/proximity/make_convex_hull_mesh_impl.h"
 #include "drake/geometry/proximity/meshing_utilities.h"
 #include "drake/geometry/proximity/obj_to_surface_mesh.h"
@@ -322,68 +323,56 @@ double CalcMeshVolumeFromFile(const MeshType& mesh) {
   return internal::CalcEnclosedVolume(surface_mesh);
 }
 
-class CalcVolumeReifier final : public ShapeReifier {
- public:
-  CalcVolumeReifier() = default;
-
-  using ShapeReifier::ImplementGeometry;
-
-  void ImplementGeometry(const Box& box, void*) final {
-    volume_ = box.width() * box.depth() * box.height();
-  }
-  void ImplementGeometry(const Capsule& capsule, void*) final {
-    volume_ = M_PI * std::pow(capsule.radius(), 2) * capsule.length() +
-              4.0 / 3.0 * M_PI * std::pow(capsule.radius(), 3);
-  }
-  void ImplementGeometry(const Convex& mesh, void*) {
-    volume_ = CalcMeshVolumeFromFile(mesh);
-  }
-  void ImplementGeometry(const Cylinder& cylinder, void*) final {
-    volume_ = M_PI * std::pow(cylinder.radius(), 2) * cylinder.length();
-  }
-  void ImplementGeometry(const Ellipsoid& ellipsoid, void*) final {
-    volume_ = 4.0 / 3.0 * M_PI * ellipsoid.a() * ellipsoid.b() * ellipsoid.c();
-  }
-  void ImplementGeometry(const HalfSpace&, void*) final {
-    volume_ = std::numeric_limits<double>::infinity();
-  }
-  void ImplementGeometry(const Mesh& mesh, void*) {
-    volume_ = CalcMeshVolumeFromFile(mesh);
-  }
-  void ImplementGeometry(const MeshcatCone& cone, void*) final {
-    volume_ = 1.0 / 3.0 * M_PI * cone.a() * cone.b() * cone.height();
-  }
-  void ImplementGeometry(const Sphere& sphere, void*) final {
-    volume_ = 4.0 / 3.0 * M_PI * std::pow(sphere.radius(), 3);
-  }
-
-  double volume() const { return volume_; }
-
- private:
-  double volume_{0.0};
-};
-
 }  // namespace
 
 double CalcVolume(const Shape& shape) {
-  CalcVolumeReifier reifier;
-  shape.Reify(&reifier);
-  return reifier.volume();
+  return shape.Visit<double>(overloaded{
+      [](const Box& box) {
+        return box.width() * box.depth() * box.height();
+      },
+      [](const Capsule& capsule) {
+        return M_PI * std::pow(capsule.radius(), 2) * capsule.length() +
+               4.0 / 3.0 * M_PI * std::pow(capsule.radius(), 3);
+      },
+      [](const Convex& mesh) {
+        return CalcMeshVolumeFromFile(mesh);
+      },
+      [](const Cylinder& cylinder) {
+        return M_PI * std::pow(cylinder.radius(), 2) * cylinder.length();
+      },
+      [](const Ellipsoid& ellipsoid) {
+        return 4.0 / 3.0 * M_PI * ellipsoid.a() * ellipsoid.b() * ellipsoid.c();
+      },
+      [](const HalfSpace&) {
+        return std::numeric_limits<double>::infinity();
+      },
+      [](const Mesh& mesh) {
+        return CalcMeshVolumeFromFile(mesh);
+      },
+      [](const MeshcatCone& cone) {
+        return 1.0 / 3.0 * M_PI * cone.a() * cone.b() * cone.height();
+      },
+      [](const Sphere& sphere) {
+        return 4.0 / 3.0 * M_PI * std::pow(sphere.radius(), 3);
+      }});
 }
 
 // The NVI function definitions are enough boilerplate to merit a macro to
 // implement them, and we might as well toss in the dtor for good measure.
 
-#define DRAKE_DEFINE_SHAPE_SUBCLASS_BOILERPLATE(ShapeType)              \
-  ShapeType::~ShapeType() = default;                                    \
-  void ShapeType::DoReify(ShapeReifier* shape_reifier, void* user_data) \
-      const {                                                           \
-    shape_reifier->ImplementGeometry(*this, user_data);                 \
-  }                                                                     \
-  std::unique_ptr<Shape> ShapeType::DoClone() const {                   \
-    return std::unique_ptr<ShapeType>(new ShapeType(*this));            \
-  }                                                                     \
-  std::string_view ShapeType::do_type_name() const { return #ShapeType; }
+#define DRAKE_DEFINE_SHAPE_SUBCLASS_BOILERPLATE(ShapeType)                \
+  ShapeType::~ShapeType() = default;                                      \
+  void ShapeType::DoReify(ShapeReifier* shape_reifier, void* user_data)   \
+      const {                                                             \
+    shape_reifier->ImplementGeometry(*this, user_data);                   \
+  }                                                                       \
+  std::unique_ptr<Shape> ShapeType::DoClone() const {                     \
+    return std::unique_ptr<ShapeType>(new ShapeType(*this));              \
+  }                                                                       \
+  std::string_view ShapeType::do_type_name() const { return #ShapeType; } \
+  Shape::VariantShapeConstPtr ShapeType::get_variant_this() const {       \
+    return this;                                                          \
+  }
 
 DRAKE_DEFINE_SHAPE_SUBCLASS_BOILERPLATE(Box)
 DRAKE_DEFINE_SHAPE_SUBCLASS_BOILERPLATE(Capsule)

geometry/shape_specification.h

@@ -2,6 +2,7 @@
 
 #include <memory>
 #include <string>
+#include <variant>
 
 #include "drake/common/drake_copyable.h"
 #include "drake/common/drake_deprecated.h"
@@ -36,16 +37,15 @@ class Sphere;
 //
 // When you add a new subclass of Shape to Drake, you must:
 //
-// 1. Add a virtual function ImplementGeometry() for the new shape in
+// 1. Adjust the VariantShapeConstPtr typedef to list the new subclass.
+//
+// 2. Add a virtual function ImplementGeometry() for the new shape in
 //    ShapeReifier that invokes the ThrowUnsupportedGeometry method, and add to
 //    the test for it in shape_specification_test.cc.
 //
-// 2. Implement ImplementGeometry in derived ShapeReifiers to continue support
-//    if desired, otherwise ensure unimplemented functions are not hidden in new
-//    derivations of ShapeReifier with `using`, for example, `using
-//    ShapeReifier::ImplementGeometry`. Existing subclasses should already have
-//    this. Otherwise, you might get a runtime error; we do not have an
-//    automatic way to enforce them at compile time.
+// 3. Grep Drake for the line `using ShapeReifier::ImplementGeometry;` (a trick
+//    that selects a default-throw implementation) and choose which (if any) of
+//    those reifiers you want to add support for this new shape into.
 
 /** The abstract base class for all shape specifications. Concrete subclasses
   exist for specific shapes (e.g., Box, Mesh, etc.).
@@ -76,6 +76,33 @@ class Shape {
   /** Returns a string representation of this shape. */
   std::string to_string() const { return do_to_string(); }
 
+  /** Calls the given `visitor` function with `*this` as the sole argument, but
+  with `*this` downcast to be the shape's concrete subclass. For example, if
+  this shape is a %Box then calls `visitor(static_cast<const Box&>(*this))`.
+  @tparam ReturnType The return type to coerce return values into. When not
+  `void`, anything returned by the visitor must be implicitly convertible to
+  this type. When `void`, the return type will be whatever the Vistor's call
+  operator returns by default.
+
+  To see examples of how this is used, you can check the Drake source code,
+  e.g., check the implementation of CalcVolume() for one example. */
+  template <typename ReturnType = void, typename Visitor>
+  decltype(auto) Visit(Visitor&& visitor) const {
+    if constexpr (std::is_same_v<ReturnType, void>) {
+      return std::visit(
+          [&visitor](auto* shape) {
+            return visitor(*shape);
+          },
+          get_variant_this());
+    } else {
+      return std::visit(
+          [&visitor](auto* shape) -> ReturnType {
+            return visitor(*shape);
+          },
+          get_variant_this());
+    }
+  }
+
  protected:
   /** (Internal use only) Constructor for use by derived classes.
   All subclasses of Shape must be marked `final`. */
@@ -102,6 +129,21 @@ class Shape {
 
   /** (Internal use only) NVI for to_string(). */
   virtual std::string do_to_string() const = 0;
+
+  /** (Internal use only) All concrete subclasses, as const pointers. */
+  using VariantShapeConstPtr = std::variant<  //
+      const Box*,                             //
+      const Capsule*,                         //
+      const Convex*,                          //
+      const Cylinder*,                        //
+      const Ellipsoid*,                       //
+      const HalfSpace*,                       //
+      const Mesh*,                            //
+      const MeshcatCone*,                     //
+      const Sphere*>;
+
+  /** (Internal use only) NVI-like helper function for Visit(). */
+  virtual VariantShapeConstPtr get_variant_this() const = 0;
 };
 
 /** Definition of a box. The box is centered on the origin of its canonical
@@ -146,6 +188,7 @@ class Box final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   Vector3<double> size_;
 };
@@ -180,6 +223,7 @@ class Capsule final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   double radius_{};
   double length_{};
@@ -246,6 +290,7 @@ class Convex final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   std::string filename_;
   std::string extension_;
@@ -279,6 +324,7 @@ class Cylinder final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   double radius_{};
   double length_{};
@@ -321,6 +367,7 @@ class Ellipsoid final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   Vector3<double> radii_;
 };
@@ -361,6 +408,7 @@ class HalfSpace final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 };
 
 // TODO(DamrongGuoy): Update documentation when mesh is fully supported (i.e.,
@@ -440,6 +488,7 @@ class Mesh final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   // NOTE: Cannot be const to support default copy/move semantics.
   std::string filename_;
@@ -488,6 +537,7 @@ class MeshcatCone final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   double height_{};
   double a_{};
@@ -514,6 +564,7 @@ class Sphere final : public Shape {
   std::unique_ptr<Shape> DoClone() const final;
   std::string_view do_type_name() const final;
   std::string do_to_string() const final;
+  VariantShapeConstPtr get_variant_this() const final;
 
   double radius_{};
 };

geometry/test/shape_specification_test.cc

@@ -7,6 +7,7 @@
 
 #include "drake/common/find_resource.h"
 #include "drake/common/fmt_eigen.h"
+#include "drake/common/overloaded.h"
 #include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_no_throw.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
@@ -253,6 +254,49 @@ TEST_F(ReifierTest, CloningShapes) {
   ASSERT_TRUE(sphere_made_);
 }
 
+GTEST_TEST(VisitTest, ReturnTypeVoid) {
+  const Box box(1.0, 2.0, 3.0);
+  box.Visit(overloaded{
+      [&](const Box& arg) {
+        EXPECT_EQ(&arg, &box);
+      },
+      [](const auto&) {
+        GTEST_FAIL();
+      },
+  });
+
+  const Sphere sphere(1.0);
+  sphere.Visit(overloaded{
+      [&](const Sphere& arg) {
+        EXPECT_EQ(&arg, &sphere);
+      },
+      [](const auto&) {
+        GTEST_FAIL();
+      },
+  });
+}
+
+GTEST_TEST(VisitTest, ReturnTypeConversion) {
+  const Box box(1.0, 2.0, 3.0);
+  const Sphere sphere(1.0);
+
+  auto get_size = overloaded{
+      [](const Box& arg) {
+        return arg.size();
+      },
+      [](const Sphere& arg) {
+        return Vector1d(arg.radius());
+      },
+      [](const auto&) -> Eigen::VectorXd {
+        DRAKE_UNREACHABLE();
+      },
+  };
+
+  Eigen::VectorXd dims;
+  dims = box.Visit<Eigen::VectorXd>(get_size);
+  dims = sphere.Visit<Eigen::VectorXd>(get_size);
+}
+
 // Given the pose of a plane and its expected translation and z-axis, confirms
 // that the pose conforms to expectations. Also confirms that the rotational
 // component is orthonormal.

multibody/tree/BUILD.bazel

@@ -257,6 +257,7 @@ drake_cc_library(
     hdrs = ["geometry_spatial_inertia.h"],
     deps = [
         ":spatial_inertia",
+        "//common:overloaded",
         "//geometry:shape_specification",
         "//geometry/proximity:make_mesh_from_vtk",
         "//geometry/proximity:obj_to_surface_mesh",