static void cgCustom(Data *d, Work *w, const double * s, int max_its, double tol) {
/* solves (I+A'A)x = b */
/* warm start cg with s */
int i = 0, n = d->n;
double *x = w->p->x;
double *p = w->p->p; // cg direction
double *Ap = w->p->Ap; // updated CG direction
double *r = w->p->r; // cg residual
double *G = w->p->G; // Gram matrix
double alpha, beta, rsnew=0;
if (s==NULL) {
memset(x,0,n*sizeof(double));
}
else {
memcpy(x,s,n*sizeof(double));
cblas_dsymv(CblasColMajor, CblasUpper,n, -1, G, n, x,1, 1, r, 1);
//b_dsymv('U', n, -1, G, n, x,1, 1, r, 1);
}
memcpy(p, r, n*sizeof(double));
//double rsold=cblas_dnrm2(n,r,1);
double rsold=calcNorm(r,n);
for (i=0; i< max_its; i++) {
cblas_dsymv(CblasColMajor, CblasUpper,n, 1, G, n, p, 1, 0, Ap, 1);
//b_dsymv('U', n, 1, G, n, p, 1, 0, Ap, 1);
//beta = cblas_ddot(n, p, 1, Ap, 1);
beta = innerProd(p,Ap,n);
alpha=(rsold*rsold)/beta;
addScaledArray(x,p,n,alpha);
//cblas_daxpy(n,alpha,p,1,x,1);
addScaledArray(r,Ap,n,-alpha);
//cblas_daxpy(n,-alpha,Ap,1,r,1);
//rsnew=cblas_dnrm2(n,r,1);
rsnew=calcNorm(r,n);
if (rsnew<tol) {
break;
}
scaleArray(p,(rsnew*rsnew)/(rsold*rsold),n);
//cblas_dscal(n,(rsnew*rsnew)/(rsold*rsold),p,1);
addScaledArray(p,r,n,1);
//cblas_daxpy(n,1,r,1,p,1);
rsold=rsnew;
}
//printf("terminating cg residual = %4f, took %i itns\n",rsnew,i);
}
static scs_int pcg(const AMatrix * A, const Settings * stgs, Priv * pr, const scs_float * s, scs_float * b, scs_int max_its,
scs_float tol) {
scs_int i, n = A->n;
scs_float ipzr, ipzrOld, alpha;
scs_float *p = pr->p; /* cg direction */
scs_float *Gp = pr->Gp; /* updated CG direction */
scs_float *r = pr->r; /* cg residual */
scs_float *z = pr->z; /* for preconditioning */
scs_float *M = pr->M; /* inverse diagonal preconditioner */
if (s == NULL) {
memcpy(r, b, n * sizeof(scs_float));
memset(b, 0, n * sizeof(scs_float));
} else {
matVec(A, stgs, pr, s, r);
addScaledArray(r, b, n, -1);
scaleArray(r, -1, n);
memcpy(b, s, n * sizeof(scs_float));
}
/* check to see if we need to run CG at all */
if (calcNorm(r, n) < MIN(tol, 1e-18)) {
return 0;
}
applyPreConditioner(M, z, r, n, &ipzr);
memcpy(p, z, n * sizeof(scs_float));
for (i = 0; i < max_its; ++i) {
matVec(A, stgs, pr, p, Gp);
alpha = ipzr / innerProd(p, Gp, n);
addScaledArray(b, p, n, alpha);
addScaledArray(r, Gp, n, -alpha);
if (calcNorm(r, n) < tol) {
#if EXTRAVERBOSE > 0
scs_printf("tol: %.4e, resid: %.4e, iters: %li\n", tol, calcNorm(r, n), (long) i+1);
#endif
return i + 1;
}
ipzrOld = ipzr;
applyPreConditioner(M, z, r, n, &ipzr);
scaleArray(p, ipzr / ipzrOld, n);
addScaledArray(p, z, n, 1);
}
return i;
}
static pfloat fastCalcPrimalResid(Data * d, Work * w, pfloat * nmAxs) {
idxint i, n = d->n, m = d->m;
pfloat pres = 0, scale, *pr = w->pr, *D = w->D, tau = ABS(w->u[n + m]);
*nmAxs = 0;
memcpy(pr, &(w->u[n]), m * sizeof(pfloat)); /* overwrite pr */
addScaledArray(pr, &(w->u_prev[n]), m, d->ALPHA - 2);
addScaledArray(pr, &(w->u_t[n]), m, 1 - d->ALPHA);
addScaledArray(pr, d->b, m, w->u_t[n + m]); /* pr = Ax + s */
for (i = 0; i < 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) */
}
/*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);
}
/* status < 0 indicates failure */
static idxint projectLinSys(Data * d, Work * w, idxint iter) {
/* ut = u + v */
idxint n = d->n, m = d->m, l = n + m + 1, status;
memcpy(w->u_t, w->u, l * sizeof(pfloat));
addScaledArray(w->u_t, w->v, l, 1.0);
scaleArray(w->u_t, d->RHO_X, n);
addScaledArray(w->u_t, w->h, l - 1, -w->u_t[l - 1]);
addScaledArray(w->u_t, w->h, l - 1, -innerProd(w->u_t, w->g, l - 1) / (w->gTh + 1));
scaleArray(&(w->u_t[n]), -1, m);
status = solveLinSys(d, w->p, w->u_t, w->u, iter);
w->u_t[l - 1] += innerProd(w->u_t, w->h, l - 1);
return status;
}
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) */
}
