void mkl_vector_corr_permutation(vec_p x, vec_p y,
double* result,
unsigned int nPerm) {
double ret = 0.0;
assert(x->len == y->len && x->len > 0);
double xmean = 0.0, ymean = 0.0;
for (int i = 0; i < x->len; i++){
xmean += x->value[i];
ymean += y->value[i];
}
xmean /= x->len;
ymean /= x->len;
/* vec_p temp; */
/* temp = vector_new (x->len); */
double xstd = 0.0, ystd = 0.0, xycov = 0.0;
double tempx, tempy;
for (int i = 0; i < x-> len; i++) {
x->value[i] -= xmean;
y->value[i] -= ymean;
}
xstd = sqrt(cblas_dnrm2(x->len, x->value, 1));
ystd = sqrt(cblas_dnrm2(x->len, y->value, 1));
for (int n = 0; n < nPerm; n++ ) {
inplace_shuffle(y->value, y->len);
xycov = 0.0;
xycov = cblas_ddot(x->len, x->value, 1, y->value, 1);
result[n] = xycov / sqrt(xstd * ystd);
}
return ;
}
int checkTrivialCase_vi(VariationalInequality* problem, double* x,
double* w, SolverOptions* options)
{
int n = problem->size;
if (problem->ProjectionOnX)
{
problem->ProjectionOnX(problem,x,w);
}
else
{
cblas_dcopy(problem->size, x, 1, w, 1);
project_on_set(problem->size, w, problem->set);
}
cblas_daxpy(n, -1.0,x, 1, w , 1);
double nnorm = cblas_dnrm2(n,w,1);
DEBUG_PRINTF("checkTrivialCase_vi, nnorm = %6.4e\n",nnorm);
if (nnorm > fmin(options->dparam[0], 1e-12))
return 1;
problem->F(problem,n,x,w);
nnorm = cblas_dnrm2(n,w,1);
DEBUG_PRINTF("checkTrivialCase_vi, nnorm = %6.4e\n",nnorm);
if (nnorm > fmin(options->dparam[0], 1e-12))
return 1;
if (verbose == 1)
printf("variationalInequality driver, trivial solution F(x) = 0, x in X.\n");
return 0;
}
int variationalInequality_computeError(
VariationalInequality* problem,
double *z , double *w, double tolerance,
SolverOptions * options, double * error)
{
assert(problem);
assert(z);
assert(w);
assert(error);
int incx = 1;
int n = problem->size;
*error = 0.;
if (!options->dWork)
{
options->dWork = (double*)calloc(2*n,sizeof(double));
}
double *ztmp = options->dWork;
double *wtmp = &(options->dWork[n]);
if (!problem->istheNormVIset)
{
for (int i=0;i<n;i++)
{
ztmp[i]=0.0 ;
}
problem->F(problem,n,ztmp,w);
problem->normVI= cblas_dnrm2(n , w , 1);
DEBUG_PRINTF("problem->normVI = %12.8e\n", problem->normVI);
problem->istheNormVIset=1;
}
double normq =problem->normVI;
DEBUG_PRINTF("normq = %12.8e\n", normq);
problem->F(problem,n,z,w);
cblas_dcopy(n , z , 1 , ztmp, 1);
cblas_daxpy(n, -1.0, w , 1, ztmp , 1) ;
problem->ProjectionOnX(problem,ztmp,wtmp);
cblas_daxpy(n, -1.0, z , 1, wtmp , 1) ;
*error = cblas_dnrm2(n , wtmp , incx);
/* Computes error */
*error = *error / (normq + 1.0);
if (*error > tolerance)
{
if (verbose > 1)
printf(" Numerics - variationalInequality_compute_error: error = %g > tolerance = %g.\n",
*error, tolerance);
return 1;
}
else
return 0;
}
double norm(int N, double *vec){
// thread variables
int nthds, tid;
// compute variables
int m, stride, start, stop;
double nrm;
/* Fork a team of threads giving them their own copies of variables */
#pragma omp parallel private(nthds, tid) shared(m)
{
// compute thread variables
nthds = omp_get_num_threads();
tid = omp_get_thread_num();
if(tid == 0){
m = nthds;
}
}
//printf("m = %d\n",m);
double pnrms[m];
/* Fork a team of threads giving them their own copies of variables */
#pragma omp parallel private(nthds, tid, stride, start, stop) shared(N, vec, pnrms)
{
// compute thread variables
nthds = omp_get_num_threads();
tid = omp_get_thread_num();
// compute stride
stride = ceil((long double)N/nthds);
// compute start and stop
start = tid*stride;
stop = (int)fminl((long double)(tid+1)*stride,(long double)N);
pnrms[tid] = cblas_dnrm2(stop-start,&vec[start],1);
//printf("pnrms[%d] = %+e\n",tid,pnrms[tid]);
}
nrm = cblas_dnrm2(m,&pnrms[0],1);
//printf("nrm = %+e\n",nrm);
return nrm;
}
void grdm_ds(double* A, double* b, double* x, int n, double tol){
double alpha, *d, *tmp;
d = (double*) malloc(n * sizeof(double));
tmp = (double*) malloc(n * sizeof(double));
while(1){
//printf("x[0] = %f\n", x[0]);
//d_k = A*x_k - b
cblas_dcopy(n, b, 1, d, 1);
cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, A, n, x, 1, -1.0, d, 1);
//alpha_k = dot(d_k, d_k) / dot(d_k, d_k)_A
cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, A, n, d, 1, 0.0, tmp, 1);
alpha = cblas_ddot(n, d, 1, d, 1) / cblas_ddot(n, d, 1, tmp, 1);
cblas_dcopy(n, x, 1, tmp, 1);
//x_k+1 = x_k + alpha_k * d_k
cblas_daxpy(n, -alpha, d, 1, x, 1);
cblas_daxpy(n, -1.0, x, 1, tmp, 1);
//convergence check
if(cblas_dnrm2(n, tmp, 1) < tol) break;
}
free(d);
free(tmp);
}
// x(k+1) = M^(-1) * (b - D * x(k))
void fixpoint_iteration(double *Beta, double *D, double *x, double *b, int N, double tol)
{
int i, j;
double *xk, *temp, error;
xk = (double *) malloc(N*N*sizeof(double));
temp = (double *) malloc(N*N*sizeof(double));
for (i=0; i<N*N; i++)
{
for (j=0; j<N*N; j++) xk[j] = x[j];
dgemm(temp, D, xk, N);
// if (i == 0) printf(" Dgemm finish. \n");
for (j=0; j<N*N; j++) temp[j] = b[j] - Beta[j]*temp[j];
fastpoisson(temp, x, N);
// if(i == 0) printf(" Fast Poisson finish. \n");
for (j=0; j<N*N; j++) temp[j] = x[j] - xk[j];
error = cblas_dnrm2(N*N, temp, 1);
// printf(" Step %d finish. \n", i+1);
if ( error < tol)
{
printf("\n Converges at %d step ! \n", i+1);
break;
}
}
}
/* Computes the Frobenius norm of a matrix. */
double nrm2(Mat mA) {
const int n2 = MatN2(mA);
const void* a = MatElems(mA);
const bool dev = MatDev(mA);
double norm;
switch (MatElemSize(mA)) {
case 4:
if (dev) {
float norm32;
cublasSnrm2(g_cublasHandle, n2, a, 1, (float*)&norm32);
norm = norm32;
} else {
norm = cblas_snrm2(n2, a, 1);
}
break;
case 8:
if (dev) {
cublasDnrm2(g_cublasHandle, n2, a, 1, (double*)&norm);
} else {
norm = cblas_dnrm2(n2, a, 1);
}
break;
}
return norm;
}
void plotMeritToZsol(double *z)
{
int incx = 1, incy = 1;
double q_0, q_tk;
/* double merit_k; */
/* double tmin = 1e-12; */
double tk = 1;
/* double m1=0.5; */
double aux;
int i = 0;
int ii;
if (!sPlotMerit || !sZsol)
return;
FILE *fp;
for (ii = 0; ii < sN; ii++)
szzaux[ii] = sZsol[ii] - z[ii];
if (sPlotMerit)
{
/* sPlotMerit=0;*/
strcpy(fileName, "outputLSZsol");
(*sFphi)(sN, z, sphi_z, 0);
q_0 = cblas_dnrm2(sN, sphi_z , incx);
q_0 = 0.5 * q_0 * q_0;
fp = fopen(fileName, "w");
/* sPlotMerit=0;*/
tk = 1;
aux = -tk;
for (i = 0; i < 2e3; i++)
{
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(sN , aux , szzaux , incx , sz2 , incy);
(*sFphi)(sN, sz2, sphi_z, 0);
q_tk = cblas_dnrm2(sN, sphi_z , incx);
q_tk = 0.5 * q_tk * q_tk;
fprintf(fp, "%e %e\n", aux, q_tk);
aux += tk / 1e3;
}
fclose(fp);
}
}
double computeDenseEuclideanNorm(const double *vec1, const int elements){
#ifdef USE_INTEL_MKL
return cblas_dnrm2 (elements, vec1, 1);
#endif
#ifndef USE_INTEL_MKL
return 0;
#endif
}
void eblas_dnrm2_sub(size_t iStart, size_t iStop, const double* x, int incx, double* ret, std::mutex* lock)
{ //Compute this thread's contribution:
double retSub = cblas_dnrm2(iStop-iStart, x+incx*iStart, incx);
//Accumulate over threads (need sync):
lock->lock();
*ret += retSub*retSub;
lock->unlock();
}
inline void computeColNorms(const MAT * A, double *prob) {
int n = A->n, j, m = A->m;
memset(prob, 0, n * sizeof(double));
for (j = 0; j < n; j++) {
prob[j] = cblas_dnrm2(m, (A->val + j * A->m), 1);
prob[j] = pow(prob[j], 2);
}
}
/** Create the Householder symmetric matrix v*v^T
*
* This matrix is used to process the rows and columns of the input matrix.
*/
static void create_house_matrix_packed(size_t order, double shift, double *source, size_t incs, double *hhp) {
double h[order];
int i;
/* zero out the destination hhp (it's packed aka triangular, so the size is
* non-square) */
for (i = 0; i < order * (order + 1) / 2; i++) {
hhp[i] = 0;
}
/* create and normalize householder vector h */
cblas_dcopy(order, source, incs, h, 1);
h[0] += MYSIGN(h[0]) * cblas_dnrm2(order, h, 1);
h[0] -= shift;
cblas_dscal(order, 1.0 / cblas_dnrm2(order, h, 1), h, 1);
/* hhp = h h^T */
cblas_dspr(CblasRowMajor, CblasUpper, order, 1.0, h, 1, hhp);
}
int NonMonotomnelineSearch(double *z, double Rk)
{
int incx = 1, incy = 1;
double q_0, q_tk;
/* double merit_k; */
double tmin = 1e-12;
double tmax = 1000;
double tk = 1;
/* double m1=0.5; */
(*sFphi)(sN, z, sphi_z, 0);
q_0 = cblas_dnrm2(sN, sphi_z , incx);
q_0 = 0.5 * q_0 * q_0;
while ((tmax - tmin) > 1e-1)
{
tk = (tmax + tmin) / 2;
/*q_tk = 0.5*|| phi(z+tk*d) ||*/
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(sN , tk , sdir_descent , incx , sz2 , incy);
(*sFphi)(sN, sz2, sphi_z, 0);
q_tk = cblas_dnrm2(sN, sphi_z , incx);
q_tk = 0.5 * q_tk * q_tk;
if (fabs(q_tk - q_0) < Rk)
tmin = tk;
else
tmax = tk;
}
printf("NonMonotomnelineSearch, tk = %e\n", tk);
cblas_dcopy(sN, sz2, incx, z, incx);
if (tk <= tmin)
{
printf("NonMonotomnelineSearch warning, resulting tk < tmin, linesearch stopped.\n");
return 0;
}
return 1;
}
double eblas_dnrm2(int N, const double* x, int incx)
{
#ifdef MKL_PROVIDES_BLAS
return cblas_dnrm2(N, x, incx);
#else
double ret = 0.;
std::mutex lock;
threadLaunch((N<100000) ? 1 : 0, eblas_dnrm2_sub, N, x, incx, &ret, &lock);
return sqrt(ret);
#endif
}
bool verify(const double result, const double *data, const size_t size)
{
bool status = false;
// double cblas_dnrm2(const int N, const double *X, const int incX);
double stdResult = cblas_dnrm2(size, data, 1);
status = (abs(stdResult - result) < 1e-16);
#ifdef DEBUG
printf("The verification result is %d\n", status);
#endif
return status;
}
int soclcp_compute_error_v(SecondOrderConeLinearComplementarityProblem* problem, double *z , double *w, double tolerance, SolverOptions *options, double * error)
{
/* Checks inputs */
if(problem == NULL || z == NULL || w == NULL)
numericsError("soclcp_compute_error", "null input for problem and/or z and/or w");
/* Computes w = Mz + q */
int incx = 1, incy = 1;
int nc = problem->nc;
int n = problem->n;
double *mu = problem->mu;
double invmu = 0.0;
cblas_dcopy(n , problem->q , incx , z , incy); // z <-q
// Compute the current reaction
prodNumericsMatrix(n, n, 1.0, problem->M, w, 1.0, z);
*error = 0.;
double rho = 1.0;
for(int ic = 0 ; ic < nc ; ic++)
{
int dim = problem->coneIndex[ic+1]-problem->coneIndex[ic];
double * worktmp = (double *)malloc(dim*sizeof(double)) ;
int nic = problem->coneIndex[ic];
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - rho * z[nic+i];
}
invmu = 1.0 / mu[ic];
projectionOnSecondOrderCone(worktmp, invmu, dim);
for (int i=0; i < dim; i++)
{
worktmp[i] = w[nic+i] - worktmp[i];
*error += worktmp[i] * worktmp[i];
}
free(worktmp);
}
*error = sqrt(*error);
/* Computes error */
double normq = cblas_dnrm2(n , problem->q , incx);
*error = *error / (normq + 1.0);
if(*error > tolerance)
{
/* if (verbose > 0) printf(" Numerics - soclcp_compute_error_velocity failed: error = %g > tolerance = %g.\n",*error, tolerance); */
return 1;
}
else
return 0;
}
/*
*check M z + q =0
*
*
*/
double LinearSystem_computeError(LinearSystemProblem* problem, double *z)
{
double * pM = problem->M->matrix0;
double * pQ = problem->q;
double error = 10;
int n = problem->size;
double * res = (double*)malloc(n * sizeof(double));
memcpy(res, pQ, n * sizeof(double));
cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, 1.0, pM, n, z, 1, 1.0, res, 1);
error = cblas_dnrm2(n, res, 1);
free(res);
return error;
}
void Orthogonalize(OrthoContext* c, double* p, int numBases, double* orthonormalBases)
{
memcpy(c->Pv->Data, p, c->Pv->Count * sizeof(double));
memcpy(c->Bases->Data, orthonormalBases, numBases * c->Pv->Count * sizeof(double));
c->Bases->RowCount = numBases;
c->Dp->Count = numBases;
int basisLen = c->Pv->Count;
GEMV(1, c->Bases, c->Pv, 0, c->Dp);
for (int i = 0, offset = 0; i < numBases; i++, offset += basisLen)
AXPY2(-1 * c->Dp->Data[i], c->Bases->Data + offset, basisLen, c->Pv->Data);
double mag = cblas_dnrm2(basisLen, c->Pv->Data, 1);
cblas_dscal(basisLen, 1.0 / mag, c->Pv->Data, 1);
memcpy(p, c->Pv->Data, basisLen * sizeof(double));
}
void linesearch_Armijo(int n, double *z, double* dir, double psi_k,
double descentCondition, NewtonFunctionPtr* phi)
{
double * phiVector = (double*)malloc(n * sizeof(*phiVector));
if (phiVector == NULL)
{
fprintf(stderr, "NonSmoothNewton::linesearch_Armijo, memory allocation failed for phiVector\n");
exit(EXIT_FAILURE);
}
/* IN :
psi_k (merit function for current iteration)
jacobian_psi_k (jacobian of the merit function)
dk: descent direction
OUT: tk, z
*/
double sigma = 1e-4;
double tk = 1;
int incx = 1, incy = 1;
double merit, merit_k;
double tmin = 1e-12;
/* z1 = z0 + dir */
cblas_daxpy(n , 1.0 , dir , incx , z , incy);
while (tk > tmin)
{
/* Computes merit function = 1/2*norm(phi(z_{k+1}))^2 */
(*phi)(n, z, phiVector, 0);
merit = cblas_dnrm2(n, phiVector , incx);
merit = 0.5 * merit * merit;
merit_k = psi_k + sigma * tk * descentCondition;
if (merit < merit_k) break;
tk = tk * 0.5;
/* Computes z_k+1 = z0 + tk.dir
warning: (-tk) because we need to start from z0 at each step while z_k+1 is saved in place of z_k ...*/
cblas_daxpy(n , -tk , dir , incx , z , incy);
}
free(phiVector);
if (tk <= tmin)
if (verbose > 0)
printf("NonSmoothNewton::linesearch_Armijo warning, resulting tk < tmin, linesearch stopped.\n");
}
VALUE rb_blas_xnrm2(int argc, VALUE *argv, VALUE self)
{
Matrix *dx;
int incx;
int incy;
int n;
//char error_msg[64];
VALUE n_value, incx_value;
rb_scan_args(argc, argv, "02", &incx_value, &n_value);
Data_Get_Struct(self, Matrix, dx);
if(incx_value == Qnil)
incx = 1;
else
incx = NUM2INT(incx_value);
if(n_value == Qnil)
n = dx->nrows;
else
n = NUM2INT(n_value);
if(dx == NULL || dx->ncols != 1)
{ //sprintf(error_msg, "Self is not a Vector");
rb_raise(rb_eRuntimeError, "Self is not a Vector");
}
switch(dx->data_type)
{
case Single_t: //s
return rb_float_new(cblas_snrm2(n , (float *)dx->data, incx));
case Double_t: //d
return rb_float_new(cblas_dnrm2(n , (double *)dx->data, incx));
case Complex_t: //c
return rb_float_new(cblas_scnrm2(n , dx->data, incx));
case Double_Complex_t: //z
return rb_float_new(cblas_dznrm2(n , dx->data, incx));
default:
//sprintf(error_msg, "Invalid data_type (%d) in Matrix", dx->data_type);
rb_raise(rb_eRuntimeError, "Invalid data_type (%d) in Matrix", dx->data_type);
return Qnil; //Never reaches here.
}
}
int main(int iArgCnt, char* sArrArgs[])
{
int iIterationNo = 0;
double dNormOfResult = 0;
double dTime0 = 0, dTime1 = 0, dTimeDiff = 0, dMinTimeDiff = DBL_MAX, dMaxTimeDiff = 0;
parseInputs(iArgCnt, sArrArgs);
MPI_Init(&iArgCnt, &sArrArgs);
MPI_Comm_size(MPI_COMM_WORLD, &GiProcessCnt);
MPI_Comm_rank(MPI_COMM_WORLD, &GiProcessRank);
initData();
for(iIterationNo = 0; iIterationNo < GiIterationCnt; iIterationNo++)
{
MPI_Barrier(MPI_COMM_WORLD);
dTime0 = MPI_Wtime();
cblas_dgemv(CblasRowMajor, CblasNoTrans, GiRowCntForOneProc, GiVectorLength, 1.0, GdArrSubMatrix, GiVectorLength, GdArrVector, 1, 0.0, GdArrSubResult, 1);
MPI_Barrier(MPI_COMM_WORLD);
MPI_Gather(GdArrSubResult, GiRowCntForOneProc, MPI_DOUBLE, GdArrTotalResult, GiRowCntForOneProc, MPI_DOUBLE, 0, MPI_COMM_WORLD);
dTime1 = MPI_Wtime();
dTimeDiff = (dTime1 - dTime0);
if(dTimeDiff > dMaxTimeDiff)
dMaxTimeDiff = dTimeDiff;
if(dTimeDiff < dMinTimeDiff)
dMinTimeDiff = dTimeDiff;
}
if(GiProcessRank == 0)
{
dNormOfResult = cblas_dnrm2(GiVectorLength, GdArrTotalResult, 1);
printf("Result=%f\nMin Time=%f uSec\nMax Time=%f uSec\n", dNormOfResult, (1.e6 * dMinTimeDiff), (1.e6 * dMaxTimeDiff));
}
MPI_Finalize();
return 0;
}
int lcp_compute_error(LinearComplementarityProblem* problem, double *z , double *w, double tolerance, double * error)
{
/* Checks inputs */
if (problem == NULL || z == NULL || w == NULL)
numerics_error("lcp_compute_error", "null input for problem and/or z and/or w");
/* Computes w = Mz + q */
int incx = 1, incy = 1;
unsigned int n = problem->size;
cblas_dcopy(n , problem->q , incx , w , incy); // w <-q
prodNumericsMatrix(n, n, 1.0, problem->M, z, 1.0, w);
double normq = cblas_dnrm2(n , problem->q , incx);
lcp_compute_error_only(n, z, w, error);
*error = *error / (normq + 1.0); /* Need some comments on why this is needed */
if (*error > tolerance)
{
if (verbose > 0) printf(" Numerics - lcp_compute_error : error = %g > tolerance = %g.\n", *error, tolerance);
return 1;
}
else
return 0;
}
void lanczos(double *F, double *Es, double *L, int n_eigs, int n_patch,
int LANCZOS_ITR)
{
double *b;
double b_norm;
double *z;
double *alpha, *beta;
double *q;
int i;
double *eigvec; // eigenvectors
// generate random b with norm 1.
srand((unsigned int)time(NULL));
b = (double *)malloc(n_patch * sizeof(double));
for (i = 0; i < n_patch; i++)
b[i] = rand();
b_norm = norm2(b, n_patch);
for (i = 0; i < n_patch; i++)
b[i] /= b_norm;
alpha = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
beta = (double *)malloc( (LANCZOS_ITR + 1) * sizeof(double) );
beta[0] = 0.0; // beta_0 <- 0
z = (double *)malloc( n_patch * sizeof(double));
q = (double *)malloc( n_patch * (LANCZOS_ITR + 2) * sizeof(double) );
memset(&q[0], 0, n_patch * sizeof(double)); // q_0 <- 0
memcpy(&q[n_patch], b, n_patch * sizeof(double)); // q_1 <- b
for (i = 1; i <= LANCZOS_ITR; i++) {
// z = L * Q(:, i)
cblas_dsymv(CblasColMajor, CblasLower, n_patch, 1.0, L,
n_patch, &q[i * n_patch], 1, 0.0, z, 1);
// alpha(i) = Q(:, i)' * z;
alpha[i] = cblas_ddot(n_patch, &q[i * n_patch], 1, z, 1);
// z = z - alpha(i) * Q(:, i)
cblas_daxpy(n_patch, -alpha[i], &q[i * n_patch], 1, z, 1);
// z = z - beta(i - 1) * Q(:, i - 1);
cblas_daxpy(n_patch, -beta[i - 1], &q[(i - 1) * n_patch], 1, z, 1);
// beta(i) = norm(z, 2);
beta[i] = cblas_dnrm2(n_patch, z, 1);
// Q(:, i + 1) = z / beta(i);
divide_copy(&q[(i + 1) * n_patch], z, n_patch, beta[i]);
}
// compute approximate eigensystem
eigvec = (double *)malloc(LANCZOS_ITR * LANCZOS_ITR * sizeof(double));
LAPACKE_dstedc(LAPACK_COL_MAJOR, 'I', LANCZOS_ITR, &alpha[1], &beta[1],
eigvec, LANCZOS_ITR);
// copy specified number of eigenvalues
memcpy(Es, &alpha[1], n_eigs * sizeof(double));
// V = Q(:, 1:k) * U
cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, n_patch,
LANCZOS_ITR, LANCZOS_ITR, 1.0, &q[n_patch], n_patch, eigvec,
LANCZOS_ITR, 0.0, L, n_patch);
// copy the corresponding eigenvectors
memcpy(F, L, n_patch * n_eigs * sizeof(double));
free(b);
free(z);
free(alpha);
free(beta);
free(q);
free(eigvec);
}
void plotMerit(double *z, double psi_k, double descentCondition)
{
int incx = 1, incy = 1;
double q_0, q_tk, qp_tk, merit_k;
/* double tmin = 1e-12; */
double tk = 1, aux;
double m1 = 1e-4;
double Nstep = 0;
int i = 0;
FILE *fp;
(*sFphi)(sN, z, sphi_z, 0);
aux = cblas_dnrm2(sN, sphi_z, 1);
/* Computes merit function */
aux = 0.5 * aux * aux;
printf("plot psi_z %e\n", aux);
if (!sPlotMerit)
return;
if (sPlotMerit)
{
/* sPlotMerit=0;*/
strcpy(fileName, "outputLS");
(*sFphi)(sN, z, sphi_z, 0);
q_0 = cblas_dnrm2(sN, sphi_z , incx);
q_0 = 0.5 * q_0 * q_0;
fp = fopen(fileName, "w");
/* sPlotMerit=0;*/
tk = 5e-7;
aux = -tk;
Nstep = 1e4;
for (i = 0; i < 2 * Nstep; i++)
{
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(sN , aux , sdir_descent , incx , sz2 , incy);
(*sFphi)(sN, sz2, sphi_z, 0);
q_tk = cblas_dnrm2(sN, sphi_z , incx);
q_tk = 0.5 * q_tk * q_tk;
(*sFjacobianPhi)(sN, sz2, sjacobianPhi_z, 1);
/* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
cblas_dgemv(CblasColMajor,CblasTrans, sN, sN, 1.0, sjacobianPhi_z, sN, sphi_z, incx, 0.0, sgrad_psi_z, incx);
qp_tk = cblas_ddot(sN, sgrad_psi_z, 1, sdir_descent, 1);
merit_k = psi_k + m1 * aux * descentCondition;
fprintf(fp, "%e %.16e %.16e %e\n", aux, q_tk, merit_k, qp_tk);
if (i == Nstep - 1)
aux = 0;
else
aux += tk / Nstep;
}
fclose(fp);
}
}
/*
* (input) double *z : size n+m
* (output)double *w : size n+m
*
*
*/
int mlcp_compute_error(MixedLinearComplementarityProblem* problem, double *z, double *w, double tolerance, double * error)
{
/* Checks inputs */
if (problem == NULL || z == NULL || w == NULL)
numerics_error("mlcp_compute_error", "null input for problem and/or z and/or w");
int param = 1;
int NbLines = problem->M->size0; /* Equalities */
int n = problem->n; /* Equalities */
int m = problem->m; /* Inequalities */
int incx = 1, incy = 1;
/* Computation of w: depends on the way the problem is written */
/* Problem in the form (M,q) */
if (problem->isStorageType1)
{
if (problem->M == NULL)
numerics_error("mlcp_compute_error", "null input for M");
/* Computes w = Mz + q */
cblas_dcopy(NbLines , problem->q , incx , w , incy);
prodNumericsMatrix(problem->M->size1, problem->M->size0, 1.0, problem->M, z, 1.0, w);
}
/* Problem in the form ABCD */
else //if (problem->isStorageType2)
{
/* Checks inputs */
if (problem->A == NULL || problem->B == NULL || problem->C == NULL || problem->D == NULL)
{
numerics_error("mlcp_compute_error: ", "null input for A, B, C or D");
}
/* Links to problem data */
double *a = &problem->q[0];
double *b = &problem->q[NbLines - m];
double *A = problem->A;
double *B = problem->B;
double *C = problem->C;
double *D = problem->D;
/* Compute "equalities" part, we = Au + Cv + a - Must be equal to 0 */
cblas_dcopy(NbLines - m , a , incx , w , incy); // we = w[0..n-1] <-- a
cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, n , 1.0 , A , NbLines - m , &z[0] , incx , 1.0 , w , incy); // we <-- A*u + we
cblas_dgemv(CblasColMajor,CblasNoTrans , NbLines - m, m , 1.0 , C , NbLines - m , &z[n] , incx , 1.0 , w , incy); // we <-- C*v + we
/* Computes part which corresponds to complementarity */
double * pwi = w + NbLines - m; // No copy!!
cblas_dcopy(m , b , incx , pwi , incy); // wi = w[n..m] <-- b
// following int param, we recompute the product wi = Du+BV +b and we = Au+CV +a
// The test is then more severe if we compute w because it checks that the linear equation is satisfied
if (param == 1)
{
cblas_dgemv(CblasColMajor,CblasNoTrans , m, n , 1.0 , D , m , &z[0] , incx , 1.0 , pwi , incy); // wi <-- D*u+ wi
cblas_dgemv(CblasColMajor,CblasNoTrans , m , m , 1.0 , B , m , &z[n] , incx , 1.0 , pwi , incy); // wi <-- B*v + wi
}
}
/* Error on equalities part */
double error_e = 0;
/* Checks complementarity (only for rows number n to size) */
double error_i = 0.;
double zi, wi;
double *q = problem->q;
double norm_e = 1;
double norm_i = 1;
if (problem->blocksRows)
{
int numBlock = 0;
while (problem->blocksRows[numBlock] < n + m)
{
if (!problem->blocksIsComp[numBlock])
{
error_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
norm_e += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], q + problem->blocksRows[numBlock] , incx);
}
else
{
for (int numLine = problem->blocksRows[numBlock]; numLine < problem->blocksRows[numBlock + 1] ; numLine++)
{
zi = z[numLine];
wi = w[numLine];
if (zi < 0.0)
{
error_i += -zi;
if (wi < 0.0) error_i += zi * wi;
}
if (wi < 0.0) error_i += -wi;
if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
}
norm_i += cblas_dnrm2(problem->blocksRows[numBlock + 1] - problem->blocksRows[numBlock], w + problem->blocksRows[numBlock] , incx);
}
numBlock++;
}
}
else
{
printf("WARNING, DEPRECATED MLCP API\n");
/* Error on equalities part */
error_e = cblas_dnrm2(NbLines - m , w , incx);;
/* Checks complementarity (only for rows number n to size) */
error_i = 0.;
for (int i = 0 ; i < m ; i++)
{
zi = z[n + i];
wi = w[(NbLines - m) + i];
if (zi < 0.0)
{
error_i += -zi;
if (wi < 0.0) error_i += zi * wi;
}
if (wi < 0.0) error_i += -wi;
if ((zi > 0.0) && (wi > 0.0)) error_i += zi * wi;
}
/* Computes error */
norm_i += cblas_dnrm2(m , q + NbLines - m , incx);
norm_e += cblas_dnrm2(NbLines - m , q , incx);
}
if (error_i / norm_i >= error_e / norm_e)
{
*error = error_i / (1.0 + norm_i);
}
else
{
*error = error_e / (1.0 + norm_e);
}
if (*error > tolerance)
{
/*if (isVerbose > 0) printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n",*error, tolerance);*/
if (verbose)
printf(" Numerics - mlcp_compute_error failed: error = %g > tolerance = %g.\n", *error, tolerance);
/* displayMLCP(problem);*/
return 1;
}
else
{
if (verbose > 0) printf("Siconos/Numerics: mlcp_compute_error: Error evaluation = %g \n", *error);
return 0;
}
}
int nonSmoothNewtonNeigh(int n, double* z, NewtonFunctionPtr* phi, NewtonFunctionPtr* jacobianPhi, int* iparam, double* dparam)
{
int itermax = iparam[0]; // maximum number of iterations allowed
int iterMaxWithSameZ = itermax / 4;
int niter = 0; // current iteration number
double tolerance = dparam[0];
/* double coef; */
sFphi = phi;
sFjacobianPhi = jacobianPhi;
// verbose=1;
if (verbose > 0)
{
printf(" ============= Starting of Newton process =============\n");
printf(" - tolerance: %14.7e\n - maximum number of iterations: %i\n", tolerance, itermax);
}
int incx = 1;
/* int n2 = n*n; */
int infoDGESV;
/** merit function and its jacobian */
double psi_z;
/** The algorithm is alg 4.1 of the paper of Kanzow and Kleinmichel, "A new class of semismooth Newton-type methods
for nonlinear complementarity problems", in Computational Optimization and Applications, 11, 227-251 (1998).
We try to keep the same notations
*/
double rho = 1e-8;
double descentCondition, criterion, norm_jacobian_psi_z, normPhi_z;
double p = 2.1;
double terminationCriterion = 1;
double norm;
int findNewZ, i, j, NbLookingForANewZ;
/* int naux=0; */
double aux = 0;
/* double aux1=0; */
int ii;
int resls = 1;
/* char c; */
/* double * oldz; */
/* oldz=(double*)malloc(n*sizeof(double));*/
NbLookingForANewZ = 0;
/** Iterations ... */
while ((niter < itermax) && (terminationCriterion > tolerance))
{
scmp++;
++niter;
/** Computes phi and its jacobian */
if (sZsol)
{
for (ii = 0; ii < sN; ii++)
szzaux[ii] = sZsol[ii] - z[ii];
printf("dist zzsol %.32e.\n", cblas_dnrm2(n, szzaux, 1));
}
(*sFphi)(n, z, sphi_z, 0);
(*sFjacobianPhi)(n, z, sjacobianPhi_z, 1);
/* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
cblas_dgemv(CblasColMajor,CblasTrans, n, n, 1.0, sjacobianPhi_z, n, sphi_z, incx, 0.0, sgrad_psi_z, incx);
norm_jacobian_psi_z = cblas_dnrm2(n, sgrad_psi_z, 1);
/* Computes norm2(phi) */
normPhi_z = cblas_dnrm2(n, sphi_z, 1);
/* Computes merit function */
psi_z = 0.5 * normPhi_z * normPhi_z;
if (normPhi_z < tolerance)
{
/*it is the solution*/
terminationCriterion = tolerance / 2.0;
break;
}
if (verbose > 0)
{
printf("Non Smooth Newton, iteration number %i, norm grad psi= %14.7e , psi = %14.7e, normPhi = %e .\n", niter, norm_jacobian_psi_z, psi_z, normPhi_z);
printf(" -----------------------------------------------------------------------\n");
}
NbLookingForANewZ++;
if (niter > 2)
{
if (10 * norm_jacobian_psi_z < tolerance || !resls || NbLookingForANewZ > iterMaxWithSameZ)
{
NbLookingForANewZ = 0;
resls = 1;
/* if (NbLookingForANewZ % 10 ==1 && 0){
printf("Try NonMonotomnelineSearch\n");
cblas_dcopy(n,sgrad_psi_z,1,sdir_descent,1);
cblas_dscal( n , -1.0 ,sdir_descent,incx);
NonMonotomnelineSearch( z, phi, 10);
continue;
}
*/
/* FOR DEBUG ONLY*/
if (sZsol)
{
printf("begin plot prev dir\n");
plotMerit(z, 0, 0);
printf("end\n");
/* gets(&c);*/
(*sFphi)(n, sZsol, szaux, 0);
printf("value psi(zsol)=%e\n", cblas_dnrm2(n, szaux, 1));
cblas_dcopy(n, sZsol, incx, szaux, incx);
cblas_daxpy(n , -1 , z , 1 , szaux , 1);
printf("dist to sol %e \n", cblas_dnrm2(n, szaux, 1));
for (ii = 0; ii < n; ii++)
sdir_descent[ii] = sZsol[ii] - z[ii];
aux = norm;
norm = 1;
printf("begin plot zzsol dir\n");
plotMerit(z, 0, 0);
printf("end\n");
/* gets(&c);*/
norm = aux;
}
printf("looking for a new Z...\n");
/*may be a local minimal*/
/*find a gradiant going out of this cul-de-sac.*/
norm = n / 2;
findNewZ = 0;
for (j = 0; j < 20; j++)
{
for (i = 0; i < n; i++)
{
if (sZsol)
{
/* FOR DEBUG ONLY*/
(*sFphi)(n, sZsol, sphi_zaux, 0);
norm = cblas_dnrm2(n, sphi_zaux, 1);
printf("Norm of the sol %e.\n", norm);
for (ii = 0; ii < n; ii++)
sdir_descent[ii] = sZsol[ii] - z[ii];
norm = 1;
}
else
{
for (ii = 0; ii < n; ii++)
{
sdir_descent[ii] = 1.0 * rand();
}
cblas_dscal(n, 1 / cblas_dnrm2(n, sdir_descent, 1), sdir_descent, incx);
cblas_dscal(n, norm, sdir_descent, incx);
}
cblas_dcopy(n, z, incx, szaux, incx);
// cblas_dscal(n,0.0,zaux,incx);
/* zaux = z + dir */
cblas_daxpy(n , norm , sdir_descent , 1 , szaux , 1);
/* Computes the jacobian of the merit function, jacobian_psi_zaux = transpose(jacobianPhi_zaux).phi_zaux */
(*sFphi)(n, szaux, sphi_zaux, 0);
(*sFjacobianPhi)(n, szaux, sjacobianPhi_zaux, 1);
/* FOR DEBUG ONLY*/
if (sZsol)
{
aux = cblas_dnrm2(n, sphi_zaux, 1);
printf("Norm of the sol is now %e.\n", aux);
for (ii = 0; ii < n; ii++)
printf("zsol %e zaux %e \n", sZsol[ii], szaux[ii]);
}
cblas_dgemv(CblasColMajor, CblasTrans, n, n, 1.0, sjacobianPhi_zaux, n, sphi_zaux, incx, 0.0, sgrad_psi_zaux, incx);
cblas_dcopy(n, szaux, 1, szzaux, 1);
cblas_daxpy(n , -1 , z , incx , szzaux , incx);
/*zzaux must be a descente direction.*/
/*ie jacobian_psi_zaux.zzaux <0
printf("jacobian_psi_zaux : \n");*/
/*cblas_dcopy(n,sdir,incx,sdir_descent,incx);
plotMerit(z, phi);*/
aux = cblas_ddot(n, sgrad_psi_zaux, 1, szzaux, 1);
/* aux1 = cblas_dnrm2(n,szzaux,1);
aux1 = cblas_dnrm2(n,sgrad_psi_zaux,1);*/
aux = aux / (cblas_dnrm2(n, szzaux, 1) * cblas_dnrm2(n, sgrad_psi_zaux, 1));
/* printf("aux: %e\n",aux);*/
if (aux < 0.1 * (j + 1))
{
//zaux is the new point.
findNewZ = 1;
cblas_dcopy(n, szaux, incx, z, incx);
break;
}
}
if (findNewZ)
break;
if (j == 10)
{
norm = n / 2;
}
else if (j > 10)
norm = -2 * norm;
else
norm = -norm / 2.0;
}
if (! findNewZ)
{
printf("failed to find a new z\n");
/* exit(1);*/
continue;
}
else
continue;
}
}
/* Stops if the termination criterion is satisfied */
terminationCriterion = norm_jacobian_psi_z;
/* if(terminationCriterion < tolerance){
break;
}*/
/* Search direction calculation
Find a solution dk of jacobianPhiMatrix.d = -phiVector.
dk is saved in phiVector.
*/
cblas_dscal(n , -1.0 , sphi_z, incx);
DGESV(n, 1, sjacobianPhi_z, n, sipiv, sphi_z, n, &infoDGESV);
if (infoDGESV)
{
printf("DGEV error %d.\n", infoDGESV);
}
cblas_dcopy(n, sphi_z, 1, sdir_descent, 1);
criterion = cblas_dnrm2(n, sdir_descent, 1);
/* printf("norm dir descent %e\n",criterion);*/
/*printf("begin plot descent dir\n");
plotMerit(z, phi);
printf("end\n");
gets(&c);*/
/*printf("begin plot zzsol dir\n");
plotMeritToZsol(z,phi);
printf("end\n");
gets(&c);*/
/*
norm = cblas_dnrm2(n,sdir_descent,1);
printf("norm desc %e \n",norm);
cblas_dscal( n , 1/norm , sdir_descent, 1);
*/
/* descentCondition = jacobian_psi.dk */
descentCondition = cblas_ddot(n, sgrad_psi_z, 1, sdir_descent, 1);
/* Criterion to be satisfied: error < -rho*norm(dk)^p */
criterion = -rho * pow(criterion, p);
/* printf("ddddddd %d\n",scmp);
if (scmp>100){
displayMat(sjacobianPhi_z,n,n,n);
exit(1);
}*/
// if ((infoDGESV != 0 || descentCondition > criterion) && 0)
// {
// printf("no a desc dir, get grad psy\n");
/* dk = - jacobian_psi (remind that dk is saved in phi_z) */
// cblas_dcopy(n, sgrad_psi_z, 1, sdir_descent, 1);
// cblas_dscal(n , -1.0 , sdir_descent, incx);
/*DEBUG ONLY*/
/*printf("begin plot new descent dir\n");
plotMerit(z);
printf("end\n");
gets(&c);*/
// }
/* coef=fabs(norm_jacobian_psi_z*norm_jacobian_psi_z/descentCondition);
if (coef <1){
cblas_dscal(n,coef,sdir_descent,incx);
printf("coef %e norm dir descent is now %e\n",coef,cblas_dnrm2(n,sdir_descent,1));
}*/
/* Step-3 Line search: computes z_k+1 */
/*linesearch_Armijo(n,z,sdir_descent,psi_z, descentCondition, phi);*/
/* if (niter == 10){
printf("begin plot new descent dir\n");
plotMerit(z);
printf("end\n");
gets(&c);
}*/
/* memcpy(oldz,z,n*sizeof(double));*/
resls = linesearch2_Armijo(n, z, psi_z, descentCondition);
if (!resls && niter > 1)
{
/* displayMat(sjacobianPhi_z,n,n,n);
printf("begin plot new descent dir\n");
plotMerit(oldz,psi_z, descentCondition);
printf("end\n");
gets(&c);*/
}
/* lineSearch_Wolfe(z, descentCondition, phi,jacobianPhi);*/
/* if (niter>3){
printf("angle between prev dir %e.\n",acos(cblas_ddot(n, sdir_descent, 1, sPrevDirDescent, 1)/(cblas_dnrm2(n,sdir_descent,1)*cblas_dnrm2(n,sPrevDirDescent,1))));
}*/
cblas_dcopy(n, sdir_descent, 1, sPrevDirDescent, 1);
/* for (j=20;j<32;j++){
if (z[j]<0)
z[j]=0;
}*/
/* if( 1 || verbose>0)
{
printf("Non Smooth Newton, iteration number %i, error grad equal to %14.7e , psi value is %14.7e .\n",niter, terminationCriterion,psi_z);
printf(" -----------------------------------------------------------------------\n");
}*/
}
/* Total number of iterations */
iparam[1] = niter;
/* Final error */
dparam[1] = terminationCriterion;
/** Free memory*/
if (verbose > 0)
{
if (dparam[1] > tolerance)
printf("Non Smooth Newton warning: no convergence after %i iterations\n" , niter);
else
printf("Non Smooth Newton: convergence after %i iterations\n" , niter);
printf(" The residue is : %e \n", dparam[1]);
}
/* free(oldz);*/
if (dparam[1] > tolerance)
return 1;
else return 0;
}
void soclcp_VI_ExtraGradient(SecondOrderConeLinearComplementarityProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* Dimension of the problem */
int n = problem->n;
VariationalInequality *vi = (VariationalInequality *)malloc(sizeof(VariationalInequality));
//vi.self = &vi;
vi->F = &Function_VI_SOCLCP;
vi->ProjectionOnX = &Projection_VI_SOCLCP;
int iter=0;
double error=1e24;
SecondOrderConeLinearComplementarityProblem_as_VI *soclcp_as_vi= (SecondOrderConeLinearComplementarityProblem_as_VI*)malloc(sizeof(SecondOrderConeLinearComplementarityProblem_as_VI));
vi->env =soclcp_as_vi ;
vi->size = n;
/*Set the norm of the VI to the norm of problem->q */
vi->normVI= cblas_dnrm2(n , problem->q , 1);
vi->istheNormVIset=1;
soclcp_as_vi->vi = vi;
soclcp_as_vi->soclcp = problem;
/* soclcp_display(fc3d_as_vi->fc3d); */
SolverOptions * visolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));
variationalInequality_setDefaultSolverOptions(visolver_options,
SICONOS_VI_EG);
int isize = options->iSize;
int dsize = options->dSize;
int vi_isize = visolver_options->iSize;
int vi_dsize = visolver_options->dSize;
if (isize != vi_isize )
{
printf("size problem in soclcp_VI_ExtraGradient\n");
}
if (dsize != vi_dsize )
{
printf("size problem in soclcp_VI_ExtraGradient\n");
}
int i;
for (i = 0; i < min(isize,vi_isize); i++)
{
if (options->iparam[i] != 0 )
visolver_options->iparam[i] = options->iparam[i] ;
}
for (i = 0; i < min(dsize,vi_dsize); i++)
{
if (fabs(options->dparam[i]) >= 1e-24 )
visolver_options->dparam[i] = options->dparam[i] ;
}
variationalInequality_ExtraGradient(vi, reaction, velocity , info , visolver_options);
/* **** Criterium convergence **** */
soclcp_compute_error(problem, reaction , velocity, options->dparam[0], options, &error);
/* for (i =0; i< n ; i++) */
/* { */
/* printf("reaction[%i]=%f\t",i,reaction[i]); printf("velocity[%i]=F[%i]=%f\n",i,i,velocity[i]); */
/* } */
error = visolver_options->dparam[SICONOS_DPARAM_RESIDU];
iter = visolver_options->iparam[SICONOS_IPARAM_ITER_DONE];
options->dparam[SICONOS_DPARAM_RESIDU] = error;
options->dparam[SICONOS_VI_EG_DPARAM_RHO] = visolver_options->dparam[SICONOS_VI_EG_DPARAM_RHO];
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (verbose > 0)
{
printf("--------------- SOCLCP - VI Extra Gradient (VI_EG) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
free(vi);
solver_options_delete(visolver_options);
free(visolver_options);
visolver_options=NULL;
free(soclcp_as_vi);
}
int lineSearch_Wolfe(double *z, double qp_0)
{
int incx = 1, incy = 1;
double q_0, q_tk, qp_tk;
double tmin = 1e-12;
int maxiter = 100;
int niter = 0;
double tk = 1;
double tg, td;
double m1 = 0.1;
double m2 = 0.9;
(*sFphi)(sN, z, sphi_z, 0);
q_0 = cblas_dnrm2(sN, sphi_z , incx);
q_0 = 0.5 * q_0 * q_0;
tg = 0;
td = 10e5;
tk = (tg + td) / 2.0;
while (niter < maxiter || (td - tg) < tmin)
{
niter++;
/*q_tk = 0.5*|| phi(z+tk*d) ||*/
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(sN , tk , sdir_descent , incx , sz2 , incy);
(*sFphi)(sN, sz2, sphi_z, 0);
q_tk = cblas_dnrm2(sN, sphi_z , incx);
q_tk = 0.5 * q_tk * q_tk;
(*sFjacobianPhi)(sN, sz2, sjacobianPhi_z, 1);
/* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
cblas_dgemv(CblasColMajor,CblasTrans, sN, sN, 1.0, sjacobianPhi_z, sN, sphi_z, incx, 0.0, sgrad_psi_z, incx);
qp_tk = cblas_ddot(sN, sgrad_psi_z, 1, sdir_descent, 1);
if (qp_tk < m2 * qp_0 && q_tk < q_0 + m1 * tk * qp_0)
{
/*too small*/
if (niter == 1)
break;
tg = tk;
tk = (tg + td) / 2.0;
continue;
}
else if (q_tk > q_0 + m1 * tk * qp_0)
{
/*too big*/
td = tk;
tk = (tg + td) / 2.0;
continue;
}
else
break;
}
cblas_dcopy(sN, sz2, incx, z, incx);
if ((td - tg) <= tmin)
{
printf("NonSmoothNewton2::lineSearchWolfe warning, resulting tk < tmin, linesearch stopped.\n");
return 0;
}
return 1;
}
/* Linesearch */
int linesearch2_Armijo(int n, double *z, double psi_k, double descentCondition)
{
/* IN :
psi_k (merit function for current iteration)
jacobian_psi_k (jacobian of the merit function)
dk: descent direction
OUT: tk, z
*/
double m1 = 0.1;
double tk = 1;
double tkl, tkr, tkaux;
int incx = 1, incy = 1;
double merit, merit_k;
double tmin = 1e-14;
double qp_tk;
/* cblas_dcopy(sN, z, incx,sz2,incx);*/
/* z1 = z0 + dir */
/* cblas_daxpy(n , 1.0 , sdir_descent , incx , z , incy );*/
tk = 3.25;
while (tk > tmin)
{
/* Computes merit function = 1/2*norm(phi(z_{k+1}))^2 */
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(n , tk , sdir_descent , incx , sz2 , incy);
(*sFphi)(n, sz2, sphi_z, 0);
merit = cblas_dnrm2(n, sphi_z , incx);
merit = 0.5 * merit * merit;
merit_k = psi_k + m1 * tk * descentCondition;
if (merit < merit_k)
{
tkl = 0;
tkr = tk;
/*calcul merit'(tk)*/
(*sFjacobianPhi)(sN, sz2, sjacobianPhi_z, 1);
/* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
cblas_dgemv(CblasColMajor,CblasTrans, sN, sN, 1.0, sjacobianPhi_z, sN, sphi_z, incx, 0.0, sgrad_psi_zaux, incx);
qp_tk = cblas_ddot(sN, sgrad_psi_zaux, 1, sdir_descent, 1);
if (qp_tk > 0)
{
while (fabs(tkl - tkr) > tmin)
{
tkaux = 0.5 * (tkl + tkr);
cblas_dcopy(sN, z, incx, sz2, incx);
cblas_daxpy(n , tkaux , sdir_descent , incx , sz2 , incy);
/*calcul merit'(tk)*/
(*sFphi)(n, sz2, sphi_z, 0);
(*sFjacobianPhi)(sN, sz2, sjacobianPhi_z, 1);
/* Computes the jacobian of the merit function, jacobian_psi = transpose(jacobianPhiMatrix).phiVector */
cblas_dgemv(CblasColMajor,CblasTrans, sN, sN, 1.0, sjacobianPhi_z, sN, sphi_z, incx, 0.0, sgrad_psi_zaux, incx);
qp_tk = cblas_ddot(sN, sgrad_psi_zaux, 1, sdir_descent, 1);
if (qp_tk > 0)
{
tkr = tkaux;
}
else
{
tkl = tkaux;
}
}
}
/* printf("merit = %e, merit_k=%e,tk= %e,tkaux=%e \n",merit,merit_k,tk,tkaux);*/
cblas_dcopy(sN, sz2, incx, z, incx);
break;
}
tk = tk * 0.5;
}
if (tk <= tmin)
{
cblas_dcopy(sN, sz2, incx, z, incx);
printf("NonSmoothNewton::linesearch2_Armijo warning, resulting tk=%e < tmin, linesearch stopped.\n", tk);
return 0;
}
return 1;
}
int Fixe(int n, double* z, int* iparam, double* dparam)
{
int itermax = iparam[0]; // maximum number of iterations allowed
int niter = 0; // current iteration number
double tolerance = dparam[0];
if (verbose > 0)
{
printf(" ============= Starting of Newton process =============\n");
printf(" - tolerance: %f\n - maximum number of iterations: %i\n", tolerance, itermax);
}
int i;
/* Connect F and its jacobian to input functions */
// setFuncEval(F);
/* Memory allocation for phi and its jacobian */
double * www = (double*)malloc(sizeof(double) * n);
double terminationCriterion = 1;
/** Iterations ... */
while ((niter < itermax) && (terminationCriterion > tolerance))
{
++niter;
//printf(" ============= Fixed Point Iteration ============= %i\n",niter);
for (i = 0; i < n ; ++i)
compute_Z_GlockerFixedP(i, www);
terminationCriterion = cblas_dnrm2(n, www, 1);
//printf(" error = %14.7e\n", terminationCriterion);
if (verbose > 0)
{
printf("Non Smooth Newton, iteration number %i, error equal to %14.7e .\n", niter, terminationCriterion);
printf(" -----------------------------------------------------------------------");
}
}
/* Total number of iterations */
iparam[1] = niter;
/* Final error */
dparam[1] = terminationCriterion;
/** Free memory*/
free(www);
if (verbose > 0)
{
if (dparam[1] > tolerance)
printf("Non Smooth Newton warning: no convergence after %i iterations\n" , niter);
else
printf("Non Smooth Newton: convergence after %i iterations\n" , niter);
printf(" The residue is : %e \n", dparam[1]);
}
if (dparam[1] > tolerance)
return 1;
else return 0;
}