void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x)
{
int iter = 0;
int m = A.size().height;
int n = A.size().width;
Mat_<double> AT = A.t();
double error = 1e-8;
Mat_<double> H = AT*A;
Mat_<double> f = -AT*b;
Mat_<double> x_old = Mat_<double>::zeros(n,1);
Mat_<double> x_new = Mat_<double>::zeros(n,1);
Mat_<double> mu_old = Mat_<double>::zeros(n,1);
Mat_<double> mu_new = Mat_<double>::zeros(n,1);
Mat_<double> r = Mat_<double>::zeros(n,1);
f.copyTo(mu_old);
while(iter < NNLS_MAX_ITER)
{
iter++;
for(int k=0;k<n;k++)
{
x_old.copyTo(x_new);
x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) );
if(x_new(k,0) != x_old(k,0))
{
r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k);
r.copyTo(mu_new);
}
x_new.copyTo(x_old);
mu_new.copyTo(mu_old);
}
if(eKKT(H,f,x_new,error) == true)
{
break;
}
}
x_new.copyTo(x);
}
void LcpSolverGaussSeidel<T>::Solve()
{
//MatrixNxN<T> &A=*m_matM;
MatrixCSR<T> &A = *m_matMCSR;
VectorN<T> &b=*m_vQ;
VectorN<T> &x=*m_vZ;
int n = A.m_iN;
VectorN<T> x_old(n);
T delta;
int i;
for(iterationsUsed_=0; iterationsUsed_<m_iMaxIterations; ++iterationsUsed_)
{
//loop over the rows
for(i=0; i<n; ++i)
{
T inv_aii=1.0;
delta = 0.0;
//loop over column
for(int j(0); j<A.m_iRowPtr[i+1]-A.m_iRowPtr[i]; ++j)
{
//get the index into the values array
int index = A.m_iRowPtr[i]+j;
//get the column index
int col = A.m_iColInd[index];
//if this is the diagonal element
if(col==i)
inv_aii/=A.m_dValues[index];
else
{
delta += A.m_dValues[index] * x(col);
}
}
//compute the update
delta=inv_aii*(b(i)-delta);
//backup the old solution
x_old(i)=x(i);
//update the solution
x(i)=x(i) + m_dOmega*(delta - x(i));
//projection step
if(x(i) < 0.0)x(i)=0.0;
}
//check the residual
m_dResidual = VectorN<T>::CompNorm(x,x_old);
//T dResidual_inf = VectorN<T>::CompMaxNorm(x, x_old);
//std::cout << "Residual L2: " << m_dResidual << std:endl;
//if (iterationsUsed_ == 0)
//{
// printf("Initial Residual L2: %E\n", m_dResidual);
// printf("Initial Residual L-inf: %E\n", dResidual_inf);
// printf("Desired Residual L2: %E\n", m_dResidual*0.0001);
//}
//std::cout << "Residual L2: " << std::endl;
//std::cout << "Residual MaxNorm: " << x.CompMaxNorm() << std:endl;
//if(m_dResidual < 1e-8 || iterationsUsed_ == m_iMaxIterations-1)
if (m_dResidual < 1e-8)
{
//printf("Final Residual L2: %E\n", m_dResidual);
//printf("Final Residual L-inf: %E\n", dResidual_inf);
//A.outputToFile("meshes/matrix.dat");
//return;
break;
}
}
printf("Final Residual L2: %E\n", m_dResidual);
}
double QuadProg::solve_quadprog(Matrix<double>& G, Vector<double>& g0,
const Matrix<double>& CE, const Vector<double>& ce0,
const Matrix<double>& CI, const Vector<double>& ci0,
Vector<double>& x)
{
std::ostringstream msg;
{
//Ensure that the dimensions of the matrices and vectors can be
//safely converted from unsigned int into to int without overflow.
unsigned mx = std::numeric_limits<int>::max();
if(G.ncols() >= mx || G.nrows() >= mx ||
CE.nrows() >= mx || CE.ncols() >= mx ||
CI.nrows() >= mx || CI.ncols() >= mx ||
ci0.size() >= mx || ce0.size() >= mx || g0.size() >= mx){
msg << "The dimensions of one of the input matrices or vectors were "
<< "too large." << std::endl
<< "The maximum allowable size for inputs to solve_quadprog is:"
<< mx << std::endl;
throw std::logic_error(msg.str());
}
}
int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
if ((int)G.nrows() != n)
{
msg << "The matrix G is not a square matrix (" << G.nrows() << " x "
<< G.ncols() << ")";
throw std::logic_error(msg.str());
}
if ((int)CE.nrows() != n)
{
msg << "The matrix CE is incompatible (incorrect number of rows "
<< CE.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if ((int)ce0.size() != p)
{
msg << "The vector ce0 is incompatible (incorrect dimension "
<< ce0.size() << ", expecting " << p << ")";
throw std::logic_error(msg.str());
}
if ((int)CI.nrows() != n)
{
msg << "The matrix CI is incompatible (incorrect number of rows "
<< CI.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if ((int)ci0.size() != m)
{
msg << "The vector ci0 is incompatible (incorrect dimension "
<< ci0.size() << ", expecting " << m << ")";
throw std::logic_error(msg.str());
}
x.resize(n);
register int i, j, k, l; /* indices */
int ip; // this is the index of the constraint to be added to the active set
Matrix<double> R(n, n), J(n, n);
Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
double inf;
if (std::numeric_limits<double>::has_infinity)
inf = std::numeric_limits<double>::infinity();
else
inf = 1.0E300;
double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
* and the full step length t2 */
Vector<int> A(m + p), A_old(m + p), iai(m + p);
int q, iq, iter = 0;
Vector<bool> iaexcl(m + p);
/* p is the number of equality constraints */
/* m is the number of inequality constraints */
q = 0; /* size of the active set A (containing the indices of the active constraints) */
#ifdef TRACE_SOLVER
std::cout << std::endl << "Starting solve_quadprog" << std::endl;
print_matrix("G", G);
print_vector("g0", g0);
print_matrix("CE", CE);
print_vector("ce0", ce0);
print_matrix("CI", CI);
print_vector("ci0", ci0);
#endif
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = 0.0;
for (i = 0; i < n; i++)
{
c1 += G[i][i];
}
/* decompose the matrix G in the form L^T L */
cholesky_decomposition(G);
#ifdef TRACE_SOLVER
print_matrix("G", G);
#endif
/* initialize the matrix R */
for (i = 0; i < n; i++)
{
d[i] = 0.0;
for (j = 0; j < n; j++)
R[i][j] = 0.0;
}
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
c2 = 0.0;
for (i = 0; i < n; i++)
{
d[i] = 1.0;
forward_elimination(G, z, d);
for (j = 0; j < n; j++)
J[i][j] = z[j];
c2 += z[i];
d[i] = 0.0;
}
#ifdef TRACE_SOLVER
print_matrix("J", J);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
cholesky_solve(G, x, g0);
for (i = 0; i < n; i++)
x[i] = -x[i];
/* and compute the current solution value */
f_value = 0.5 * scalar_product(g0, x);
#ifdef TRACE_SOLVER
std::cout << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < p; i++)
{
for (j = 0; j < n; j++)
np[j] = CE[j][i];
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, n, iq);
print_vector("z", z);
print_vector("r", r, iq);
print_vector("d", d);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
/* set x = x + t2 * z */
for (k = 0; k < n; k++)
x[k] += t2 * z[k];
/* set u = u+ */
u[iq] = t2;
for (k = 0; k < iq; k++)
u[k] -= t2 * r[k];
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
A[i] = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// Equality constraints are linearly dependent
throw std::runtime_error("Constraints are linearly dependent");
return f_value;
}
}
/* set iai = K \ A */
for (i = 0; i < m; i++)
iai[i] = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x);
#endif
/* step 1: choose a violated constraint */
for (i = p; i < iq; i++)
{
ip = A[i];
iai[ip] = -1;
}
/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < m; i++)
{
iaexcl[i] = true;
sum = 0.0;
for (j = 0; j < n; j++)
sum += CI[j][i] * x[j];
sum += ci0[i];
s[i] = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
return f_value;
}
/* save old values for u and A */
for (i = 0; i < iq; i++)
{
u_old[i] = u[i];
A_old[i] = A[i];
}
/* and for x */
for (i = 0; i < n; i++)
x_old[i] = x[i];
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < m; i++)
{
if (s[i] < ss && iai[i] != -1 && iaexcl[i])
{
ss = s[i];
ip = i;
}
}
if (ss >= 0.0)
{
q = iq;
return f_value;
}
/* set np = n[ip] */
for (i = 0; i < n; i++)
np[i] = CI[i][ip];
/* set u = [u 0]^T */
u[iq] = 0.0;
/* add ip to the active set A */
A[iq] = ip;
#ifdef TRACE_SOLVER
std::cout << "Trying with constraint " << ip << std::endl;
print_vector("np", np);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
print_vector("z", z);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d);
print_vector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+[x] / r */
for (k = p; k < iq; k++)
{
if (r[k] > 0.0)
{
if (u[k] / r[k] < t1)
{
t1 = u[k] / r[k];
l = A[k];
}
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s[ip] / scalar_product(z, np);
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q = iq;
return inf;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1] and drop constraint l from the active set A */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
std::cout << " in dual space: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("z", z);
print_vector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
/* set x = x + t * z */
for (k = 0; k < n; k++)
x[k] += t * z[k];
/* update the solution value */
f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
/* u = u + t * [-r 1] */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
#ifdef TRACE_SOLVER
std::cout << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_vector("A", A, iq + 1);
#endif
if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
{
#ifdef TRACE_SOLVER
std::cout << "Full step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, n, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
for (i = 0; i < m; i++)
iai[i] = i;
for (i = p; i < iq; i++)
{
A[i] = A_old[i];
u[i] = u_old[i];
iai[A[i]] = -1;
}
for (i = 0; i < n; i++)
x[i] = x_old[i];
goto l2; /* go to step 2 */
}
else
iai[ip] = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cout << "Partial step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* drop constraint l */
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
#endif
/* update s[ip] = CI * x + ci0 */
sum = 0.0;
for (k = 0; k < n; k++)
sum += CI[k][ip] * x[k];
s[ip] = sum + ci0[ip];
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
goto l2a;
}
void mover( Matrix& x, Matrix& v, int npart, double L,
double mpv, double vwall, double tau,
Matrix& strikes, Matrix& delv, long& seed ) {
// mover - Function to move particles by free flight
// Also handles collisions with walls
// Inputs
// x Positions of the particles
// v Velocities of the particles
// npart Number of particles in the system
// L System length
// mpv Most probable velocity off the wall
// vwall Wall velocities
// tau Time step
// seed Random number seed
// Outputs
// x,v Updated positions and velocities
// strikes Number of particles striking each wall
// delv Change of y-velocity at each wall
// seed Random number seed
//* Move all particles pretending walls are absent
Matrix x_old(npart);
x_old = x; // Remember original position
int i;
for( i=1; i<= npart; i++ )
x(i) = x_old(i) + v(i,1)*tau;
//* Check each particle to see if it strikes a wall
strikes.set(0.0); delv.set(0.0);
Matrix xwall(2), vw(2), direction(2);
xwall(1) = 0; xwall(2) = L; // Positions of walls
vw(1) = -vwall; vw(2) = vwall; // Velocities of walls
double stdev = mpv/sqrt(2.);
// Direction of particle leaving wall
direction(1) = 1; direction(2) = -1;
for( i=1; i<=npart; i++ ) {
//* Test if particle strikes either wall
int flag = 0;
if( x(i) <= 0 )
flag=1; // Particle strikes left wall
else if( x(i) >= L )
flag=2; // Particle strikes right wall
//* If particle strikes a wall, reset its position
// and velocity. Record velocity change.
if( flag > 0 ) {
strikes(flag)++;
double vyInitial = v(i,2);
//* Reset velocity components as biased Maxwellian,
// Exponential dist. in x; Gaussian in y and z
v(i,1) = direction(flag)*sqrt(-log(1.-rand(seed))) * mpv;
v(i,2) = stdev*randn(seed) + vw(flag); // Add wall velocity
v(i,3) = stdev*randn(seed);
// Time of flight after leaving wall
double dtr = tau*(x(i)-xwall(flag))/(x(i)-x_old(i));
//* Reset position after leaving wall
x(i) = xwall(flag) + v(i,1)*dtr;
//* Record velocity change for force measurement
delv(flag) += (v(i,2) - vyInitial);
}
}
}
void TV::IterativeReconstruction(CVector &data_gpu, CVector &x, CVector &b1_gpu)
{
unsigned N = width * height * frames;
ComputeTimeSpaceWeights(params.timeSpaceWeight, params.ds, params.dt);
Log("Setting ds: %.3e, dt: %.3e\n", params.ds, params.dt);
Log("Setting Primal-Dual Gap of %.3e as stopping criterion \n", params.stopPDGap);
// primal
CVector x_old(N);
CVector ext(N);
agile::copy(x, ext);
// dual
std::vector<CVector> y;
y.push_back(CVector(N));
y.push_back(CVector(N));
y.push_back(CVector(N));
y[0].assign(N, 0);
y[1].assign(N, 0);
y[2].assign(N, 0);
std::vector<CVector> tempGradient;
tempGradient.push_back(CVector(N));
tempGradient.push_back(CVector(N));
tempGradient.push_back(CVector(N));
CVector z(data_gpu.size());
zTemp.resize(data_gpu.size(), 0.0);
z.assign(z.size(), 0.0);
CVector norm(N);
unsigned loopCnt = 0;
// loop
Log("Starting iteration\n");
while ( loopCnt < params.maxIt )
{
// dual ascent step
utils::Gradient(ext, tempGradient, width, height, params.ds, params.ds,
params.dt);
agile::addScaledVector(y[0], params.sigma, tempGradient[0], y[0]);
agile::addScaledVector(y[1], params.sigma, tempGradient[1], y[1]);
agile::addScaledVector(y[2], params.sigma, tempGradient[2], y[2]);
mrOp->BackwardOperation(ext, zTemp, b1_gpu);
agile::addScaledVector(z, params.sigma, zTemp, z);
// Proximal mapping
utils::ProximalMap3(y, 1.0);
agile::subScaledVector(z, params.sigma, data_gpu, z);
agile::scale((float)(1.0 / (1.0 + params.sigma / params.lambda)), z, z);
// primal descent
mrOp->ForwardOperation(z, imgTemp, b1_gpu);
utils::Divergence(y, divTemp, width, height, frames, params.ds, params.ds,
params.dt);
agile::subVector(imgTemp, divTemp, divTemp);
agile::subScaledVector(x, params.tau, divTemp, ext);
// save x_n+1
agile::copy(ext, x_old);
// extra gradient
agile::scale(2.0f, ext, ext);
agile::subVector(ext, x, ext);
// x_n = x_n+1
agile::copy(x_old, x);
// adapt step size
if (loopCnt < 10 || (loopCnt % 50 == 0))
{
CVector temp(N);
agile::subVector(ext, x, temp);
AdaptStepSize(temp, b1_gpu);
}
// compute PD Gap (export,verbose,stopping)
if ( (verbose && (loopCnt < 10 || (loopCnt % 50 == 0)) ) ||
((debug) && (loopCnt % debugstep == 0)) ||
((params.stopPDGap > 0) && (loopCnt % 20 == 0)) )
{
RType pdGap =
ComputePDGap(x, y, z, data_gpu, b1_gpu);
pdGap=pdGap/N;
pdGapExport.push_back( pdGap );
Log("Normalized Primal-Dual Gap after %d iterations: %.4e\n", loopCnt, pdGap);
if ( pdGap < params.stopPDGap )
return;
}
loopCnt++;
if (loopCnt % 10 == 0)
std::cout << "." << std::flush;
}
std::cout << std::endl;
}
inline double solve_quadprog(MatrixXd & G, VectorXd & g0,
const MatrixXd & CE, const VectorXd & ce0,
const MatrixXd & CI, const VectorXd & ci0,
VectorXd& x)
{
int i, j, k, l; /* indices */
int ip, me, mi;
int n=g0.size(); int p=ce0.size(); int m=ci0.size();
MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
LLT<MatrixXd,Lower> chol(G.cols());
VectorXd s(m+p), z(n), r(m + p), d(n), np(n), u(m + p);
VectorXd x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
const double inf = std::numeric_limits<double>::infinity();
double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
* and the full step length t2 */
VectorXi A(m + p), A_old(m + p), iai(m + p);
int q;
int iq, iter = 0;
bool iaexcl[m + p];
me = p; /* number of equality constraints */
mi = m; /* number of inequality constraints */
q = 0; /* size of the active set A (containing the indices of the active constraints) */
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = G.trace();
/* decompose the matrix G in the form LL^T */
chol.compute(G);
/* initialize the matrix R */
d.setZero();
R.setZero();
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
// J = L^-T
J.setIdentity();
J = chol.matrixU().solve(J);
c2 = J.trace();
#ifdef TRACE_SOLVER
print_matrix("J", J, n);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
x = chol.solve(g0);
x = -x;
/* and compute the current solution value */
f_value = 0.5 * g0.dot(x);
#ifdef TRACE_SOLVER
std::cerr << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x, n);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < me; i++)
{
np = CE.col(i);
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, iq);
print_vector("z", z, n);
print_vector("r", r, iq);
print_vector("d", d, n);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (internal::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
/* set u = u+ */
u(iq) = t2;
u.head(iq) -= t2 * r.head(iq);
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * z.dot(np);
A(i) = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// FIXME: it should raise an error
// Equality constraints are linearly dependent
return f_value;
}
}
/* set iai = K \ A */
for (i = 0; i < mi; i++)
iai(i) = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x, n);
#endif
/* step 1: choose a violated constraint */
for (i = me; i < iq; i++)
{
ip = A(i);
iai(ip) = -1;
}
/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < mi; i++)
{
iaexcl[i] = true;
sum = CI.col(i).dot(x) + ci0(i);
s(i) = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, mi);
#endif
if (internal::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
return f_value;
}
/* save old values for u, x and A */
u_old.head(iq) = u.head(iq);
A_old.head(iq) = A.head(iq);
x_old = x;
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < mi; i++)
{
if (s(i) < ss && iai(i) != -1 && iaexcl[i])
{
ss = s(i);
ip = i;
}
}
if (ss >= 0.0)
{
q = iq;
return f_value;
}
/* set np = n(ip) */
np = CI.col(ip);
/* set u = (u 0)^T */
u(iq) = 0.0;
/* add ip to the active set A */
A(iq) = ip;
#ifdef TRACE_SOLVER
std::cerr << "Trying with constraint " << ip << std::endl;
print_vector("np", np, n);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cerr << "Step direction z" << std::endl;
print_vector("z", z, n);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d, n);
print_ivector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
for (k = me; k < iq; k++)
{
double tmp;
if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
{
t1 = tmp;
l = A(k);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (internal::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q = iq;
return inf;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u.head(iq) -= t * r.head(iq);
u(iq) += t;
iai(l) = l;
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
std::cerr << " in dual space: "
<< f_value << std::endl;
print_vector("x", x, n);
print_vector("z", z, n);
print_ivector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
x += t * z;
/* update the solution value */
f_value += t * z.dot(np) * (0.5 * t + u(iq));
u.head(iq) -= t * r.head(iq);
u(iq) += t;
#ifdef TRACE_SOLVER
std::cerr << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x, n);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_ivector("A", A, iq + 1);
#endif
if (t == t2)
{
#ifdef TRACE_SOLVER
std::cerr << "Full step has taken " << t << std::endl;
print_vector("x", x, n);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
for (i = 0; i < m; i++)
iai(i) = i;
for (i = 0; i < iq; i++)
{
A(i) = A_old(i);
iai(A(i)) = -1;
u(i) = u_old(i);
}
x = x_old;
goto l2; /* go to step 2 */
}
else
iai(ip) = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cerr << "Partial step has taken " << t << std::endl;
print_vector("x", x, n);
#endif
/* drop constraint l */
iai(l) = l;
delete_constraint(R, J, A, u, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R, n);
print_ivector("A", A, iq);
#endif
s(ip) = CI.col(ip).dot(x) + ci0(ip);
#ifdef TRACE_SOLVER
print_vector("s", s, mi);
#endif
goto l2a;
}