void mlcp_FB(MixedLinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options) { *info = 1; NewtonFunctionPtr F = &F_MCPFischerBurmeister; NewtonFunctionPtr jacobianF = &jacobianF_MCPFischerBurmeister; double err; double tol = options->dparam[0]; int i; /*only for debug double * zz = (double *)malloc((sN+sM)*sizeof(double)); memcpy(zz,z,(sN+sM)*sizeof(double));*/ *info = nonSmoothNewtonNeigh(sN + sM, z, &F, &jacobianF, options->iparam, options->dparam); if (*info > 0) { fprintf(stderr, "Numerics, mlcp_FB failed, reached max. number of iterations without convergence. Error = %f\n", options->dparam[1]); /*ONLY FOR DEBUG mixedLinearComplementarity_display(problem); printf("with z init;\n"); for (i=0;i<sN+sM;i++) printf("%.32e \n",zz[i]); exit(1);*/ } /* free(zz);*/ mlcp_compute_error(problem, z, w, tol, &err); for (i = 0; i < sM; i++) { if (z[sN + i] > w[sN + i]) w[sN + i] = 0; } if (err > sMaxError) sMaxError = err; if (verbose || 1) printf("FB : MLCP Solved, error %10.10f and max error %10.10f \n", err, sMaxError); return; }
void mlcp_pgs(MixedLinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options) { if (!problem->isStorageType2) { printf("Siconos/Numerics: mlcp_pgs: Wrong Storage (!isStorageType2) for PGS solver\n"); exit(EXIT_FAILURE); } double* A = problem->A; double* B = problem->B; double* C = problem->C; double* D = problem->D; double* a = problem->a; double* b = problem->b; int n = problem->n; int m = problem->m; double *u = &z[0]; double *v = &z[n]; double *Buf; int incx, incy, incAx, incAy, incBx, incBy; int i, iter; int itermax, verbose; int pgsExplicit; double err, vi; double tol; double prev; double *diagA, *diagB; verbose = 0; incx = 1; incy = 1; /* Recup input */ itermax = options->iparam[0]; pgsExplicit = options->iparam[2]; tol = options->dparam[0]; /* Initialize output */ options->iparam[1] = 0; options->dparam[1] = 0.0; /* Allocation */ diagA = (double*)malloc(n * sizeof(double)); diagB = (double*)malloc(m * sizeof(double)); incx = 1; incy = 1; /* Preparation of the diagonal of the inverse matrix */ for (i = 0 ; i < n ; ++i) { if ((fabs(A[i * n + i]) < DBL_EPSILON)) { if (verbose > 0) { printf(" Vanishing diagonal term \n"); printf(" The local problem cannot be solved \n"); } *info = 2; free(diagA); free(diagB); *info = 1; return; } else { diagA[i] = 1.0 / A[i * n + i]; } } for (i = 0 ; i < m ; ++i) { if ((fabs(B[i * m + i]) < DBL_EPSILON)) { if (verbose > 0) { printf(" Vanishing diagonal term \n"); printf(" The local problem cannot be solved \n"); } *info = 2; free(diagA); free(diagB); return; } else { diagB[i] = 1.0 / B[i * m + i]; } } /*start iterations*/ iter = 0; err = 1.; incx = 1; incy = 1; incAx = n; incAy = 1; incBx = m; incBy = 1; mlcp_compute_error(problem, z, w, tol, &err); while ((iter < itermax) && (err > tol)) { ++iter; incx = 1; incy = 1; if (pgsExplicit) { /*Use w like a buffer*/ cblas_dcopy(n , w , incx , u , incy); //w <- q Buf = w; for (i = 0 ; i < n ; ++i) { prev = Buf[i]; Buf[i] = 0; //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i]; u[i] = - (a[i] + cblas_ddot(n , &A[i] , incAx , Buf , incAy) + cblas_ddot(m , &C[i] , incAx , v , incBy)) * diagA[i]; Buf[i] = prev; } for (i = 0 ; i < m ; ++i) { v[i] = 0.0; //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i]; vi = -(b[i] + cblas_ddot(n , &D[i] , incBx , u , incAy) + cblas_ddot(m , &B[i] , incBx , v , incBy)) * diagB[i]; if (vi < 0) v[i] = 0.0; else v[i] = vi; } } else { for (i = 0 ; i < n ; ++i) { u[i] = 0.0; //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i]; u[i] = - (a[i] + cblas_ddot(n , &A[i] , incAx , u , incAy) + cblas_ddot(m , &C[i] , incAx , v , incBy)) * diagA[i]; } for (i = 0 ; i < m ; ++i) { v[i] = 0.0; //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i]; vi = -(b[i] + cblas_ddot(n , &D[i] , incBx , u , incAy) + cblas_ddot(m , &B[i] , incBx , v , incBy)) * diagB[i]; if (vi < 0) v[i] = 0.0; else v[i] = vi; } } /* **** Criterium convergence compliant with filter_result_MLCP **** */ mlcp_compute_error(problem, z, w, tol, &err); if (verbose == 2) { printf(" # i%d -- %g : ", iter, err); for (i = 0 ; i < n ; ++i) printf(" %g", u[i]); for (i = 0 ; i < m ; ++i) printf(" %g", v[i]); for (i = 0 ; i < m ; ++i) printf(" %g", w[i]); printf("\n"); } /* **** ********************* **** */ } options->iparam[1] = iter; options->dparam[1] = err; if (err > tol) { printf("Siconos/Numerics: mlcp_pgs: No convergence of PGS after %d iterations\n" , iter); printf("Siconos/Numerics: mlcp_pgs: The residue is : %g \n", err); *info = 1; } else { if (verbose > 0) { printf("Siconos/Numerics: mlcp_pgs: Convergence of PGS after %d iterations\n" , iter); printf("Siconos/Numerics: mlcp_pgs: The residue is : %g \n", err); } *info = 0; } free(diagA); free(diagB); return; }
void FB_compute_error_mlcp(void* data_opaque, double* z, double* w, double* nabla_theta, double tol, double* err) { mlcp_compute_error((MixedLinearComplementarityProblem *)data_opaque, z, w, tol, err); }
void mlcp_pgs_SBM(MixedLinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options) { /* Notes: - we suppose that the trivial solution case has been checked before, and that all inputs differs from NULL since this function is supposed to be called from lcp_driver_global(). - Input matrix M of the problem is supposed to be sparse-block with no null row (ie no rows with all blocks equal to null) */ if (problem->M->matrix1 == NULL) { fprintf(stderr, "mlcp_NSGS_SBM error: wrong storage type for input matrix M of the LCP.\n"); exit(EXIT_FAILURE); } /* The options for the global "block" solver are defined in options[0].\n options[i], for 0<i<numberOfSolvers-1 correspond to local solvers. */ /* Global Solver parameters*/ int itermax = options[0].iparam[0]; double tolerance = options[0].dparam[0]; /* Matrix M/vector q of the MLCP */ SparseBlockStructuredMatrix* blmat = problem->M->matrix1; double * q = problem->q; /* Number of non-null blocks in blmat */ int nbOfNonNullBlocks = blmat->nbblocks; if (nbOfNonNullBlocks < 1) { fprintf(stderr, "Numerics::mlcp_NSGS_SBM error: empty M matrix (all blocks = NULL).\n"); exit(EXIT_FAILURE); } /* Local problem initialization */ /* We choose to set a LCP problem which can be solved by a Linear System solver if the block corresponds to equalities */ LinearComplementarityProblem * local_problem = (LinearComplementarityProblem *)malloc(sizeof(*local_problem)); local_problem->M = (NumericsMatrix *)malloc(sizeof(*local_problem->M)); local_problem->M->storageType = 0; // dense storage local_problem->M->matrix0 = NULL; local_problem->M->matrix1 = NULL; local_problem->M->matrix2 = NULL; local_problem->M->internalData = NULL; /* Memory allocation for q. Size of q = blsizemax, size of the largest square-block in blmat */ int blsizemax = blmat->blocksize0[0]; int k; for (unsigned int i = 1 ; i < blmat->blocknumber0 ; i++) { k = blmat->blocksize0[i] - blmat->blocksize0[i - 1]; if (k > blsizemax) blsizemax = k; } local_problem->q = (double*)malloc(blsizemax * sizeof(double)); /* Current row (of blocks) number */ unsigned int rowNumber; /***** Gauss-Seidel iterations *****/ int iter = 0; /* Current iteration number */ double error = 1.; /* Current error */ int hasNotConverged = 1; /* Output from local solver */ options[0].iparam[2] = 0; options[0].dparam[2] = 0.0; if (options->numberOfInternalSolvers < 1) { numericsError("mlcp_nsgs_SBM", "The MLCP_PGS_SBM method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1"); } /*Number of the local solver */ int localSolverNum = options->numberOfInternalSolvers ; SolverOptions * internalSolvers = options->internalSolvers ; int pos = 0; /* Output from local solver */ int infoLocal = -1; while ((iter < itermax) && (hasNotConverged > 0)) { ++iter; /* Loop over the rows of blocks in blmat */ localSolverNum = 0; pos = 0; /* cblas_dcopy(problem->size,w,1,wBackup,1); */ for (rowNumber = 0; rowNumber < blmat->blocknumber0; ++rowNumber) { /* Local problem formalization */ mlcp_pgs_sbm_buildLocalProblem(rowNumber, blmat, local_problem, q, z); /* Solve local problem */ if (problem->blocksIsComp[rowNumber]) { infoLocal = lcp_driver_DenseMatrix(local_problem, &z[pos], &w[pos], &internalSolvers[localSolverNum]); } else /* Solve a linear system*/ { if (local_problem->size ==1) { double d = local_problem->M->matrix0[0]; if (fabs(d) < DBL_EPSILON) { printf("Numerics::mlcp_pgs_sbm, error: vanishing diagonal term \n"); printf(" The problem cannot be solved with this method \n"); exit(EXIT_FAILURE); } w[pos] = 0.0; z[pos] = local_problem->q[0]/d; } else { printf("Numerics::mlcp_pgs_sbm, error: nontrivial diagonal term no yet implemented\n"); exit(EXIT_FAILURE); } } pos += local_problem->size; /* sum of local number of iterations (output from local_driver)*/ if (options[localSolverNum].iparam != NULL) options[0].iparam[2] += internalSolvers[localSolverNum].iparam[1]; /* sum of local errors (output from local_driver)*/ options[0].dparam[2] += internalSolvers[localSolverNum].dparam[1]; if (infoLocal > 0) { //free(local_problem->q); //free(local_problem->M); //free(local_problem); /* Number of GS iterations */ options[0].iparam[1] = iter; fprintf(stderr, "MCLP_PGS_SBM error: Warning local LCP solver at global iteration %d.\n for block-row number %d. Output info equal to %d.\n", iter, rowNumber, infoLocal); //exit(EXIT_FAILURE); break; } while (localSolverNum < options->numberOfInternalSolvers - 1) localSolverNum++; } /* cblas_dcopy(problem->size , problem->q , 1 , w , 1); */ /* prod(problem->size,problem->size, 1.0, problem->M,z,1.0,w); */ /* cblas_daxpy(problem->size, -1.0, w,1,wBackup, 1); */ /* num = cblas_dnrm2(problem->size,wBackup,1); */ /* error = num*den; */ /* Criterium convergence */ hasNotConverged = mlcp_compute_error(problem, z, w, tolerance, &error); /* if(error<tolerance) hasNotConverged = 0; */ } *info = hasNotConverged; /* Number of GS iterations */ options[0].iparam[1] = iter; /* Resulting error */ options[0].dparam[1] = error; free(local_problem->q); free(local_problem->M); free(local_problem); /* free(wBackup); */ }
void mlcp_path(MixedLinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options) { *info = 1; #ifdef HAVE_PATHFERRIS *info = 0; MCP_Termination termination; double tol = options->dparam[0]; double * M = problem->M->matrix0; double * q = problem->q; int nnz, i, j, n, m, dim, numLine; n = problem->n; m = problem->m; dim = m + n; /* if (verbose){ printf("initial values for z:\n"); for (int i=0;i<dim;i++) printf("%.15e\n", z[i]); printf("initial values for w:\n"); for (int i=0;i<dim;i++) printf("%.15e\n", w[i]); }*/ nnz = nbNonNulElems(dim, M, 1.0e-18); int * m_i = (int *)calloc(nnz + 1, sizeof(int)); int * m_j = (int *)calloc(nnz + 1, sizeof(int)); double * m_ij = (double *)calloc(nnz + 1, sizeof(double)); double * lb = (double *)calloc(dim + 1, sizeof(double)); double * ub = (double *)calloc(dim + 1, sizeof(double)); // double * u = z; // double * v = z+n; double err; FortranToPathSparse(dim, M, 1.0e-18, m_i, m_j, m_ij); if (problem->blocksRows) { int numBlock = 0; while (problem->blocksRows[numBlock] < n + m) { if (!problem->blocksIsComp[numBlock]) { for (numLine = problem->blocksRows[numBlock] ; numLine < problem->blocksRows[numBlock + 1]; numLine++) { lb[numLine] = -1e20; ub[numLine] = 1e20; } } else { for (numLine = problem->blocksRows[numBlock] ; numLine < problem->blocksRows[numBlock + 1]; numLine++) { lb[numLine] = 0; ub[numLine] = 1e20; } } numBlock++; } } else { printf("DEPRECED MLCP INTERFACE\n"); for (i = 0; i < n; i++) { lb[i] = -1e20; ub[i] = 1e20; } for (i = n; i < n + m; i++) { lb[i] = 0; ub[i] = 1e20; } } if (verbose) printLCP(dim, nnz, m_i, m_j, m_ij, q, lb, ub); SimpleLCP(dim, nnz, m_i, m_j, m_ij, q, lb, ub, &termination, z); if (termination == MCP_Error) { *info = 1; if (verbose) printf("PATH : Error in the solution.\n"); } else if (termination == MCP_Solved) { /* for (i=0;i<n;i++){ */ /* u[i]=z[i]; */ /* } */ if (problem->blocksRows) { int numBlock = 0; while (problem->blocksRows[numBlock] < n + m) { if (!problem->blocksIsComp[numBlock]) { for (numLine = problem->blocksRows[numBlock] ; numLine < problem->blocksRows[numBlock + 1]; numLine++) { w[numLine] = 0; } } else { for (numLine = problem->blocksRows[numBlock] ; numLine < problem->blocksRows[numBlock + 1]; numLine++) { w[numLine] = -q[numLine]; for (int jj = 0; jj < n + m; jj++) { w[numLine] += M[numLine + dim * jj] * z[jj]; } } } numBlock++; } } else { for (i = 0; i < n; i++) w[i] = 0; for (i = n; i < n + m; i++) { w[i] = -q[i]; for (j = 0; j < n + m; j++) { w[i] += M[i + dim * j] * z[j]; } } } /*1e-7 because it is the default tol of path.*/ mlcp_compute_error(problem, z, w, tol, &err); if (err > 1e-7) { printf("PATH : MLCP Solved, error %10.7f.\n", err); //*info = 1; } if (problem->blocksRows) { int numBlock = 0; while (problem->blocksRows[numBlock] < n + m) { if (problem->blocksIsComp[numBlock]) { for (numLine = problem->blocksRows[numBlock] ; numLine < problem->blocksRows[numBlock + 1]; numLine++) { if (z[numLine] > w[numLine]) w[numLine] = 0; } } numBlock++; } } else { for (i = 0; i < m; i++) { if (z[n + i] > w[n + i]) w[n + i] = 0; } } if (verbose) printf("PATH : MLCP Solved, error %10.7f.\n", err); } else { if (verbose) printf("PATH : MLCP Other error: %d\n", termination); } free(m_i); free(m_j); free(m_ij); free(lb); free(ub); #endif /*HAVE_PATHFERRIS*/ return; }