void QPSolver::preSolve()
{
if (mMethodType == SM_Normal)
{
mU = MatrixXd::Identity(mNumVar, mNumVar);
mCU = mc;
mSigma = mQ;
mAU = mA;
}
else if (mMethodType == SM_SVD)
{
JacobiSVD<MatrixXd> svd(mQ, ComputeThinU | ComputeThinV);
mU = svd.matrixU();
VectorXd singularValues = svd.singularValues();
mSigma = MatrixXd::Zero(mNumVar, mNumVar);
for (int i = 0; i < singularValues.size(); ++i)
{
mSigma(i, i) = singularValues[i];
}
MatrixXd uTrans = mU.transpose();
mCU = uTrans * mc;
mAU = mA * mU;
//MatrixXd recoveredQ = mU * mSigma * uTrans;
//for (int i = 0; i < recoveredQ.rows(); ++i)
//{
// for (int j = 0; j < recoveredQ.cols(); ++j)
// {
// if (abs((recoveredQ(i, j) - mQ(i, j)) / mQ(i, j)) > 1e-6)
// LOG(INFO) << "(" << i << ", " << j << "): " << recoveredQ(i, j) << ": " << mQ(i, j);
// }
//}
}
else if (mMethodType == SM_Separable)
{
const MatrixXd& lhs = mLhs;
int numRows = lhs.rows();
mNumVarOriginal = mNumVar;
mNumConOriginal = mNumCon;
mNumVar += numRows;
mNumCon += numRows;
mSigma = MatrixXd::Zero(mNumVar, mNumVar);
mSigma.block(mNumVarOriginal, mNumVarOriginal, numRows, numRows) = MatrixXd::Identity(numRows, numRows);
mCU = VectorXd::Zero(mNumVar);
mCU.head(mNumVarOriginal) = mc.head(mNumVarOriginal);
mAU = MatrixXd::Zero(mNumCon, mNumVar);
if (mNumConOriginal)
mAU.block(0, 0, mNumConOriginal, mNumVarOriginal) = mA;
mAU.block(mNumConOriginal, 0, numRows, mNumVarOriginal) = lhs;
mAU.block(mNumConOriginal, mNumVarOriginal, numRows, numRows) = -MatrixXd::Identity(numRows, numRows);
VectorXd newLbc = VectorXd::Zero(mNumCon);
VectorXd newUbc = VectorXd::Zero(mNumCon);
VectorXi newBLowerBounded = VectorXi::Zero(mNumCon);
VectorXi newBUpperBounded = VectorXi::Zero(mNumCon);
if (mNumConOriginal)
{
newLbc.head(mNumConOriginal) = mlbc;
newUbc.head(mNumConOriginal) = mubc;
newBLowerBounded.head(mNumConOriginal) = mbConstraintLowerBounded;
newBUpperBounded.head(mNumConOriginal) = mbConstraintUpperBounded;
}
newBLowerBounded.tail(numRows) = VectorXi::Constant(numRows, 1);
newBUpperBounded.tail(numRows) = VectorXi::Constant(numRows, 1);
mlbc = newLbc;
mubc = newUbc;
mbConstraintLowerBounded = newBLowerBounded;
mbConstraintUpperBounded = newBUpperBounded;
VectorXd newLb = VectorXd::Zero(mNumVar);
VectorXd newUb = VectorXd::Zero(mNumVar);
VectorXi newValueLowerBounded = VectorXi::Zero(mNumVar);
VectorXi newValueUpperBounded = VectorXi::Zero(mNumVar);
newLb.head(mNumVarOriginal) = mlb;
newUb.head(mNumVarOriginal) = mub;
newValueLowerBounded.head(mNumVarOriginal) = mbLowerBounded;
newValueUpperBounded.head(mNumVarOriginal) = mbUpperBounded;
mlb = newLb;
mub = newUb;
mbLowerBounded = newValueLowerBounded;
mbUpperBounded = newValueUpperBounded;
}
}
//#define IGL_LINPROG_VERBOSE
IGL_INLINE bool igl::linprog(
const Eigen::VectorXd & c,
const Eigen::MatrixXd & _A,
const Eigen::VectorXd & b,
const int k,
Eigen::VectorXd & x)
{
// This is a very literal translation of
// http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m
using namespace Eigen;
using namespace std;
bool success = true;
// number of constraints
const int m = _A.rows();
// number of original variables
const int n = _A.cols();
// number of iterations
int it = 0;
// maximum number of iterations
//const int MAXIT = 10*m;
const int MAXIT = 100*m;
// residual tolerance
const double tol = 1e-10;
const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd
{
Eigen::VectorXd Bsign(B.size());
for(int i = 0;i<B.size();i++)
{
Bsign(i) = B(i)>0?1:(B(i)<0?-1:0);
}
return Bsign;
};
// initial (inverse) basis matrix
VectorXd Dv = sign(sign(b).array()+0.5);
Dv.head(k).setConstant(1.);
MatrixXd D = Dv.asDiagonal();
// Incorporate slack variables
MatrixXd A(_A.rows(),_A.cols()+D.cols());
A<<_A,D;
// Initial basis
VectorXi B = igl::colon<int>(n,n+m-1);
// non-basis, may turn out that vector<> would be better here
VectorXi N = igl::colon<int>(0,n-1);
int j;
double bmin = b.minCoeff(&j);
int phase;
VectorXd xb;
VectorXd s;
VectorXi J;
if(k>0 && bmin<0)
{
phase = 1;
xb = VectorXd::Ones(m);
// super cost
s.resize(n+m+1);
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1);
N.resize(n+1);
N<<igl::colon<int>(0,n-1),B(j);
J.resize(B.size()-1);
// [0 1 2 3 4]
// ^
// [0 1]
// [3 4]
J.head(j) = B.head(j);
J.tail(B.size()-j-1) = B.tail(B.size()-j-1);
B(j) = n+m;
MatrixXd AJ;
igl::slice(A,J,2,AJ);
const VectorXd a = b - AJ.rowwise().sum();
{
MatrixXd old_A = A;
A.resize(A.rows(),A.cols()+a.cols());
A<<old_A,a;
}
D.col(j) = -a/a(j);
D(j,j) = 1./a(j);
}else if(k==m)
{
phase = 2;
xb = b;
s.resize(c.size()+m);
// cost function
s<<c,VectorXd::Zero(m);
}else //k = 0 or bmin >=0
{
phase = 1;
xb = b.array().abs();
s.resize(n+m);
// super cost
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k);
}
while(phase<3)
{
double df = -1;
int t = std::numeric_limits<int>::max();
// Lagrange mutipliers fro Ax=b
VectorXd yb = D.transpose() * igl::slice(s,B);
while(true)
{
if(MAXIT>0 && it>=MAXIT)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! maximum iterations without convergence."<<endl;
#endif
success = false;
break;
}
// no freedom for minimization
if(N.size() == 0)
{
break;
}
// reduced costs
VectorXd sN = igl::slice(s,N);
MatrixXd AN = igl::slice(A,N,2);
VectorXd r = sN - AN.transpose() * yb;
int q;
// determine new basic variable
double rmin = r.minCoeff(&q);
// optimal! infinity norm
if(rmin>=-tol*(sN.array().abs().maxCoeff()+1))
{
break;
}
// increment iteration count
it++;
// apply Bland's rule to avoid cycling
if(df>=0)
{
if(MAXIT == -1)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! degenerate vertex"<<endl;
#endif
success = false;
}
igl::find((r.array()<0).eval(),J);
double Nq = igl::slice(N,J).minCoeff();
// again seems like q is assumed to be a scalar though matlab code
// could produce a vector for multiple matches
(N.array()==Nq).cast<int>().maxCoeff(&q);
}
VectorXd d = D*A.col(N(q));
VectorXi I;
igl::find((d.array()>tol).eval(),I);
if(I.size() == 0)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! solution is unbounded"<<endl;
#endif
// This seems dubious:
it=-it;
success = false;
break;
}
VectorXd xbd = igl::slice(xb,I).array()/igl::slice(d,I).array();
// new use of r
int p;
{
double r;
r = xbd.minCoeff(&p);
p = I(p);
// apply Bland's rule to avoid cycling
if(df>=0)
{
igl::find((xbd.array()==r).eval(),J);
double Bp = igl::slice(B,igl::slice(I,J)).minCoeff();
// idiotic way of finding index in B of Bp
// code down the line seems to assume p is a scalar though the matlab
// code could find a vector of matches)
(B.array()==Bp).cast<int>().maxCoeff(&p);
}
// update x
xb -= r*d;
xb(p) = r;
// change in f
df = r*rmin;
}
// row vector
RowVectorXd v = D.row(p)/d(p);
yb += v.transpose() * (s(N(q)) - d.transpose()*igl::slice(s,B));
d(p)-=1;
// update inverse basis matrix
D = D - d*v;
t = B(p);
B(p) = N(q);
if(t>(n+k-1))
{
// remove qth entry from N
VectorXi old_N = N;
N.resize(N.size()-1);
N.head(q) = old_N.head(q);
N.head(q) = old_N.head(q);
N.tail(old_N.size()-q-1) = old_N.tail(old_N.size()-q-1);
}else
{
N(q) = t;
}
}
// iterative refinement
xb = (xb+D*(b-igl::slice(A,B,2)*xb)).eval();
// must be due to rounding
VectorXi I;
igl::find((xb.array()<0).eval(),I);
if(I.size()>0)
{
// so correct
VectorXd Z = VectorXd::Zero(I.size(),1);
igl::slice_into(Z,I,xb);
}
// B, xb,n,m,res=A(:,B)*xb-b
if(phase == 2 || it<0)
{
break;
}
if(xb.transpose()*igl::slice(s,B) > tol)
{
it = -it;
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning, no feasible solution"<<endl;
#endif
success = false;
break;
}
// re-initialize for Phase 2
phase = phase+1;
s*=1e6*c.array().abs().maxCoeff();
s.head(n) = c;
}
x.resize(std::max(B.maxCoeff()+1,n));
igl::slice_into(xb,B,x);
x = x.head(n).eval();
return success;
}
void QPSolver::convertToConicProblem(ConicSolver& solver)
{
const MatrixXd& F = mLhs;
int newNumVar = mNumVar + 1 + 1 + F.rows(); //x,v,w,t
int newNumCon = mNumCon + F.rows() + 1 + 2;
VectorXd newC = VectorXd::Zero(newNumVar);
newC.head(mNumVar) = mc;
newC(mNumVar) = 1.0;
solver.SetLinearTerm(newC);
MatrixXd newA = MatrixXd::Zero(newNumCon, newNumVar);
VectorXd newLbc = VectorXd::Zero(newNumCon);
VectorXd newUbc = VectorXd::Zero(newNumCon);
VectorXi newIsConstraintLowerBounded = VectorXi::Zero(newNumCon);
VectorXi newIsConstraintUpperBounded = VectorXi::Zero(newNumCon);
if (mNumCon)
{
newA.block(0, 0, mNumCon, mNumVar) = mA;
newLbc.head(mNumCon) = mlbc;
newUbc.head(mNumCon) = mubc;
newIsConstraintLowerBounded.head(mNumCon) = mbConstraintLowerBounded;
newIsConstraintUpperBounded.head(mNumCon) = mbConstraintUpperBounded;
}
newA.block(mNumCon, 0, F.rows(), mNumVar) = F;
newA.block(mNumCon, newNumVar - F.rows(), F.rows(), F.rows()) = -MatrixXd::Identity(F.rows(), F.rows());
newA(mNumCon + F.rows(), mNumVar + 1) = 1.0;
newA(mNumCon + F.rows() + 1, mNumVar) = 1.0;
newA(mNumCon + F.rows() + 2, mNumVar + 1) = 1.0;
newLbc.segment(mNumCon, F.rows()) = VectorXd::Zero(F.rows());
newLbc(mNumCon + F.rows()) = 1.0;
newUbc.segment(mNumCon, F.rows()) = VectorXd::Zero(F.rows());
newUbc(mNumCon + F.rows()) = 1.0;
newIsConstraintLowerBounded.segment(mNumCon, F.rows()) = VectorXi::Constant(F.rows(), 1);
newIsConstraintLowerBounded.tail(3) = VectorXi::Constant(3, 1);
newIsConstraintUpperBounded.segment(mNumCon, F.rows()) = VectorXi::Constant(F.rows(), 1);
newIsConstraintUpperBounded(mNumCon + F.rows()) = 1;
solver.SetConstraints(newA, newLbc, newIsConstraintLowerBounded, newUbc, newIsConstraintUpperBounded);
VectorXd newLb = VectorXd::Zero(newNumVar);
VectorXd newUb = VectorXd::Zero(newNumVar);
VectorXi newBLowerBounded = VectorXi::Zero(newNumVar);
VectorXi newBUpperBounded = VectorXi::Zero(newNumVar);
solver.SetLowerBounds(newLb, newBLowerBounded);
solver.SetUpperBounds(newUb, newBUpperBounded);
Cone cone;
for (int i = mNumVar; i < newNumVar; ++i)
{
cone.mSubscripts.push_back(i);
}
solver.AddCone(cone);
}
