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;
}
