/*y = (RHO_X * I + A'A)x */
static void matVec(const AMatrix * A, const Settings * s, Priv * p, const scs_float * x, scs_float * y) {
scs_float * tmp = p->tmp;
memset(tmp, 0, A->m * sizeof(scs_float));
accumByA(A, p, x, tmp);
memset(y, 0, A->n * sizeof(scs_float));
accumByAtrans(A, p, tmp, y);
addScaledArray(y, x, A->n, s->rho_x);
}
static pfloat calcPrimalResid(Data * d, Work * w, pfloat * x, pfloat * s, pfloat tau, pfloat *nmAxs) {
idxint i;
pfloat pres = 0, scale, *pr = w->pr, *D = w->D;
*nmAxs = 0;
memset(pr, 0, d->m * sizeof(pfloat));
accumByA(d, w->p, x, pr);
addScaledArray(pr, s, d->m, 1.0); /* pr = Ax + s */
for (i = 0; i < d->m; ++i) {
scale = d->NORMALIZE ? D[i] / (w->sc_b * d->SCALE) : 1;
scale = scale * scale;
*nmAxs += (pr[i] * pr[i]) * scale;
pres += (pr[i] - d->b[i] * tau) * (pr[i] - d->b[i] * tau) * scale;
}
*nmAxs = SQRTF(*nmAxs);
return SQRTF(pres); /* norm(Ax + s - b * tau) */
}
scs_int solveLinSys(const AMatrix * A, const Settings * stgs, Priv * p, scs_float * b, const scs_float * s, scs_int iter) {
scs_int cgIts;
scs_float cgTol = calcNorm(b, A->n)
* (iter < 0 ? CG_BEST_TOL : CG_MIN_TOL / POWF((scs_float) iter + 1, stgs->cg_rate));
tic(&linsysTimer);
/* solves Mx = b, for x but stores result in b */
/* s contains warm-start (if available) */
accumByAtrans(A, p, &(b[A->n]), b);
/* solves (I+A'A)x = b, s warm start, solution stored in b */
cgIts = pcg(A, stgs, p, s, b, A->n, MAX(cgTol, CG_BEST_TOL));
scaleArray(&(b[A->n]), -1, A->m);
accumByA(A, p, b, &(b[A->n]));
if (iter >= 0) {
totCgIts += cgIts;
}
totalSolveTime += tocq(&linsysTimer);
#if EXTRAVERBOSE > 0
scs_printf("linsys solve time: %1.2es\n", tocq(&linsysTimer) / 1e3);
#endif
return 0;
}
