/* Main program */
int main() {
/* Locals */
int n = N,lda = LDA;
int info = 0;
double a[LDA*N] = {
2, -1, 0,
-1, 2, -1,
0, -1, 2
};
/* Executable statements */
printf( "Dpotrf Example Program Results\n" );
DPOTRF(LA_UP, n, a, LDA, &info );
/* Check for the exact singularity */
if( info > 0 ) {
printf( "The diagonal element of the triangular factor of A,\n" );
printf( "U(%i,%i) is zero, so that A is singular;\n", info, info );
printf( "the solution could not be computed.\n" );
exit( 1 );
}
/* Print details of LU factorization */
print_matrix( "Details of LU factorization", n, n, a, lda );
exit( 0 );
}
void ProtoMol::Lapack::dpotrf(char *transA, int *n, double *A, int *lda,
int *info) {
FAHCheckIn();
#if defined(HAVE_LAPACK)
dpotrf_(transA, n, A, lda, info);
#elif defined(HAVE_MKL_LAPACK)
DPOTRF(transA, n, A, lda, info);
#else
THROW(std::string(__func__) + " not supported");
#endif
}
DLLEXPORT int d_cholesky_factor(int n, double* a) {
char uplo = 'L';
int info = 0;
DPOTRF(&uplo, &n, a, &n, &info);
for (int i = 0; i < n; ++i)
{
int index = i * n;
for (int j = 0; j < n && i > j; ++j)
{
a[index + j] = 0;
}
}
return info;
}
void FrictionContact2D_latin(FrictionContactProblem* problem , double *reaction , double *velocity , int *info, SolverOptions* options)
{
int nc = problem->numberOfContacts;
assert(nc>0);
double * vec = problem->M->matrix0;
double *qq = problem->q;
double * mu = problem->mu;
int info77 = 0;
int i, j, kk, iter1, ino, ddl, nrhs;
int info2 = 0;
int n = 2 * nc;
size_t idim, nbno;
int incx = 1, incy = 1;
size_t taille, taillet, taillen, itt;
int *ddln;
int *ddlt, *vectnt;
assert(n>0);
double errmax, alpha, beta, maxa, k_latin;
double aa, nt, wn, tc, zc0;
double err1, num11, err0;
double den11, den22, knz0, ktz0, *ktz, *wf;
double *wc, *zc, *wt, *maxwt, *wnum1, *znum1;
double *zt, *maxzt;
double *kn, *kt;
// char trans='T', diag='N';
// char uplo='U', notrans='N';
double *k, *DPO, *kf, *kninv;
double *kinvwden1, *kzden1, *kfinv, *knz, *wtnc;
/* Recup input */
itt = options->iparam[0];
errmax = options->dparam[0];
k_latin = options->dparam[2];
/* Initialize output */
options->iparam[1] = 0;
options->dparam[1] = 0.0;
/* Allocations */
k = (double*) malloc(n * n * sizeof(double));
DPO = (double*) malloc(n * n * sizeof(double));
kf = (double*) malloc(n * n * sizeof(double));
kfinv = (double*) malloc(n * n * sizeof(double));
kninv = (double*) malloc(nc * nc * sizeof(double));
kn = (double*) malloc(nc * nc * sizeof(double));
kt = (double*) malloc(nc * nc * sizeof(double));
kinvwden1 = (double*) malloc(n * sizeof(double));
kzden1 = (double*) malloc(n * sizeof(double));
wc = (double*) malloc(n * sizeof(double));
zc = (double*) malloc(n * sizeof(double));
znum1 = (double*) malloc(n * sizeof(double));
wnum1 = (double*) malloc(n * sizeof(double));
wt = (double*) malloc(n * sizeof(double));
maxzt = (double*) malloc(n * sizeof(double));
knz = (double*) malloc(nc * sizeof(double));
wtnc = (double*) malloc(nc * sizeof(double));
ktz = (double*) malloc(nc * sizeof(double));
wf = (double*) malloc(nc * sizeof(double));
maxwt = (double*) malloc(nc * sizeof(double));
zt = (double*) malloc(nc * sizeof(double));
vectnt = (int*) malloc(n * sizeof(int));
ddln = (int*) malloc(nc * sizeof(int));
ddlt = (int*) malloc(nc * sizeof(int));
/* Initialization */
for (i = 0; i < n * n; i++)
{
k[i] = 0.;
kf[i] = 0.;
kfinv[i] = 0.;
if (i < nc * nc)
{
kn[i] = 0.0;
kt[i] = 0.0;
kninv[i] = 0.0;
if (i < n)
{
wc[i] = 0.0;
zc[i] = 0.;
reaction[i] = 0.;
velocity[i] = 0.;
znum1[i] = 0.;
wnum1[i] = 0.;
wt[i] = 0.;
maxzt[i] = 0.;
if (i < nc)
{
maxwt[i] = 0.;
zt[i] = 0.;
knz[i] = 0.;
ktz[i] = 0.;
wf[i] = 0.;
wtnc[i] = 0.;
}
}
}
}
for (i = 0; i < n; i++)
{
if (fabs(vec[i * n + i]) < DBL_EPSILON)
{
if (verbose > 0)
printf("\n Warning nul diagonal term in M matrix \n");
free(k);
free(DPO);
free(kf);
free(kfinv);
free(kninv);
free(kn);
free(kt);
free(kinvwden1);
free(kzden1);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(wt);
free(maxzt);
free(knz);
free(wtnc);
free(ktz);
free(wf);
free(maxwt);
free(zt);
free(vectnt);
free(ddln);
free(ddlt);
*info = 3;
return;
}
else
{
k[i + n * i] = k_latin / vec[i * n + i];
vectnt[i] = i + 1;
}
}
for (i = 0; i < nc; i++)
{
ddln[i] = vectnt[2 * i];
if (i != 0) ddlt[i] = vectnt[2 * i - 1];
else ddlt[i] = 0;
}
for (i = 0; i < nc; i++)
{
kn[i + nc * i] = k[ddln[i] + n * ddln[i]];
kt[i + nc * i] = k[ddlt[i] + n * ddlt[i]];
}
taillen = sizeof(ddln) / sizeof(ddln[0]);
taillet = sizeof(ddlt) / sizeof(ddlt[0]);
idim = 1 + taillen / taillet;
taille = 0;
for (i = 0; i < n; i++)
taille = sizeof(qq[i]) + taille;
taille = taille / sizeof(qq[0]);
nbno = taille / idim;
for (i = 0; i < nc; i++)
{
kf[ddln[i] + n * ddln[i]] = kn[i + nc * i];
kf[ddlt[i] + n * ddlt[i]] = kt[i + nc * i];
}
for (i = 0; i < n; i++)
{
kfinv[i + n * i] = 1. / kf[i + n * i];
if (i < nc)
kninv[i + nc * i] = 1. / kt[i + nc * i];
}
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
DPO[i + n * j] = vec[j * n + i] + kfinv[i + n * j];
DPOTRF(LA_UP, n, DPO , n, &info2);
if (info2 != 0)
{
if (verbose > 0)
printf("\n Matter with Cholesky factorization \n");
free(k);
free(DPO);
free(kf);
free(kfinv);
free(kninv);
free(kn);
free(kt);
free(kinvwden1);
free(kzden1);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(wt);
free(maxzt);
free(knz);
free(wtnc);
free(ktz);
free(wf);
free(maxwt);
free(zt);
free(vectnt);
free(ddln);
free(ddlt);
*info = 2;
return;
}
/* Iteration loops */
iter1 = 0;
err1 = 1.;
while ((iter1 < itt) && (err1 > errmax))
{
/* Linear stage (zc,wc) -> (z,w) */
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kfinv, n, zc, incx, beta, wc, incy);
cblas_dcopy(n, qq, incx, znum1, incy);
alpha = -1.;
cblas_dscal(n , alpha , znum1 , incx);
alpha = 1.;
cblas_daxpy(n, alpha, wc, incx, znum1, incy);
nrhs = 1;
DTRTRS(LA_UP, LA_TRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info77);
DTRTRS(LA_UP, LA_NOTRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info77);
cblas_dcopy(n, znum1, incx, reaction, incy);
alpha = -1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kfinv, n, reaction, incx, beta, wc, incy);
cblas_dcopy(n, wc, incx, velocity, incy);
/* Local stage (z,w)->(zc,wc) */
for (i = 0; i < n; i++)
{
zc[i] = 0.;
wc[i] = 0.0;
}
/* Normal party */
for (i = 0; i < nc; i++)
{
knz0 = 0.;
for (kk = 0; kk < nc; kk++)
{
knz[i] = kt[i + nc * kk] * velocity[ddlt[kk]] + knz0;
knz0 = knz[i];
}
zt[i] = reaction[ddlt[i]] - knz[i];
if (zt[i] > 0.0)
{
zc[ddlt[i]] = zt[i];
maxzt[i] = 0.0;
}
else
{
zc[ddlt[i]] = 0.0;
maxzt[i] = -zt[i];
}
}
for (i = 0; i < nc; i++)
{
zc0 = 0.;
ktz0 = 0.;
for (j = 0; j < nc; j++)
{
wc[ddlt[i]] = kninv[i + nc * j] * maxzt[j] + zc0;
zc0 = wc[ddlt[i]];
ktz[i] = kn[i + nc * j] * velocity[ddln[j]] + ktz0;
ktz0 = ktz[i];
}
wf[i] = reaction[ddln[i]] - ktz[i];
}
/* Loop other nodes */
for (ino = 0; ino < nbno; ino++)
{
ddl = ddln[ino];
nt = fabs(wf[ino]);
/* Tangential vector */
if (nt < 1.e-8) tc = 0.;
else tc = wf[ino] / nt;
/* Tangentiel component */
wn = zc[ddlt[ino]];
aa = nt - mu[ino] * wn;
if (aa > 0.0)
{
maxa = aa;
}
else
{
maxa = 0.0;
}
wc[ddl] = (maxa / (-1 * kn[ino + nc * ino])) * tc;
aa = -nt + mu[ino] * wn;
if (aa > 0.0)
{
maxa = aa;
}
else
{
maxa = 0.0;
}
zc[ddl] = (mu[ino] * wn - maxa) * tc;
}
/* Convergence criterium */
cblas_dcopy(n, reaction, incx, znum1, incy);
alpha = -1.;
cblas_daxpy(n, alpha, zc, incx, znum1, incy);
cblas_dcopy(n, velocity, incx, wnum1, incy);
cblas_daxpy(n, alpha, wc, incx, wnum1, incy);
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kf, n, wnum1, incx, beta, znum1, incy);
num11 = 0.;
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kfinv, n, znum1, incx, beta, wnum1, incy);
num11 = cblas_ddot(n, wnum1, incx, znum1, incy);
cblas_dcopy(n, reaction, incx, znum1, incy);
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kf, n, velocity, incx, beta, znum1, incy);
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kfinv, n, znum1, incx, beta, wnum1, incy);
den11 = cblas_ddot(n, wnum1, incx, znum1, incy);
cblas_dcopy(n, zc, incx, znum1, incy);
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kf, n, wc, incx, beta, znum1, incy);
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, alpha, kfinv, n, znum1, incx, beta, wnum1, incy);
den22 = cblas_ddot(n, znum1, incx, wnum1, incy);
err0 = num11 / (den11 + den22);
err1 = sqrt(err0);
options->iparam[1] = iter1;
options->dparam[1] = err1;
iter1 = iter1 + 1;
}
if (err1 > errmax)
{
if (verbose > 0)
printf("No convergence after %d iterations, the residue is %g\n", iter1, err1);
*info = 1;
}
else
{
if (verbose > 0)
printf("Convergence after %d iterations, the residue is %g \n", iter1, err1);
*info = 0;
}
free(k);
free(DPO);
free(kf);
free(kfinv);
free(kninv);
free(kn);
free(kt);
free(kinvwden1);
free(kzden1);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(wt);
free(maxzt);
free(knz);
free(wtnc);
free(ktz);
free(wf);
free(maxwt);
free(zt);
free(vectnt);
free(ddln);
free(ddlt);
}
void lcp_latin(LinearComplementarityProblem* problem, double *z, double *w, int *info , SolverOptions* options)
{
/* matrix M of the lcp */
double * M = problem->M->matrix0;
/* size of the LCP */
int n = problem->size;
int n2 = n * n;
int i, j, iter1, nrhs;
int info2 = 0;
int itt, it_end;
int incx, incy;
int itermax = options->iparam[0];
double tol = options->dparam[0];
double k_latin = options->dparam[2];
double alpha, beta;
double err1;
double res, errmax;
double *wc, *zc, *kinvden1, *kinvden2, *wt;
double *maxwt, *wnum1, *znum1, *ww, *zz;
double *num1, *kinvnum1, *den1, *den2, *wden1, *zden1;
double *kinvwden1, *kzden1;
double *k, *kinv, *DPO;
// char trans='T', notrans='N', uplo='U', diag='N';
incx = 1;
incy = 1;
/* Recup input */
errmax = tol;
itt = itermax;
/* Initialize output */
options->iparam[1] = 0;
options->dparam[1] = 0.0;
/* Allocations */
ww = (double*) malloc(n * sizeof(double));
zz = (double*) malloc(n * sizeof(double));
wc = (double*) malloc(n * sizeof(double));
zc = (double*) malloc(n * sizeof(double));
znum1 = (double*) malloc(n * sizeof(double));
wnum1 = (double*) malloc(n * sizeof(double));
kinvden1 = (double*) malloc(n * sizeof(double));
kinvden2 = (double*) malloc(n * sizeof(double));
wt = (double*) malloc(n * sizeof(double));
maxwt = (double*) malloc(n * sizeof(double));
num1 = (double*) malloc(n * sizeof(double));
kinvnum1 = (double*) malloc(n * sizeof(double));
den1 = (double*) malloc(n * sizeof(double));
den2 = (double*) malloc(n * sizeof(double));
wden1 = (double*) malloc(n * sizeof(double));
zden1 = (double*) malloc(n * sizeof(double));
kinvwden1 = (double*) malloc(n * sizeof(double));
kzden1 = (double*) malloc(n * sizeof(double));
DPO = (double*) malloc(n2 * sizeof(double));
k = (double*) malloc(n2 * sizeof(double));
kinv = (double*) malloc(n2 * sizeof(double));
/* Initialization */
for (i = 0; i < n2; i++)
{
if (i < n)
{
wc[i] = 0.0;
zc[i] = 0.0;
z[i] = 0.0;
w[i] = 0.0;
znum1[i] = 0.0;
wnum1[i] = 0.0;
kinvden1[i] = 0.0;
kinvden2[i] = 0.0;
wt[i] = 0.0;
maxwt[i] = 0.0;
num1[i] = 0.0;
kinvnum1[i] = 0.0;
den1[i] = 0.0;
den2[i] = 0.0;
}
k[i] = 0.0;
kinv[i] = 0.0;
DPO[i] = 0.0;
}
for (i = 0 ; i < n ; i++)
{
k[i * n + i] = k_latin * M[i * n + i];
if (fabs(k[i * n + i]) < DBL_EPSILON)
{
if (verbose > 0)
{
printf(" Warning nul diagonal term in k matrix \n");
}
free(ww);
free(zz);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(kinvden1);
free(kinvden2);
free(wt);
free(maxwt);
free(num1);
free(kinvnum1);
free(den1);
free(den2);
free(wden1);
free(zden1);
free(kinvwden1);
free(kzden1);
free(DPO);
free(k);
free(kinv);
*info = 3;
return;
}
else
kinv[i + n * i] = 1.0 / k[i + n * i];
}
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
DPO[i + n * j] = M[j * n + i] + k[i + n * j];
/* Cholesky */
DPOTRF(LA_UP, n, DPO , n, &info2);
if (info2 != 0)
{
printf(" Matter with Cholesky Factorization \n ");
free(ww);
free(zz);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(kinvden1);
free(kinvden2);
free(wt);
free(maxwt);
free(num1);
free(kinvnum1);
free(den1);
free(den2);
free(wden1);
free(zden1);
free(kinvwden1);
free(kzden1);
free(DPO);
free(k);
free(kinv);
*info = 2;
return;
}
/* End of Cholesky */
/* Iteration loops */
iter1 = 0;
err1 = 1.;
while ((iter1 < itt) && (err1 > errmax))
{
/* Linear stage (zc,wc) -> (z,w)*/
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, zc, incx, beta, wc, incy);
cblas_dcopy(n, problem->q, incx, znum1, incy);
alpha = -1.;
cblas_dscal(n , alpha , znum1 , incx);
alpha = 1.;
cblas_daxpy(n, alpha, wc, incx, znum1, incy);
nrhs = 1;
DTRTRS(LA_UP, LA_TRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info2);
DTRTRS(LA_UP, LA_NOTRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info2);
cblas_dcopy(n, znum1, incx, z, incy);
alpha = -1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, z, incx, beta, wc, incy);
cblas_dcopy(n, wc, incx, w, incy);
/* Local Stage */
cblas_dcopy(n, w, incx, wt, incy);
alpha = -1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, z, incx, beta, wt, incy);
for (i = 0; i < n; i++)
{
if (wt[i] > 0.0)
{
wc[i] = wt[i];
zc[i] = 0.0;
}
else
{
wc[i] = 0.0;
zc[i] = -wt[i] / k[i + n * i];
}
}
/* Convergence criterium */
cblas_dcopy(n, w, incx, wnum1, incy);
alpha = -1.;
cblas_daxpy(n, alpha, wc, incx, wnum1, incy);
cblas_dcopy(n, z, incx, znum1, incy);
cblas_daxpy(n, alpha, zc, incx, znum1, incy);
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, znum1, incx, beta, wnum1, incy);
/* wnum1(:) =(w(:)-wc(:))+matmul( k(:,:),(z(:)-zc(:))) */
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, kinv, n, wnum1, incx, beta, kinvnum1, incy);
cblas_dcopy(n, z, incx, zz, incy);
cblas_dcopy(n, w, incx, ww, incy);
alpha = 1.;
cblas_daxpy(n, alpha, wc, incx, ww, incy);
cblas_daxpy(n, alpha, zc, incx, zz, incy);
beta = 0.;
alpha = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, zz, incx, beta, kzden1, incy);
beta = 0.;
alpha = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, kinv, n, ww, incx, beta, kinvwden1, incy);
lcp_compute_error_only(n, z, w, &err1);
it_end = iter1;
res = err1;
iter1 = iter1 + 1;
options->iparam[1] = it_end;
options->dparam[1] = res;
}
if (isnan(err1) || (err1 > errmax))
{
if (verbose > 0) printf("No convergence of LATIN after %d iterations, the residue is %g\n", iter1, err1);
*info = 1;
}
else
{
if (verbose > 0) printf("Convergence of LATIN after %d iterations, the residue is %g \n", iter1, err1);
*info = 0;
}
free(wc);
free(DPO);
free(k);
free(kinv);
free(zz);
free(ww);
free(zc);
free(znum1);
free(wnum1);
free(kinvden1);
free(kinvden2);
free(wt);
free(maxwt);
free(num1);
free(kinvnum1);
free(den1);
free(den2);
free(wden1);
free(zden1);
free(kinvwden1);
free(kzden1);
}
void dr_latin(RelayProblem* problem, double *z, double *w, int *info, SolverOptions* options)
{
double* vec = problem->M->matrix0;
double* qq = problem->q;
int n = problem -> size;
double *a = problem->ub;
double *b = problem->lb;
//\todo Rewrite completely the algorithm with a projection.
int ib;
for (ib = 0; ib < n; ib++) b[ib] = -b[ib];
double k_latin = options->dparam[2];
int itermax = options->iparam[0];
double errmax = options->dparam[0];
int i, j, iter1, nrhs;
int info2 = 0;
int incx = 1, incy = 1;
double alpha, beta, mina;
double err1, num11, err0;
double den11, den22;
double *wc, *zc, *wt, *wnum1, *znum1;
double *zt, *kinvnum1;
/* char trans='T',notrans='N', uplo='U', diag='N'; */
double *k, *kinv, *DPO;
/* Allocations */
k = (double*) malloc(n * n * sizeof(double));
DPO = (double*) malloc(n * n * sizeof(double));
kinv = (double*) malloc(n * n * sizeof(double));
wc = (double*) malloc(n * sizeof(double));
zc = (double*) malloc(n * sizeof(double));
znum1 = (double*) malloc(n * sizeof(double));
wnum1 = (double*) malloc(n * sizeof(double));
wt = (double*) malloc(n * sizeof(double));
zt = (double*) malloc(n * sizeof(double));
kinvnum1 = (double*) malloc(n * sizeof(double));
/* Initialisation */
for (i = 0; i < n * n; i++)
{
k[i] = 0.0;
DPO[i] = 0.0;
kinv[i] = 0.0;
if (i < n)
{
wc[i] = 0.0;
zc[i] = 0.0;
z[i] = 0.0;
w[i] = 0.0;
znum1[i] = 0.0;
wnum1[i] = 0.0;
wt[i] = 0.0;
zt[i] = 0.0;
kinvnum1[i] = 0.0;
}
}
for (i = 0; i < n; i++)
{
k[i + n * i] = k_latin * vec[i * n + i];
if (fabs(k[i + n * i]) < DBL_EPSILON)
{
if (verbose > 0)
printf("\n Warning nul diagonal term in k matrix \n");
free(k);
free(kinv);
free(DPO);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(wt);
free(zt);
free(kinvnum1);
*info = 3;
return;
}
else
kinv[i + n * i] = 1 / k[i + n * i];
}
for (j = 0; j < n; j++)
{
for (i = 0; i < n; i++)
{
DPO[i + n * j] = vec[j * n + i] + k[i + n * j];
}
}
/* Cholesky */
DPOTRF(LA_UP, n, DPO , n, &info2);
if (info2 != 0)
{
if (verbose > 0)
printf("\n Matter with Cholesky factorization \n");
free(k);
free(kinv);
free(DPO);
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(wt);
free(zt);
free(kinvnum1);
*info = 2;
return;
}
/* End of cholesky */
/* Iteration loops */
iter1 = 0;
err1 = 1.;
while ((iter1 < itermax) && (err1 > errmax))
{
/* Linear stage (zc,wc) -> (z,w) */
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, zc, incx, beta, wc, incy);
cblas_dcopy(n, qq, incx, znum1, incy);
alpha = -1.;
cblas_dscal(n, alpha, znum1, incx);
alpha = 1.;
cblas_daxpy(n, alpha, wc, incx, znum1, incy);
nrhs = 1;
DTRTRS(LA_UP, LA_TRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info2);
DTRTRS(LA_UP, LA_NOTRANS, LA_NONUNIT, n, nrhs, DPO, n, znum1, n, &info2);
cblas_dcopy(n, znum1, incx, z, incy);
cblas_dcopy(n, wc, incx, w, incy);
alpha = -1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, z, incx, beta, w, incy);
/* Local stage (z,w)->(zc,wc) */
cblas_dcopy(n, w, incx, zt, incy);
alpha = -1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, z, incx, beta, zt, incy);
for (i = 0; i < n; i++)
{
if (a[i] < zt[i])
{
mina = a[i];
}
else
{
mina = zt[i];
}
if (mina > -b[i])
{
wc[i] = mina;
}
else
{
wc[i] = -b[i];
}
}
cblas_dcopy(n, wc, incx, wnum1, incy);
alpha = -1.;
cblas_daxpy(n, alpha, zt, incx, wnum1, incy);
cblas_dcopy(n, wnum1, incx, zt, incy);
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, kinv, n, zt, incx, beta, zc, incy);
/* Convergence criterium */
cblas_dcopy(n, w, incx, wnum1, incy);
alpha = -1.;
cblas_daxpy(n, alpha, wc, incx, wnum1, incy);
cblas_dcopy(n, z, incx, znum1, incy);
cblas_daxpy(n, alpha, zc, incx, znum1, incy);
alpha = 1.;
beta = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, znum1, incx, beta, wnum1, incy);
/* num1(:) =(w(:)-wc(:))+matmul( k(:,:),(z(:)-zc(:))) */
num11 = 0.;
alpha = 1.;
beta = 0.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, kinv, n, wnum1, incx, beta, kinvnum1, incy);
num11 = cblas_ddot(n, wnum1, incx, kinvnum1, incy);
cblas_dcopy(n, w, incx, wnum1, incy);
alpha = 1.;
cblas_daxpy(n, alpha, wc, incx, wnum1, incy);
cblas_dcopy(n, z, incx, znum1, incy);
cblas_daxpy(n, alpha, zc, incx, znum1, incy);
beta = 0.;
alpha = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, k, n, znum1, incx, beta, kinvnum1, incy);
den22 = cblas_ddot(n, znum1, incx, kinvnum1, incy);
beta = 0.;
alpha = 1.;
cblas_dgemv(CblasColMajor,CblasTrans, n, n, alpha, kinv, n, wnum1, incx, beta, kinvnum1, incy);
den11 = cblas_ddot(n, wnum1, incx, kinvnum1, incy);
err0 = num11 / (den11 + den22);
err1 = sqrt(err0);
iter1 = iter1 + 1;
options->iparam[1] = iter1;
options->dparam[1] = err1;
}
if (err1 > errmax)
{
if (verbose > 0)
printf("No convergence after %d iterations, the residue is %g\n", iter1, err1);
*info = 1;
}
else
{
if (verbose > 0)
printf("Convergence after %d iterations, the residue is %g \n", iter1, err1);
*info = 0;
}
free(wc);
free(zc);
free(znum1);
free(wnum1);
free(kinvnum1);
free(wt);
free(zt);
free(k);
free(DPO);
free(kinv);
}
