From 7d63a52710a7ab153e179964d47b9a4f45a619fb Mon Sep 17 00:00:00 2001
From: Jeremy Nimmer <jeremy.nimmer@tri.global>
Date: Tue, 29 Jun 2021 19:50:05 -0700
Subject: [PATCH] fixup! Use a function pointer for cloning

This saves more than 10 KB of object code in a Release build, and
will be friendlier in case we ever need to subclass in pydrake.

Also fix the constructor Doxygen to avoid triple backticks, which
apparently don't render correctly.
---
 geometry/optimization/convex_set.cc | 16 ++++++---
 geometry/optimization/convex_set.h  | 50 ++++++++++++++---------------
 2 files changed, 36 insertions(+), 30 deletions(-)

diff --git a/geometry/optimization/convex_set.cc b/geometry/optimization/convex_set.cc
index 78cb415e9f35..b3904b4560f6 100644
--- a/geometry/optimization/convex_set.cc
+++ b/geometry/optimization/convex_set.cc
@@ -22,18 +22,26 @@ using solvers::VectorXDecisionVariable;
 using std::pair;
 using std::sqrt;
 
+ConvexSet::ConvexSet(
+    std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner,
+    int ambient_dimension)
+    : cloner_(std::move(cloner)),
+      ambient_dimension_(ambient_dimension) {
+  DRAKE_DEMAND(ambient_dimension >= 0);
+}
+
 std::unique_ptr<ConvexSet> ConvexSet::Clone() const { return cloner_(*this); }
 
 HPolyhedron::HPolyhedron(const Eigen::Ref<const Eigen::MatrixXd>& A,
                          const Eigen::Ref<const Eigen::VectorXd>& b)
-    : ConvexSet(ConvexSetTag<HPolyhedron>(), A.cols()), A_{A}, b_{b} {
+    : ConvexSet(&ConvexSetCloner<HPolyhedron>, A.cols()), A_{A}, b_{b} {
   DRAKE_DEMAND(A.rows() == b.size());
 }
 
 HPolyhedron::HPolyhedron(const QueryObject<double>& query_object,
                          GeometryId geometry_id,
                          std::optional<FrameId> expressed_in)
-    : ConvexSet(ConvexSetTag<HPolyhedron>(), 3) {
+    : ConvexSet(&ConvexSetCloner<HPolyhedron>, 3) {
   std::pair<Eigen::MatrixXd, Eigen::VectorXd> Ab_G;
   query_object.inspector().GetShape(geometry_id).Reify(this, &Ab_G);
 
@@ -134,7 +142,7 @@ void HPolyhedron::ImplementGeometry(const Box& box, void* data) {
 
 HyperEllipsoid::HyperEllipsoid(const Eigen::Ref<const Eigen::MatrixXd>& A,
                                const Eigen::Ref<const Eigen::VectorXd>& center)
-    : ConvexSet(ConvexSetTag<HyperEllipsoid>(), center.size()),
+    : ConvexSet(&ConvexSetCloner<HyperEllipsoid>, center.size()),
       A_{A},
       center_{center} {
   DRAKE_DEMAND(A.rows() == center.size());
@@ -143,7 +151,7 @@ HyperEllipsoid::HyperEllipsoid(const Eigen::Ref<const Eigen::MatrixXd>& A,
 HyperEllipsoid::HyperEllipsoid(const QueryObject<double>& query_object,
                                GeometryId geometry_id,
                                std::optional<FrameId> expressed_in)
-    : ConvexSet(ConvexSetTag<HyperEllipsoid>(), 3) {
+    : ConvexSet(&ConvexSetCloner<HyperEllipsoid>, 3) {
   Eigen::Matrix3d A_G;
   query_object.inspector().GetShape(geometry_id).Reify(this, &A_G);
   // p_GG_varᵀ * A_Gᵀ * A_G * p_GG_var ≤ 1
diff --git a/geometry/optimization/convex_set.h b/geometry/optimization/convex_set.h
index e55e4151fa30..51fee333e61e 100644
--- a/geometry/optimization/convex_set.h
+++ b/geometry/optimization/convex_set.h
@@ -4,6 +4,7 @@
 #include <optional>
 #include <utility>
 
+#include "drake/common/drake_assert.h"
 #include "drake/geometry/geometry_ids.h"
 #include "drake/geometry/query_object.h"
 #include "drake/geometry/shape_specification.h"
@@ -54,12 +55,6 @@ The geometry::optimization tools support:
 @ingroup solvers
 */
 
-/** Simple struct for instantiating the type-specific ConvexSet functionality.
- A class derived from the ConvexSet class will invoke the parent's constructor
- as ConvexSet(ConvexSetTag<DerivedSet>()). */
-template <typename S>
-struct ConvexSetTag{};
-
 /** Abstract base class for defining a convex set.
 @ingroup geometry_optimization
 */
@@ -109,29 +104,21 @@ class ConvexSet : public ShapeReifier {
  protected:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConvexSet)
 
-  /** A derived class should invoke this with e.g.:
-  ```
+  /** For use by derived classes to construct a %ConvexSet.
+
+  @param cloner Function pointer to implement Clone(), typically of the form
+  `&ConvexSetCloner<Derived>`.
+
+  Here is a typical example:
+  @code
    class MyConvexSet final : public ConvexSet {
     public:
-     MyConvexSet() : ConvexSet(ConvexSetTag<MyConvexSet>()) {}
+     MyConvexSet() : ConvexSet(&ConvexSetCloner<MyConvexSet>, 3) {}
      ...
    };
-   ```
-   */
-  template <typename S>
-  ConvexSet(ConvexSetTag<S>, int ambient_dimension)
-      : ambient_dimension_{ambient_dimension} {
-    static_assert(std::is_base_of_v<ConvexSet, S>,
-                  "Concrete sets *must* be derived from the ConvexSet class");
-
-    DRAKE_DEMAND(ambient_dimension >= 0);
-
-    cloner_ = [](const ConvexSet& set) {
-      DRAKE_DEMAND(typeid(set) == typeid(S));
-      const S& derived_set = static_cast<const S&>(set);
-      return std::unique_ptr<ConvexSet>(new S(derived_set));
-    };
-  }
+  @endcode */
+  ConvexSet(std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner,
+            int ambient_dimension);
 
   // Non-virtual interface implementations.
   virtual bool DoPointInSet(const Eigen::Ref<const Eigen::VectorXd>& x,
@@ -144,10 +131,21 @@ class ConvexSet : public ShapeReifier {
   virtual std::pair<std::unique_ptr<Shape>, math::RigidTransformd>
   DoToShapeWithPose() const = 0;
 
-  int ambient_dimension_{0};
   std::function<std::unique_ptr<ConvexSet>(const ConvexSet&)> cloner_;
+  int ambient_dimension_{0};
 };
 
+/** (Advanced) Implementation helper for ConvexSet::Clone. Refer to the
+ConvexSet::ConvexSet() constructor documentation for an example. */
+template <typename Derived>
+std::unique_ptr<ConvexSet> ConvexSetCloner(const ConvexSet& other) {
+  static_assert(std::is_base_of_v<ConvexSet, Derived>,
+                "Concrete sets *must* be derived from the ConvexSet class");
+  DRAKE_DEMAND(typeid(other) == typeid(Derived));
+  const auto& typed_other = static_cast<const Derived&>(other);
+  return std::make_unique<Derived>(typed_other);
+}
+
 // Forward declaration.
 class HyperEllipsoid;