bool MatrixEquation::AllSolutions_SVD(Vector& x0,Matrix& N) const
{
if(A.n < A.m) {
cout<<"Warning: matrix is overconstrained"<<endl;
}
SVDecomposition<Real> svd;
if(!svd.set(A)) return false;
svd.backSub(b,x0);
svd.getNullspace(N);
return true;
}
bool LP_InteriorPoint::Set(const LinearProgram& lp)
{
Matrix Aeq;
Vector beq;
int neq=0,nineq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i) == LinearProgram::Fixed) neq++;
else {
if(lp.HasLowerBound(lp.ConstraintType(i))) nineq++;
if(lp.HasUpperBound(lp.ConstraintType(i))) nineq++;
}
}
for(int i=0;i<lp.A.n;i++) {
if(lp.VariableType(i) == LinearProgram::Fixed) neq++;
else {
if(lp.HasLowerBound(lp.VariableType(i))) nineq++;
if(lp.HasUpperBound(lp.VariableType(i))) nineq++;
}
}
if(neq == 0) {
x0.clear();
N.clear();
((LinearProgram&)solver) = lp;
//solver.minimize is ignored by the solver
if(!solver.minimize) solver.c.inplaceNegative();
return true;
}
Aeq.resize(neq,lp.A.n);
beq.resize(neq);
neq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i)==LinearProgram::Fixed)
{
Vector Ai;
lp.A.getRowRef(i,Ai);
Aeq.copyRow(neq,Ai);
beq(neq) = lp.p(i);
neq++;
}
}
for (int i=0;i<lp.A.n;i++) {
if(lp.VariableType(i)==LinearProgram::Fixed)
{
Vector Aeqi;
Aeq.getRowRef(i,Aeqi);
Aeqi.setZero();
Aeqi(i) = One;
beq(neq) = lp.l(i);
neq++;
}
}
SVDecomposition<Real> svd;
if(!svd.set(Aeq)) {
if(solver.verbose>=1) cout<<"LP_InteriorPoint: Couldn't set SVD of equality constraints!!!"<<endl;
return false;
}
svd.backSub(beq,x0);
svd.getNullspace(N);
//Set the solver to use the new variable y
if(N.n == 0) { //overconstrained!
cout<<"Overconstrained!"<<endl;
solver.Resize(0,0);
return true;
}
if(nineq == 0) {
cout<<"No inequalities!"<<endl;
abort();
return true;
}
if(solver.verbose >= 1) cout<<"LP_InteriorPoint: Decomposed the problem from "<<lp.A.n<<" to "<<N.n<<" variables"<<endl;
solver.Resize(nineq,N.n);
//objective
foffset = dot(lp.c,x0);
//c is such that c'*y = lp.c'*N*y => c = N'*lp.c
N.mulTranspose(lp.c,solver.c);
solver.minimize = lp.minimize;
if(!solver.minimize) solver.c.inplaceNegative();
//inequality constraints
//q <= Aineq*x <= p
//q <= Aineq*x0 + Aineq*N*y <= p
//q - Aineq*x0 <= Aineq*N*y <= p-Aineq*x0
//==> -Aineq*N*y <= -q + Aineq*x0
nineq=0;
for(int i=0;i<lp.A.m;i++) {
if(lp.ConstraintType(i)==LinearProgram::Fixed) continue;
if(lp.HasUpperBound(lp.ConstraintType(i))) {
Vector Ai,sAi;
lp.A.getRowRef(i,Ai);
solver.A.getRowRef(nineq,sAi);
N.mulTranspose(Ai,sAi);
solver.p(nineq) = lp.p(i) - dot(Ai,x0);
nineq++;
}
if(lp.HasLowerBound(lp.ConstraintType(i))) {
Vector Ai,sAi;
lp.A.getRowRef(i,Ai);
solver.A.getRowRef(nineq,sAi);
N.mulTranspose(Ai,sAi);
sAi.inplaceNegative();
solver.p(nineq) = dot(Ai,x0) - lp.q(i);
nineq++;
}
}
//transform bounds to inequality constraints
for(int i=0;i<lp.u.n;i++) {
if(lp.VariableType(i)==LinearProgram::Fixed) continue;
if(lp.HasLowerBound(lp.VariableType(i))) {
//-xi < -li
//-ei'*N*y <= -li+ei'*x0
Vector Ni,sAi;
N.getRowRef(i,Ni);
solver.A.getRowRef(nineq,sAi);
sAi.setNegative(Ni);
solver.p(nineq) = -lp.l(i) + x0(i);
nineq++;
}
if(lp.HasUpperBound(lp.VariableType(i))) {
//xi < ui
//ei'*N*y <= ui-ei'*x0
Vector Ni,sAi;
N.getRowRef(i,Ni);
solver.A.getRowRef(nineq,sAi);
sAi.copy(Ni);
solver.p(nineq) = lp.u(i) - x0(i);
nineq++;
}
}
Assert(solver.IsValid());
return true;
}
