void SteppableLinearSolver::takeOneStep() {
if (_done) return;
++i;
A->matVecMult(d, t);
u = vecDot(n, d, t);
if (u == 0) {
printf("(SolveConjGrad) d'Ad = 0\n");
_done = true;
return;
}
// How far should we go?
alpha = rSqrLen / u;
//printf("here %f %f\n",rSqrLen, u);
// Take a step along direction d
vecAssign(n, temp, d);
vecTimesScalar(n, temp, alpha);
vecAddEqual(n, x, temp);
//printf("next length %f\n",vecSqrLen(n,x));
if (i & 0x3F) {
vecAssign(n, temp, t);
vecTimesScalar(n, temp, alpha);
vecDiffEqual(n, r, temp);
} else {
// For stability, correct r every 64th iteration
vecAssign(n, r, b);
A->matVecMult(x, temp);
vecDiffEqual(n, r, temp);
}
rSqrLenOld = rSqrLen;
rSqrLen = vecSqrLen(n,r);
// Converged! Let's get out of here
if (rSqrLen <= epsilon) {
_done = true;
return;
}
else {
//printf("Iteration %d, error %f\n",i, rSqrLen);
//fflush(fp);
}
vecAssign(n,temp,r);
P->precondVec(temp);
rSqrLen = vecDot(n,temp,r);
// Change direction: d = r + beta * d
beta = rSqrLen/rSqrLenOld;
vecTimesScalar(n, d, beta);
vecAddEqual(n, d, temp);
}
double ConjGrad(int n, implicitMatrix *A, double x[], double b[],
double epsilon, // how low should we go?
int *steps)
{
int i, iMax;
double alpha, beta, rSqrLen, rSqrLenOld, u;
double *r = (double *) malloc(sizeof(double) * n);
double *d = (double *) malloc(sizeof(double) * n);
double *t = (double *) malloc(sizeof(double) * n);
double *temp = (double *) malloc(sizeof(double) * n);
vecAssign(n, x, b);
vecAssign(n, r, b);
A->matVecMult(x, temp);
vecDiffEqual(n, r, temp);
rSqrLen = vecSqrLen(n, r);
vecAssign(n, d, r);
i = 0;
if (*steps)
iMax = *steps;
else
iMax = MAX_STEPS;
if (rSqrLen > epsilon)
while (i < iMax) {
i++;
A->matVecMult(d, t);
u = vecDot(n, d, t);
if (u == 0) {
printf("(SolveConjGrad) d'Ad = 0\n");
break;
}
// How far should we go?
alpha = rSqrLen / u;
// Take a step along direction d
vecAssign(n, temp, d);
vecTimesScalar(n, temp, alpha);
vecAddEqual(n, x, temp);
if (i & 0x3F) {
vecAssign(n, temp, t);
vecTimesScalar(n, temp, alpha);
vecDiffEqual(n, r, temp);
} else {
// For stability, correct r every 64th iteration
vecAssign(n, r, b);
A->matVecMult(x, temp);
vecDiffEqual(n, r, temp);
}
rSqrLenOld = rSqrLen;
rSqrLen = vecSqrLen(n, r);
// Converged! Let's get out of here
if (rSqrLen <= epsilon)
break;
// Change direction: d = r + beta * d
beta = rSqrLen/rSqrLenOld;
vecTimesScalar(n, d, beta);
vecAddEqual(n, d, r);
}
// free memory
free(r);
free(d);
free(t);
free(temp);
*steps = i;
return(rSqrLen);
}