From db3cebecac6d085a9a94cc6d27911c81d759d989 Mon Sep 17 00:00:00 2001
From: mpetersen94 <markpetersen@g.harvard.edu>
Date: Sat, 23 Oct 2021 17:31:44 -0400
Subject: [PATCH] Add implementation for generating VPolytope from HPolyhedron
 using qhull

---
 geometry/optimization/test/vpolytope_test.cc |  2 +-
 geometry/optimization/vpolytope.cc           | 28 +++++++++++++++++++-
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc
index d16715ca8552..bd7688b9a210 100644
--- a/geometry/optimization/test/vpolytope_test.cc
+++ b/geometry/optimization/test/vpolytope_test.cc
@@ -160,7 +160,7 @@ GTEST_TEST(VPolytopeTest, FromHPolyhedronTest) {
   VPolytope V(H);
   EXPECT_EQ(V.ambient_dimension(), 6);
   EXPECT_EQ(V.vertices().rows(), 6);
-  EXPECT_EQ(V.vertices().cols(), 8);
+  EXPECT_EQ(V.vertices().cols(), std::pow(2, 6));
 
   Vector6d in1_W{Vector6d::Constant(-.99)}, in2_W{Vector6d::Constant(.99)},
       out1_W{Vector6d::Constant(-1.01)}, out2_W{Vector6d::Constant(1.01)};
diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc
index 4b6c23ed52b9..aa59ceadea50 100644
--- a/geometry/optimization/vpolytope.cc
+++ b/geometry/optimization/vpolytope.cc
@@ -44,8 +44,34 @@ VPolytope::VPolytope(const QueryObject<double>& query_object,
 
 VPolytope::VPolytope(const HPolyhedron& hpoly)
     : ConvexSet(&ConvexSetCloner<VPolytope>, hpoly.ambient_dimension()) {
+  Eigen::MatrixXd coeffs(hpoly.A().rows(), hpoly.A().cols() + 1);
+  coeffs.leftCols(hpoly.A().cols()) = hpoly.A();
+  coeffs.col(hpoly.A().cols()) = -hpoly.b();
+
+  Eigen::MatrixXd coeffs_t = coeffs.transpose();
+  std::vector<double> flat_coeffs;
+  flat_coeffs.resize(coeffs_t.size());
+  Eigen::VectorXd::Map(&flat_coeffs[0], coeffs_t.size()) = coeffs_t;
+
+  Eigen::VectorXd eigen_center = hpoly.ChebyshevCenter();
+  std::vector<double> center;
+  center.resize(eigen_center.size());
+  Eigen::VectorXd::Map(&center[0], eigen_center.size()) = eigen_center;
+
   orgQhull::Qhull qhull;
-  unused(qhull);
+  qhull.setFeasiblePoint(orgQhull::Coordinates(center));
+  qhull.runQhull("", hpoly.A().cols() + 1, hpoly.A().rows(), flat_coeffs.data(),
+                 "H");
+
+  vertices_.resize(hpoly.ambient_dimension(), qhull.facetCount());
+  int ii = 0;
+  for (orgQhull::QhullFacet facet = qhull.beginFacet();
+       facet != qhull.endFacet(); facet = facet.next()) {
+    std::vector<double> vertex = facet.getCenter().toStdVector();
+    vertices_.col(ii) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(
+        vertex.data(), vertex.size()) * hpoly.ambient_dimension();
+    ii++;
+  }
 }
 
 VPolytope::~VPolytope() = default;