bool Polygon::convertToInequalityConstraints(Eigen::MatrixXd& A, Eigen::VectorXd& b) const
{
Eigen::MatrixXd V(nVertices(), 2);
for (unsigned int i = 0; i < nVertices(); ++i)
V.row(i) = vertices_[i];
// Create k, a list of indices from V forming the convex hull.
// TODO: Assuming counter-clockwise ordered convex polygon.
// MATLAB: k = convhulln(V);
Eigen::MatrixXi k;
k.resizeLike(V);
for (unsigned int i = 0; i < V.rows(); ++i)
k.row(i) << i, (i+1) % V.rows();
Eigen::RowVectorXd c = V.colwise().mean();
V.rowwise() -= c;
A = Eigen::MatrixXd::Constant(k.rows(), V.cols(), NAN);
unsigned int rc = 0;
for (unsigned int ix = 0; ix < k.rows(); ++ix) {
Eigen::MatrixXd F(2, V.cols());
F.row(0) << V.row(k(ix, 0));
F.row(1) << V.row(k(ix, 1));
Eigen::FullPivLU<Eigen::MatrixXd> luDecomp(F);
if (luDecomp.rank() == F.rows()) {
A.row(rc) = F.colPivHouseholderQr().solve(Eigen::VectorXd::Ones(F.rows()));
++rc;
}
}
A = A.topRows(rc);
b = Eigen::VectorXd::Ones(A.rows());
b = b + A * c.transpose();
return true;
}
Polytope Polytope::operator&( const Polytope &P2 )
{
assert( this->H.cols() == P2.H.cols() );
Eigen::MatrixXd H( this->H.rows() + P2.H.rows(), this->H.cols() );
Eigen::VectorXd K( this->H.rows() + P2.H.rows(), 1 );
H.topRows( this->H.rows() ) = this->H;
K.topRows( this->H.rows() ) = this->K;
H.bottomRows( P2.H.rows() ) = P2.H;
K.bottomRows( P2.H.rows() ) = P2.K;
return Polytope( H, K );
}