/// Replace variables as needed
void visit(VariableExpression& e)
{
VariablePtr vold(e.var());
OptimizerImplementation::VarMap::const_iterator v = p_vmap.find(vold->name());
BOOST_ASSERT(v != p_vmap.end());
VariablePtr vnew(v->second);
BOOST_ASSERT(vold);
BOOST_ASSERT(vnew);
if (vold != vnew) {
e.var(vnew);
}
}
Matrix<double> sparse_bicgstab(SpMatrix* A, Matrix<double> b, Matrix<double> xold, double tol, int maxiter)
// Solves general linear system Ax=b using stabilized biconjugate gradient method of van der Vorst
{
Matrix<double> rold(b.rows(), 1);
Matrix<double> r(b.rows(), 1);
Matrix<double> rhat(b.rows(), 1);
Matrix<double> t(b.rows(), 1);
Matrix<double> vold(b.rows(), 1);
Matrix<double> v(b.rows(), 1);
Matrix<double> pold(b.rows(), 1);
Matrix<double> p(b.rows(), 1);
Matrix<double> s(b.rows(), 1);
Matrix<double> errdiff(b.rows(), 1);
Matrix<double> x(b.rows(), 1);
double rhoold, rho, wold, w, alpha, beta;
double error, initres;
A->multiply(xold, &t); // t = A*xold, initially (t is only a temp variable here)
rold = b - t;
initres = error = rold.l2norm();
int steps = 0;
int i;
vold.zeros(); pold.zeros();
rhat = rold;
rhoold = alpha = wold = 1.0;
while(error > tol && steps <= maxiter)
{
if(steps % 10 == 0 || steps == 1)
cout << "sparse_bicgstab(): Iteration " << steps << ": relative error = " << error << endl;
rho = rhat.dot_product(rold);
beta = rho*alpha/(rhoold*wold);
for(i = 0; i < b.rows(); i++)
p(i) = rold.get(i) + beta * (pold.get(i) - wold*vold.get(i));
A->multiply(p, &v); // v = A*p
alpha = rho / rhat.dot_product(v);
for(i = 0; i < b.rows(); i++)
s(i) = rold.get(i) - alpha*v.get(i);
if(s.dabsmax() < tol*tol)
{
x = xold + p*alpha;
break;
}
A->multiply(s, &t); // t = A*s
w = t.dot_product(s)/t.dot_product(t);
for(i = 0; i < b.rows(); i++)
{
x(i) = xold.get(i) + alpha*p.get(i) + w*s.get(i);
error += (x.get(i) - xold.get(i)) * (x.get(i)-xold.get(i));
}
error = sqrt(error);
for(i = 0; i < b.rows(); i++)
r(i) = s.get(i) - w*t.get(i);
for(i = 0; i < b.rows(); i++)
{
xold(i) = x.get(i);
rold(i) = r.get(i);
vold(i) = v.get(i);
pold(i) = p.get(i);
}
rhoold = rho;
wold = w;
steps++;
}
cout << "sparse_bicgstab(): Done. Iterations: " << steps << ", final relative error: " << error << endl;
return x;
}