Merge pull request #41 from jwnimmer-tri/convex_set-clone-function

fixup! Use a function pointer for cloning
RussTedrake · Jun 30, 2021 · fffe890 · fffe890
2 parents b061cd0 + 7d63a52
commit fffe890
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 30 deletions.
diff --git 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
@@ -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;