vec pgs::solve(const vec& q, iter it) const {
vec res = vec::Zero(q.size());
it.go([&] {
real eps = 0;
res.each([&](natural i) {
real old = res(i);
res(i) -= (q(i) + M.col(i).dot(res)) / M(i, i);
// projection
res(i) = std::max(0.0, res(i));
real delta = res(i) - old;
eps += delta * delta;
});
return std::sqrt( eps );
});
return res;
}
vec solve(const vec& c, const vec& b, const iter& it) {
natural m = c.size();
natural n = b.size();
vec at = vec::Zero( m + n );
vec delta = vec::Zero( m + n );
vec eps = vec::Constant(n, it.epsilon);
vec log_eps = eps.array().log();
vec exp;
auto kkt = [&](const math::vec& dx) {
assert(!nan(dx));
vec res = vec::Zero(m + n);
res.head(m) = Q * dx.head(m) - A.transpose() * exp.cwiseProduct( dx.tail(n) );
res.tail(n) = -exp.cwiseProduct( A * dx.head(m) ) - eps.cwiseProduct(dx.tail(n));
// core::log("res:", res.transpose());
assert(!nan(res));
return res;
};
vec rhs = vec::Zero( m + n );
math::iter sub;
sub.bound = 1 + std::log(m + n);
sub.bound = it.bound;
math::natural k = 0;
it.go([&] {
exp = at.tail(n).array().exp();
assert(!nan(exp));
rhs << -c - Q * at.head(m) + A.transpose() * exp,
-exp.cwiseProduct( b - A * at.head(m) ) - eps;
assert(!nan(rhs));
delta = math::minres(kkt).solve(rhs, sub);
at += delta;
++k;
return rhs.norm();
});
return at.head(m);
}
