Merge pull request #43 from mpetersen94/qhull_implement

Add implementation for generating VPolytope from HPolyhedron using qhull
RussTedrake · Oct 24, 2021 · ed8ff11 · ed8ff11
2 parents d52e84e + db3cebe
commit ed8ff11
Show file tree

Hide file tree

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