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) */ }