/*
* options->iWork containing the int work memory.
* options->dWork containing the double work memory.
* options->iparam[4] : use DGELS (1) or DGESV (0).
*/
int LinearSystem_driver(LinearSystemProblem* problem, double *z , double *w, SolverOptions* options)
{
int i;
assert(problem->M);
if (problem->M->storageType == 1)
numericsError("LinearSystem_driver", "forbidden type of storage for the matrix M of the LCP");
#ifdef LINEARSYSTEM_DEBUG
displayLS(problem);
#endif
for (i = 0; i < problem->size; i++)
w[i] = 0.0;
int res = 0;
if (options && options->iparam[4])
{
res = solveLeastSquareProblem(problem, z , options);
}
else
{
res = myLu(problem, z , options);
}
if (options && options->filterOn)
{
options->dparam[1] = LinearSystem_computeError(problem, z);
printf("LinearSystem_driver solved with error = %e\n", LinearSystem_computeError(problem, z));
printf("The solution is :\n");
for (i = 0; i < problem->size; i++)
printf(" %e", z[i]);
printf("\n");
}
return res;
}
int globalFrictionContact3D_setDefaultSolverOptions(SolverOptions* options, int solverId)
{
int info = -1;
switch (solverId)
{
case SICONOS_FRICTION_3D_GLOBAL_NSGS:
{
info = frictionContact3D_nsgs_setDefaultSolverOptions(options);
options->solverId = SICONOS_FRICTION_3D_GLOBAL_NSGS;
break;
}
case SICONOS_FRICTION_3D_GLOBAL_LOCALAC_WR:
{
info = globalFrictionContact3D_globalAlartCurnier_wr_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_NSGS_WR:
{
info = globalFrictionContact3D_nsgs_wr_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_NSGSV_WR:
{
info = globalFrictionContact3D_nsgs_velocity_wr_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_PROX_WR:
{
info = globalFrictionContact3D_proximal_wr_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_DSFP_WR:
{
info = globalFrictionContact3D_DeSaxceFixedPoint_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_TFP_WR:
{
info = globalFrictionContact3D_TrescaFixedPoint_setDefaultSolverOptions(options);
break;
}
case SICONOS_FRICTION_3D_GLOBAL_AC:
{
info = globalFrictionContact3D_AlartCurnier_setDefaultSolverOptions(options);
break;
}
default:
{
numericsError("globalFrictionContact3D_setDefaultSolverOptions", "Unknown Solver");
}
}
return info;
}
int myLu(LinearSystemProblem* problem, double *z , SolverOptions* options)
{
/* Checks inputs */
if (problem == NULL || z == NULL)
numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)");
/* Output info. : 0: ok - >0: problem (depends on solver) */
int info = -1;
int n = problem->size;
int n2 = n * n;
double * Maux = 0;
int * ipiv = 0;
if (options && options->dWork)
Maux = options->dWork;
else
Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double));
if (options && options->iWork)
ipiv = options->iWork;
else
ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int));
int LAinfo = 0;
//displayLS(problem);
assert(problem->M->matrix0);
assert(problem->q);
memcpy(Maux, problem->M->matrix0, n2 * sizeof(double));
// memcpy(z,problem->q,n*sizeof(double));
for (int ii = 0; ii < n; ii++)
z[ii] = -problem->q[ii];
DGESV(n, 1, Maux, n, ipiv, z, n, &LAinfo);
if (!LAinfo)
{
info = 0;
}
else
{
printf("Equality_driver:: LU factorization failed:\n");
}
//printf("LinearSystem_driver: computeError of LinearSystem : %e\n",LinearSystemComputeError(problem,z));
if (!(options && options->dWork))
free(Maux);
if (!(options && options->iWork))
free(ipiv);
return info;
}
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;
}
void AC_fillMLocal(FrictionContactProblem * problem, FrictionContactProblem * localproblem, int contact)
{
NumericsMatrix * MGlobal = problem->M;
int n = 3 * problem->numberOfContacts;
// Dense storage
int storageType = MGlobal->storageType;
if (storageType == 0)
{
int in = 3 * contact, it = in + 1, is = it + 1;
int inc = n * in;
double * MM = MGlobal->matrix0;
double * MLocal = localproblem->M->matrix0;
/* The part of MM which corresponds to the current block is copied into MLocal */
MLocal[0] = MM[inc + in];
MLocal[1] = MM[inc + it];
MLocal[2] = MM[inc + is];
inc += n;
MLocal[3] = MM[inc + in];
MLocal[4] = MM[inc + it];
MLocal[5] = MM[inc + is];
inc += n;
MLocal[6] = MM[inc + in];
MLocal[7] = MM[inc + it];
MLocal[8] = MM[inc + is];
}
else if (storageType == 1)
{
int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact);
localproblem->M->matrix0 = MGlobal->matrix1->block[diagPos];
/* cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1); */
}
else
numericsError("FrictionContact3D_AlartCurnier:AC_fillMLocal() -", "unknown storage type for matrix M");
}
int variationalInequality_setDefaultSolverOptions(SolverOptions* options, int solverId)
{
null_SolverOptions(options);
int info = -1;
switch (solverId)
{
case SICONOS_VI_EG:
{
info = variationalInequality_ExtraGradient_setDefaultSolverOptions(options);
break;
}
case SICONOS_VI_FPP:
{
info = variationalInequality_FixedPointProjection_setDefaultSolverOptions(options);
break;
}
case SICONOS_VI_HP:
{
info = variationalInequality_HyperplaneProjection_setDefaultSolverOptions(options);
break;
}
case SICONOS_VI_BOX_QI:
{
info = variationalInequality_common_setDefaultSolverOptions(options, solverId);
break;
}
default:
{
numericsError("variationalInequality_setDefaultSolverOptions", "Unknown Solver");
}
}
return info;
}
void fc3d_DeSaxceFixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
double* q = problem->q;
NumericsMatrix* M = problem->M;
double* mu = problem->mu;
/* Dimension of the problem */
int n = 3 * nc;
/* Maximum number of iterations */
int itermax = iparam[0];
/* Tolerance */
double tolerance = dparam[0];
/***** Fixed point iterations *****/
int iter = 0; /* Current iteration number */
double error = 1.; /* Current error */
int hasNotConverged = 1;
int contact; /* Number of the current row of blocks in M */
int nLocal = 3;
dparam[0] = dparam[2]; // set the tolerance for the local solver
double * velocitytmp = (double *)malloc(n * sizeof(double));
double rho = 0.0;
if (dparam[3] > 0.0)
{
rho = dparam[3];
if (verbose > 0)
{
printf("----------------------------------- FC3D - DeSaxce Fixed Point (DSFP) - Fixed stepsize with rho = %14.7e \n", rho);
}
}
else
{
numericsError("fc3d_DeSaxceFixedPoint", "The De Saxce fixed point is implemented with a fixed time--step. Use FixedPointProjection (VI_FPP) method for a variable time--step");
}
double alpha = 1.0;
double beta = 1.0;
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
/* velocitytmp <- q */
cblas_dcopy(n , q , 1 , velocitytmp, 1);
/* velocitytmp <- q + M * reaction */
beta = 1.0;
prodNumericsMatrix(n, n, alpha, M, reaction, beta, velocitytmp);
/* projection for each contact */
for (contact = 0 ; contact < nc ; ++contact)
{
int pos = contact * nLocal;
double normUT = sqrt(velocitytmp[pos + 1] * velocitytmp[pos + 1] + velocitytmp[pos + 2] * velocitytmp[pos + 2]);
reaction[pos] -= rho * (velocitytmp[pos] + mu[contact] * normUT);
reaction[pos + 1] -= rho * velocitytmp[pos + 1];
reaction[pos + 2] -= rho * velocitytmp[pos + 2];
projectionOnCone(&reaction[pos], mu[contact]);
}
/* **** Criterium convergence **** */
fc3d_compute_error(problem, reaction , velocity, tolerance, options, &error);
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env,
nc * 3, reaction, velocity,
error, NULL);
}
if (verbose > 0)
printf("----------------------------------- FC3D - DeSaxce Fixed Point (DSFP) - Iteration %i rho = %14.7e \tError = %14.7e\n", iter, rho, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
if (verbose > 0)
printf("----------------------------------- FC3D - DeSaxce Fixed point (DSFP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
iparam[7] = iter;
dparam[0] = tolerance;
dparam[1] = error;
free(velocitytmp);
}
int fc3d_driver(FrictionContactProblem* problem,
double *reaction, double *velocity,
SolverOptions* options,
NumericsOptions* global_options)
{
if (options == NULL)
numericsError("fc3d_driver", "null input for solver and/or global options");
int setnumericsoptions=0;
/* Set global options */
if (global_options)
{
setNumericsOptions(global_options);
options->numericsOptions = (NumericsOptions*) malloc(sizeof(NumericsOptions));
options->numericsOptions->verboseMode = global_options->verboseMode;
setnumericsoptions=1;
}
int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */
if (!NoDefaultOptions)
readSolverOptions(3, options);
if (verbose > 0)
printSolverOptions(options);
/* Solver name */
/*char * name = options->solverName;*/
int info = -1 ;
if (problem->dimension != 3)
numericsError("fc3d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set");
/* Check for trivial case */
info = checkTrivialCase(problem, velocity, reaction, options);
if (info == 0)
goto exit;
switch (options->solverId)
{
/* Non Smooth Gauss Seidel (NSGS) */
case SICONOS_FRICTION_3D_NSGS:
{
snPrintf(1, options,
" ========================== Call NSGS solver for Friction-Contact 3D problem ==========================\n");
fc3d_nsgs(problem, reaction , velocity , &info , options);
break;
}
case SICONOS_FRICTION_3D_NSGSV:
{
snPrintf(1, options,
" ========================== Call NSGSV solver for Friction-Contact 3D problem ==========================\n");
fc3d_nsgs_velocity(problem, reaction , velocity , &info , options);
break;
}
/* Proximal point algorithm */
case SICONOS_FRICTION_3D_PROX:
{
snPrintf(1, options,
" ========================== Call PROX (Proximal Point) solver for Friction-Contact 3D problem ==========================\n");
fc3d_proximal(problem, reaction , velocity , &info , options);
break;
}
/* Tresca Fixed point algorithm */
case SICONOS_FRICTION_3D_TFP:
{
snPrintf(1, options,
" ========================== Call TFP (Tresca Fixed Point) solver for Friction-Contact 3D problem ==========================\n");
fc3d_TrescaFixedPoint(problem, reaction , velocity , &info , options);
break;
}
/* ACLM Fixed point algorithm */
case SICONOS_FRICTION_3D_ACLMFP:
{
snPrintf(1, options,
" ========================== Call ACLM (Acary Cadoux Lemarechal Malick Fixed Point) solver for Friction-Contact 3D problem ==========================\n");
fc3d_ACLMFixedPoint(problem, reaction , velocity , &info , options);
break;
}
/* SOCLCP Fixed point algorithm */
case SICONOS_FRICTION_3D_SOCLCP:
{
snPrintf(1, options,
" ========================== Call SOCLCP solver for Friction-Contact 3D problem (Associated one) ==========================\n");
fc3d_SOCLCP(problem, reaction , velocity , &info , options);
break;
}
/* De Saxce Fixed point algorithm */
case SICONOS_FRICTION_3D_DSFP:
{
snPrintf(1, options,
" ========================== Call DeSaxce Fixed Point (DSFP) solver for Friction-Contact 3D problem ==========================\n");
fc3d_DeSaxceFixedPoint(problem, reaction , velocity , &info , options);
break;
}
/* Fixed point projection algorithm */
case SICONOS_FRICTION_3D_FPP:
{
snPrintf(1, options,
" ========================== Call Fixed Point Projection (FPP) solver for Friction-Contact 3D problem ==========================\n");
fc3d_fixedPointProjection(problem, reaction , velocity , &info , options);
break;
}
/* Extra Gradient algorithm */
case SICONOS_FRICTION_3D_EG:
{
snPrintf(1, options,
" ========================== Call ExtraGradient (EG) solver for Friction-Contact 3D problem ==========================\n");
fc3d_ExtraGradient(problem, reaction , velocity , &info , options);
break;
}
/* VI Fixed Point Projection algorithm */
case SICONOS_FRICTION_3D_VI_FPP:
{
snPrintf(1, options,
" ========================== Call VI_FixedPointProjection (VI_FPP) solver for Friction-Contact 3D problem ==========================\n");
fc3d_VI_FixedPointProjection(problem, reaction , velocity , &info , options);
break;
}
/* VI Extra Gradient algorithm */
case SICONOS_FRICTION_3D_VI_EG:
{
snPrintf(1, options,
" ========================== Call VI_ExtraGradient (VI_EG) solver for Friction-Contact 3D problem ==========================\n");
fc3d_VI_ExtraGradient(problem, reaction , velocity , &info , options);
break;
}
/* Hyperplane Projection algorithm */
case SICONOS_FRICTION_3D_HP:
{
snPrintf(1, options,
" ========================== Call Hyperplane Projection (HP) solver for Friction-Contact 3D problem ==========================\n");
fc3d_HyperplaneProjection(problem, reaction , velocity , &info , options);
break;
}
/* Alart Curnier in local coordinates */
case SICONOS_FRICTION_3D_NSN_AC:
{
snPrintf(1, options,
" ========================== Call Alart Curnier solver for Friction-Contact 3D problem ==========================\n");
if (problem->M->matrix0)
{
fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options);
}
else
{
fc3d_nonsmooth_Newton_AlartCurnier(problem, reaction , velocity , &info , options);
}
break;
}
/* Fischer Burmeister in local coordinates */
case SICONOS_FRICTION_3D_NSN_FB:
{
snPrintf(1, options,
" ========================== Call Fischer Burmeister solver for Friction-Contact 3D problem ==========================\n");
fc3d_nonsmooth_Newton_FischerBurmeister(problem, reaction , velocity , &info , options);
break;
}
case SICONOS_FRICTION_3D_NSN_NM:
{
snPrintf(1, options,
" ========================== Call natural map solver for Friction-Contact 3D problem ==========================\n");
fc3d_nonsmooth_Newton_NaturalMap(problem, reaction , velocity , &info , options);
break;
}
case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC_NU:
case SICONOS_FRICTION_3D_ONECONTACT_QUARTIC:
{
snPrintf(1, options,
" ========================== Call Quartic solver for Friction-Contact 3D problem ==========================\n");
fc3d_unitary_enumerative(problem, reaction , velocity , &info , options);
break;
}
case SICONOS_FRICTION_3D_ONECONTACT_NSN_AC:
case SICONOS_FRICTION_3D_ONECONTACT_NSN_AC_GP:
{
snPrintf(1, options,
" ========================== Call Newton-based solver for one contact Friction-Contact 3D problem ==========================\n");
fc3d_onecontact_nonsmooth_Newton_solvers_initialize(problem, problem, options);
info = fc3d_onecontact_nonsmooth_Newton_solvers_solve(problem, reaction , options);
break;
}
case SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration:
{
snPrintf(1, options,
" ========================== Call Projection on cone solver for one contact Friction-Contact 3D problem ==========================\n");
fc3d_projectionOnConeWithLocalIteration_initialize(problem, problem, options);
info = fc3d_projectionOnConeWithLocalIteration_solve(problem, reaction , options);
fc3d_projectionOnConeWithLocalIteration_free(problem, problem, options);
break;
}
case SICONOS_FRICTION_3D_GAMS_PATH:
{
snPrintf(1, options,
" ========================== Call PATH solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
fc3d_AVI_gams_path(problem, reaction , velocity, &info, options);
break;
}
case SICONOS_FRICTION_3D_GAMS_PATHVI:
{
snPrintf(1, options,
" ========================== Call PATHVI solver via GAMS for an AVI Friction-Contact 3D problem ==========================\n");
fc3d_AVI_gams_pathvi(problem, reaction , velocity, &info, options);
break;
}
case SICONOS_FRICTION_3D_GAMS_LCP_PATH:
{
snPrintf(1, options,
" ========================== Call PATH solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n");
fc3d_lcp_gams_path(problem, reaction , velocity, &info, options);
break;
}
case SICONOS_FRICTION_3D_GAMS_LCP_PATHVI:
{
snPrintf(1, options,
" ========================== Call PATHVI solver via GAMS for an LCP-based reformulation of the AVI Friction-Contact 3D problem ==========================\n");
fc3d_lcp_gams_pathvi(problem, reaction , velocity, &info, options);
break;
}
default:
{
fprintf(stderr, "Numerics, fc3d_driver failed. Unknown solver.\n");
exit(EXIT_FAILURE);
}
}
exit:
if (setnumericsoptions)
{
free(options->numericsOptions);
options->numericsOptions = NULL;
}
return info;
}
int soclcp_setDefaultSolverOptions(SolverOptions* options, int solverId)
{
options->iparam = NULL;
options->dparam = NULL;
null_SolverOptions(options);
int info = -1;
switch(solverId)
{
case SICONOS_SOCLCP_NSGS:
{
info = soclcp_nsgs_setDefaultSolverOptions(options);
break;
}
/* case SICONOS_SOCLCP_NSGSV: */
/* { */
/* info = soclcp_nsgs_velocity_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_*/
/* { */
/* info = soclcp_proximal_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_TFP: */
/* { */
/* info = soclcp_TrescaFixedPoint_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_DSFP: */
/* { */
/* info = soclcp_DeSaxceFixedPoint_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_FPP: */
/* { */
/* info = soclcp_fixedPointProjection_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_EG: */
/* { */
/* info = soclcp_ExtraGradient_setDefaultSolverOptions(options); */
/* break; */
/* } */
case SICONOS_SOCLCP_VI_FPP:
{
info = soclcp_VI_FixedPointProjection_setDefaultSolverOptions(options);
break;
}
case SICONOS_SOCLCP_VI_EG:
{
info = soclcp_VI_ExtraGradient_setDefaultSolverOptions(options);
break;
}
/* case SICONOS_SOCLCP_HP: */
/* { */
/* info = soclcp_HyperplaneProjection_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_LOCALAC: */
/* { */
/* info = soclcp_AlartCurnier_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_LOCALFB: */
/* { */
/* info = soclcp_FischerBurmeister_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_QUARTIC: */
/* { */
/* info = soclcp_unitary_enumerative_setDefaultSolverOptions(options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_QUARTIC_NU: */
/* { */
/* info = soclcp_unitary_enumerative_setDefaultSolverOptions(options); */
/* options->solverId = SICONOS_SOCLCP_QUARTIC_NU; */
/* break; */
/* } */
default:
{
numericsError("soclcp_setDefaultSolverOptions", "Unknown Solver");
}
}
return info;
}
int soclcp_driver(SecondOrderConeLinearComplementarityProblem* problem,
double *r, double *v,
SolverOptions* options,
NumericsOptions* global_options)
{
if(options == NULL)
numericsError("soclcp_driver", "null input for solver and/or global options");
int setnumericsoptions=0;
/* Set global options */
if(global_options)
{
setNumericsOptions(global_options);
options->numericsOptions = (NumericsOptions*) malloc(sizeof(NumericsOptions));
options->numericsOptions->verboseMode = global_options->verboseMode;
setnumericsoptions=1;
}
int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */
if(!NoDefaultOptions)
readSolverOptions(3, options);
if(verbose > 0)
printSolverOptions(options);
/* Solver name */
/*char * name = options->solverName;*/
int info = -1 ;
/* Check for trivial case */
info = soclcp_checkTrivialCase(problem, v, r, options);
if(info == 0)
return info;
switch(options->solverId)
{
/* Non Smooth Gauss Seidel (NSGS) */
case SICONOS_SOCLCP_NSGS:
{
snPrintf(1, options,
" ========================== Call NSGS solver for Second Order Cone LCP problem ==========================\n");
soclcp_nsgs(problem, r , v , &info , options);
break;
}
/* case SICONOS_SOCLCP_NSGSV: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call NSGSV solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_nsgs_v(problem, r , v , &info , options); */
/* break; */
/* } */
/* /\* Proximal point algorithm *\/ */
/* case SICONOS_SOCLCP_PROX: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call PROX (Proximal Point) solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_proximal(problem, r , v , &info , options); */
/* break; */
/* } */
/* /\* Tresca Fixed point algorithm *\/ */
/* case SICONOS_SOCLCP_TFP: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call TFP (Tresca Fixed Point) solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_TrescaFixedPoint(problem, r , v , &info , options); */
/* break; */
/* } */
case SICONOS_SOCLCP_VI_FPP:
{
snPrintf(1, options,
" ========================== Call VI_FixedPointProjection (VI_FPP) solver for Second Order Cone LCP problem ==========================\n");
soclcp_VI_FixedPointProjection(problem, r , v , &info , options);
break;
}
/* VI Extra Gradient algorithm */
case SICONOS_SOCLCP_VI_EG:
{
snPrintf(1, options,
" ========================== Call VI_ExtraGradient (VI_EG) solver for Second Order Cone LCP problem ==========================\n");
soclcp_VI_ExtraGradient(problem, r , v , &info , options);
break;
}
/* /\* Hyperplane Projection algorithm *\/ */
/* case SICONOS_SOCLCP_HP: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call Hyperplane Projection (HP) solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_HyperplaneProjection(problem, r , v , &info , options); */
/* break; */
/* } */
/* /\* Alart Curnier in local coordinates *\/ */
/* case SICONOS_SOCLCP_LOCALAC: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call Alart Curnier solver for Second Order Cone LCP problem ==========================\n"); */
/* if (problem->M->matrix0) */
/* { */
/* soclcp_localAlartCurnier(problem, r , v , &info , options); */
/* } */
/* else */
/* { */
/* soclcp_localAlartCurnier(problem, r , v , &info , options); */
/* } */
/* break; */
/* } */
/* /\* Fischer Burmeister in local coordinates *\/ */
/* case SICONOS_SOCLCP_LOCALFB: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call Fischer Burmeister solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_localFischerBurmeister(problem, r , v , &info , options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_QUARTIC_NU: */
/* case SICONOS_SOCLCP_QUARTIC: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */
/* soclcp_unitary_enumerative(problem, r , v , &info , options); */
/* break; */
/* } */
/* case SICONOS_SOCLCP_AlartCurnierNewton: */
/* case SICONOS_SOCLCP_DampedAlartCurnierNewton: */
/* { */
/* snPrintf(1, options, */
/* " ========================== Call Quartic solver for Second Order Cone LCP problem ==========================\n"); */
/* info =soclcp_Newton_solve(problem, r , options); */
/* break; */
/* } */
default:
{
fprintf(stderr, "Numerics, SecondOrderConeLinearComplementarity_driver failed. Unknown solver.\n");
exit(EXIT_FAILURE);
}
}
if(setnumericsoptions)
{
free(options->numericsOptions);
options->numericsOptions = NULL;
}
return info;
}
int fc2d_tolcp(FrictionContactProblem* problem, LinearComplementarityProblem * lcp_problem)
{
if (problem->dimension != 2)
{
numericsError("fc2d_tolcp", "Dimension of the problem : problem-> dimension is not compatible or is not set");
return 1;
}
int nc = problem->numberOfContacts;
lcp_problem->size = 3 * nc ;
lcp_problem->M = newNumericsMatrix();
lcp_problem->M->size0 = 3 * nc ;
lcp_problem->M->size1 = 3 * nc ;
lcp_problem->M->storageType = 0;
lcp_problem->M->matrix1 = NULL;
lcp_problem->M->matrix2 = NULL;
lcp_problem->M->internalData = NULL;
lcp_problem->M->matrix0 = (double*)malloc(lcp_problem->size * lcp_problem->size * sizeof(double));;
lcp_problem->q = (double*)malloc(lcp_problem->size * sizeof(double));
int n = 2 * nc;
int i, j;
for (i = 0; i < nc; i++)
{
for (j = 0; j < nc; j++)
{
/* first Column */
lcp_problem->M->matrix0[3 * i + 3 * j * lcp_problem->size] =
problem->M->matrix0[2 * i + 2 * j * n] - problem->mu[i] * problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // compute W_NN-mu W_NT
lcp_problem->M->matrix0[3 * i + 1 + 3 * j * lcp_problem->size] =
problem->M->matrix0[(2 * i + 1) + 2 * j * n] - problem->mu[i] * problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // compute W_TN-mu W_TT
if (i == j)
{
lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 2.0 * problem->mu[i];
}
else
{
lcp_problem->M->matrix0[3 * i + 2 + 3 * j * lcp_problem->size] = 0.0;
}
/* second Column */
lcp_problem->M->matrix0[3 * i + (3 * j + 1)*lcp_problem->size] =
problem->M->matrix0[2 * i + (2 * j + 1) * n] ; // set W_NT
lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 1)*lcp_problem->size] =
problem->M->matrix0[(2 * i + 1) + (2 * j + 1) * n] ; // set WTT
if (i == j)
{
lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] = -1.0;
}
else
{
lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 1)*lcp_problem->size] = 0.0;
}
/* Third Column */
lcp_problem->M->matrix0[3 * i + (3 * j + 2)*lcp_problem->size] = 0.0;
if (i == j)
{
lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] = 1.0;
}
else
{
lcp_problem->M->matrix0[3 * i + 1 + (3 * j + 2)*lcp_problem->size] = 0.0;
}
lcp_problem->M->matrix0[3 * i + 2 + (3 * j + 2)*lcp_problem->size] = 0.0;
}
}
for (i = 0; i < nc; i++)
{
lcp_problem->q[3 * i ] = problem->q[2 * i];
lcp_problem->q[3 * i + 1] = problem->q[2 * i + 1];
lcp_problem->q[3 * i + 2] = 0.0;;
}
return 0;
}
void fc3d_ACLMFixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Maximum number of iterations */
int itermax = iparam[0];
/* Tolerance */
double tolerance = dparam[0];
if (options->numberOfInternalSolvers < 1)
{
numericsError("fc3d_ACLMFixedpoint", "The ACLM Fixed Point method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
}
SolverOptions * internalsolver_options = options->internalSolvers;
if (verbose > 0)
{
printSolverOptions(options);
}
/***** Fixed Point Iterations *****/
int iter = 0; /* Current iteration number */
double error = 1.; /* Current error */
int hasNotConverged = 1;
soclcp_InternalSolverPtr internalsolver;
SecondOrderConeLinearComplementarityProblem* soclcp = (SecondOrderConeLinearComplementarityProblem *)malloc(sizeof(SecondOrderConeLinearComplementarityProblem));
soclcp->n = problem->numberOfContacts * problem->dimension;
soclcp->nc= problem->numberOfContacts;
soclcp->M = problem-> M;
soclcp->q = (double *) malloc(soclcp->n * sizeof(double));
soclcp->mu = problem->mu;
soclcp->coneIndex = (unsigned int *) malloc((soclcp->nc+1) * sizeof(unsigned int));
for (int i=0; i < soclcp->n; i++)
{
soclcp->q[i]=problem->q[i];
}
for (int i=0; i < soclcp->nc+1; i++)
{
soclcp->coneIndex[i] = 3*i;
}
if (internalsolver_options->solverId == SICONOS_SOCLCP_NSGS)
{
if (verbose == 1)
printf(" ========================== Call NSGS solver SOCLCP problem ==========================\n");
internalsolver = &soclcp_nsgs;
//internalsolver_options->internalSolvers->dWork = options->dWork;
}
else
{
fprintf(stderr, "Numerics, fc3d_ACLMFixedPoint failed. Unknown internal solver.\n");
exit(EXIT_FAILURE);
}
double normUT;
int cumul_iter =0;
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
// internal solver for the regularized problem
/* Compute the value of the initial value of q */
for (int ic = 0 ; ic < nc ; ic++)
{
normUT = sqrt(velocity[ic*3+1] * velocity[ic*3+1] + velocity[ic*3+2] * velocity[ic*3+2]);
soclcp->q[3*ic] = problem->q[3*ic] + problem->mu[ic]*normUT;
}
//secondOrderConeLinearComplementarityProblem_printInFilename(soclcp,"output.dat");
// DEBUG_EXPR(for (int ic = 0 ; ic < nc ; ic++) printf("problem->q[%i] = %le\n", 3*ic, problem->q[3*ic]);
// for (int ic = 0 ; ic < nc ; ic++) printf("q[%i] = %le\n", 3*ic, soclcp->q[3*ic]); );
if (iparam[1] == 0 )
{
internalsolver_options->dparam[0] = max(error/10.0, options->dparam[0]/problem->numberOfContacts);
}
else if (iparam[1] ==1)
{
internalsolver_options->dparam[0] = options->dparam[0]/2.0;
}
else
{
fprintf(stderr, "Numerics, fc3d_ACLMFixedPoint failed. Unknown startegy for driving tolerence of internal.\n");
exit(EXIT_FAILURE);
}
(*internalsolver)(soclcp, reaction , velocity , info , internalsolver_options);
cumul_iter += internalsolver_options->iparam[7];
/* **** Criterium convergence **** */
fc3d_compute_error(problem, reaction , velocity, tolerance, options, &error);
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env, nc * 3,
reaction, velocity, error, NULL);
}
if (verbose > 0)
printf("------------------------ FC3D - ACLMFP - Iteration %i Residual = %14.7e\n", iter, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
if (verbose > 0)
{
printf("----------------------------------- FC3D - ACLMFP - # Iteration %i Final Residual = %14.7e\n", iter, error);
printf("----------------------------------- FC3D - ACLMFP - # internal iteration = %i\n", cumul_iter);
}
free(soclcp->q);
free(soclcp->coneIndex);
free(soclcp);
if (internalsolver_options->internalSolvers != NULL)
internalsolver_options->internalSolvers->dWork = NULL;
dparam[0] = tolerance;
dparam[1] = error;
iparam[7] = iter;
}
int fc2d_driver(FrictionContactProblem* problem, double *reaction , double *velocity, SolverOptions* options, NumericsOptions* global_options)
{
#ifdef DUMP_PROBLEM
char fname[256];
sprintf(fname, "FrictionContactProblem%.5d.dat", fccounter++);
printf("Dump of FrictionContactProblem%.5d.dat", fccounter);
FILE * foutput = fopen(fname, "w");
frictionContact_printInFile(problem, foutput);
fclose(foutput);
#endif
if (options == NULL || global_options == NULL)
numericsError("fc2d_driver", "null input for solver and/or global options");
/* Set global options */
setNumericsOptions(global_options);
/* Checks inputs */
if (problem == NULL || reaction == NULL || velocity == NULL)
numericsError("fc2d_driver", "null input for FrictionContactProblem and/or unknowns (reaction,velocity)");
/* If the options for solver have not been set, read default values in .opt file */
int NoDefaultOptions = options->isSet; /* true(1) if the SolverOptions structure has been filled in else false(0) */
if (!NoDefaultOptions)
readSolverOptions(3, options);
if (verbose > 0)
printSolverOptions(options);
/* Solver name */
/*char * name = options->solverName;*/
int info = -1 ;
if (problem->dimension != 2)
numericsError("fc2d_driver", "Dimension of the problem : problem-> dimension is not compatible or is not set");
/* Non Smooth Gauss Seidel (NSGS) */
if (problem->M->storageType == 1)
{
if (options->solverId == SICONOS_FRICTION_2D_NSGS)
{
if (verbose)
printf(" ======================= Call Sparse NSGS solver for Friction-Contact 2D problem ======================\n");
fc2d_sparse_nsgs(problem, reaction , velocity , &info , options);
}
else
{
fprintf(stderr, "fc2d_driver error: unknown solver named: %s\n", idToName(options->solverId));
exit(EXIT_FAILURE);
}
}
else if (problem->M->storageType == 0)
{
switch (options->solverId)
{
/****** NLGS algorithm ******/
case SICONOS_FRICTION_2D_PGS:
case SICONOS_FRICTION_2D_NSGS:
{
if (verbose)
printf(" ========================== Call NLGS solver for Friction-Contact 2D problem ==========================\n");
fc2d_nsgs(problem, reaction, velocity, &info, options);
break;
}
/****** CPG algorithm ******/
case SICONOS_FRICTION_2D_CPG:
{
if (verbose)
printf(" ========================== Call CPG solver for Friction-Contact 2D problem ==========================\n");
fc2d_cpg(problem, reaction, velocity, &info, options);
break;
}
/****** Latin algorithm ******/
case SICONOS_FRICTION_2D_LATIN:
{
if (verbose)
printf(" ========================== Call Latin solver for Friction-Contact 2D problem ==========================\n");
fc2d_latin(problem, reaction, velocity, &info, options);
break;
}
/****** Lexicolemke algorithm ******/
case SICONOS_FRICTION_2D_LEMKE:
{
if (verbose)
printf(" ========================== Call Lemke solver for Friction-Contact 2D problem ==========================\n");
fc2d_lexicolemke(problem, reaction, velocity, &info, options, global_options);
break;
}
/****** Enum algorithm ******/
case SICONOS_FRICTION_2D_ENUM:
{
if (verbose)
printf(" ========================== Call Enumerative solver for Friction-Contact 2D problem ==========================\n");
fc2d_enum(problem, reaction, velocity, &info, options, global_options);
break;
}
/*error */
default:
{
fprintf(stderr, "fc2d_driver error: unknown solver named: %s\n", idToName(options->solverId));
exit(EXIT_FAILURE);
}
}
#ifdef DUMP_PROBLEM_IF_INFO
if (info)
{
char fname[256];
sprintf(fname, "FrictionContactProblem%.5d.dat", fccounter++);
printf("Dump of FrictionContactProblem%.5d.dat\n", fccounter);
FILE * foutput = fopen(fname, "w");
frictionContact_printInFile(problem, foutput);
fclose(foutput);
}
#endif
}
else
{
numericsError("fc2d_driver",
" error: unknown storagetype named");
exit(EXIT_FAILURE);
}
return info;
}
/* return 0 if ok
* otherwise return !=0
*/
int solveLeastSquareProblem(LinearSystemProblem* problem, double *z , SolverOptions* options)
{
/* Checks inputs */
if (problem == NULL || z == NULL)
numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)");
/* Output info. : 0: ok - >0: problem (depends on solver) */
int info = -1;
int n = problem->size;
int n2 = n * n;
double * Maux = 0;
int * ipiv = 0;
if (options && options->dWork)
Maux = options->dWork;
else
Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double));
if (options && options->iWork)
ipiv = options->iWork;
else
ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int));
int LAinfo = 0;
//displayLS(problem);
assert(problem->M->matrix0);
assert(problem->q);
memcpy(Maux, problem->M->matrix0, n2 * sizeof(double));
// memcpy(z,problem->q,n*sizeof(double));
for (int ii = 0; ii < n; ii++)
z[ii] = -problem->q[ii];
printf("LinearSystem : solveLeastSquareProblem LWORK is :%d\n", LWORK);
DGELS(LA_NOTRANS,n, n, 1, Maux, n, z, n, &LAinfo);
if (LAinfo)
{
printf("LinearSystem_driver: DGELS failed:\n");
goto __fin;
}
int ii;
for (ii = 0; ii < n; ii++)
{
if (isnan(z[ii]) || isinf(z[ii]))
{
printf("DGELS FAILED\n");
goto __fin;
}
}
info = 0;
printf("LinearSystem_driver: computeError of LinearSystem : %e\n", LinearSystem_computeError(problem, z));
__fin:
if (!(options && options->dWork))
free(Maux);
if (!(options && options->iWork))
free(ipiv);
return info;
}
void frictionContact3D_projection_update_with_regularization(int contact, FrictionContactProblem * problem, FrictionContactProblem * localproblem, double* reaction, SolverOptions* options)
{
/* Build a local problem for a specific contact
reaction corresponds to the global vector (size n) of the global problem.
*/
/* Call the update function which depends on the storage for MGlobal/MBGlobal */
/* Build a local problem for a specific contact
reaction corresponds to the global vector (size n) of the global problem.
*/
/* The part of MGlobal which corresponds to the current block is copied into MLocal */
NumericsMatrix * MGlobal = problem->M;
int n = 3 * problem->numberOfContacts;
int storageType = MGlobal->storageType;
if (storageType == 0)
// Dense storage
{
int in = 3 * contact, it = in + 1, is = it + 1;
int inc = n * in;
double * MM = MGlobal->matrix0;
double * MLocal = localproblem->M->matrix0;
/* The part of MM which corresponds to the current block is copied into MLocal */
MLocal[0] = MM[inc + in];
MLocal[1] = MM[inc + it];
MLocal[2] = MM[inc + is];
inc += n;
MLocal[3] = MM[inc + in];
MLocal[4] = MM[inc + it];
MLocal[5] = MM[inc + is];
inc += n;
MLocal[6] = MM[inc + in];
MLocal[7] = MM[inc + it];
MLocal[8] = MM[inc + is];
}
else if (storageType == 1)
{
int diagPos = getDiagonalBlockPos(MGlobal->matrix1, contact);
/* for (int i =0 ; i< 3*3 ; i++) localproblem->M->matrix0[i] = MGlobal->matrix1->block[diagPos][i] ; */
cblas_dcopy(9, MGlobal->matrix1->block[diagPos], 1, localproblem->M->matrix0 , 1);
}
else
numericsError("FrictionContact3D_projection -", "unknown storage type for matrix M");
/**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
excluding the block corresponding to the current contact. ****/
frictionContact3D_nsgs_computeqLocal(problem, localproblem, reaction, contact);
double rho = options->dparam[3];
for (int i = 0 ; i < 3 ; i++) localproblem->M->matrix0[i + 3 * i] += rho ;
double *qLocal = localproblem->q;
int in = 3 * contact, it = in + 1, is = it + 1;
/* qLocal computation*/
qLocal[0] -= rho * reaction[in];
qLocal[1] -= rho * reaction[it];
qLocal[2] -= rho * reaction[is];
/* Friction coefficient for current block*/
localproblem->mu[0] = problem->mu[contact];
}
void lcp_nsgs_SBM(LinearComplementarityProblem* 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, "lcp_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 LCP */
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::lcp_NSGS_SBM error: empty M matrix (all blocks = NULL).\n");
exit(EXIT_FAILURE);
}
/* Local problem initialization */
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("lcp_nsgs_SBM", "The NSGS_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 */
lcp_nsgs_SBM_buildLocalProblem(rowNumber, blmat, local_problem, q, z);
/* Solve local problem */
infoLocal = lcp_driver_DenseMatrix(local_problem, &z[pos], &w[pos], &internalSolvers[localSolverNum]);
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, "lcp_NSGS_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 = lcp_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 frictionContact3D_SOCLCP(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Tolerance */
double tolerance = dparam[0];
if (options->numberOfInternalSolvers < 1)
{
numericsError("frictionContact3D_SOCLCP", "The SOCLCP for FrictionContactProblem needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
}
SolverOptions * internalsolver_options = options->internalSolvers;
if (verbose > 0)
{
printf("Local solver data :");
printSolverOptions(internalsolver_options);
}
/***** Fixed Point Iterations *****/
double error = 1.; /* Current error */
soclcp_InternalSolverPtr internalsolver;
SecondOrderConeLinearComplementarityProblem* soclcp = (SecondOrderConeLinearComplementarityProblem *)malloc(sizeof(SecondOrderConeLinearComplementarityProblem));
soclcp->n = problem->numberOfContacts * problem->dimension;
soclcp->nc= problem->numberOfContacts;
soclcp->M = problem-> M;
soclcp->q = (double *) malloc(soclcp->n * sizeof(double));
soclcp->mu = problem->mu;
soclcp->coneIndex = (unsigned int *) malloc((soclcp->nc+1) * sizeof(unsigned int));
for (int i=0; i < soclcp->n; i++)
{
soclcp->q[i]=problem->q[i];
}
for (int i=0; i < soclcp->nc+1; i++)
{
soclcp->coneIndex[i] = 3*i;
}
if (internalsolver_options->solverId == SICONOS_SOCLCP_NSGS)
{
if (verbose == 1)
printf(" ========================== Call NSGS solver SOCLCP problem ==========================\n");
internalsolver = &soclcp_nsgs;
//internalsolver_options->internalSolvers->dWork = options->dWork;
}
else
{
fprintf(stderr, "Numerics, FrictionContact3D_SOCLCP failed. Unknown internal solver.\n");
exit(EXIT_FAILURE);
}
internalsolver_options->dparam[0]=options->dparam[0];
internalsolver_options->iparam[0]=options->iparam[0];
(*internalsolver)(soclcp, reaction , velocity , info , internalsolver_options);
error = internalsolver_options->dparam[1];
double real_error=0.0;
FrictionContact3D_compute_error(problem, reaction , velocity, tolerance, options, &real_error);
if (options->callback)
{
options->callback->collectStatsIteration(options->callback->env, nc * 3,
reaction, velocity, error, NULL);
}
if (verbose > 0)
{
printf("----------------------------------- FC3D - SOCLCP - # Iteration %i Final Error = %14.7e\n", internalsolver_options->iparam[7], error);
printf("----------------------------------- FC3D - SOCLCP - # error of the real problem = %14.7e\n", real_error );
printf("----------------------------------- FC3D - SOCLCP - # gap with the real problem = %14.7e\n", fabs(real_error-error) );
}
free(soclcp->q);
free(soclcp->coneIndex);
free(soclcp);
if (internalsolver_options->internalSolvers != NULL)
internalsolver_options->internalSolvers->dWork = NULL;
dparam[0] = tolerance;
dparam[1] = error;
dparam[2] = fabs(real_error-error);
iparam[7] = internalsolver_options->iparam[7];
}
void frictionContact3D_nsgs_velocity(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* int and double parameters */
int* iparam = options->iparam;
double* dparam = options->dparam;
/* Number of contacts */
int nc = problem->numberOfContacts;
NumericsMatrix* M = problem->M;
/* Dimension of the problem */
int n = 3 * nc;
/* Maximum number of iterations */
int itermax = iparam[0];
/* Tolerance */
double tolerance = dparam[0];
/* Check for trivial case */
/* *info = checkTrivialCase(n, q,velocity, reaction, options); */
if (*info == 0)
return;
SolverPtr local_solver = NULL;
FreeSolverPtr freeSolver = NULL;
ComputeErrorPtr computeError = NULL;
if (M->storageType == 0)
{
/* /\* Inversion of the matrix M *\/ */
/* int* ipiv = (int *)malloc(n*sizeof(*ipiv)); */
/* int infoDGETRF=0; */
/* DGETRF(n,n,M->matrix0,n, ipiv,infoDGETRF ); */
/* assert(!infoDGETRF); */
/* int infoDGETRI; */
/* DGETRI(n,M->matrix0,n, ipiv,infoDGETRI ); */
/* assert(!infoDGETRI); */
/* double* qtmp = (double*)malloc(n*sizeof(double)); */
/* cblas_dcopy(n, q, 1, qtmp, 1); */
/* cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, -1.0, M->matrix0 , n, qtmp,1,0.0,q, 1); */
/* free(ipiv); */
/* free(qtmp); */
double tolpinv = 1e-07;
pinv(M->matrix0, n, n, tolpinv);
}
else
{
fprintf(stderr, "Numerics, frictionContact3D_nsgs_velocity. Not yet implemented for storageType %i\n", M->storageType);
exit(EXIT_FAILURE);
}
if (options->numberOfInternalSolvers < 1)
{
numericsError("frictionContact3D_nsgs_velocity", "The NSGS method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
}
SolverOptions * localsolver_options = options->internalSolvers;
/* Connect local solver */
FrictionContactProblem* localproblem = 0;
initializeLocalSolver_nsgs_velocity(&local_solver, &freeSolver, &computeError, problem, localproblem, localsolver_options);
/***** NSGS_VELOCITY Iterations *****/
int iter = 0; /* Current iteration number */
double error = 1.; /* Current error */
int hasNotConverged = 1;
int contact; /* Number of the current row of blocks in M */
dparam[0] = dparam[2]; // set the tolerance for the local solver
while ((iter < itermax) && (hasNotConverged > 0))
{
++iter;
/* Loop through the contact points */
//cblas_dcopy( n , q , incx , velocity , incy );
for (contact = 0 ; contact < nc ; ++contact)
{
(*local_solver)(localproblem, velocity, localsolver_options);
for (int ncc = 0; ncc < 3; ncc ++)
{
printf("velocity[%i]=%14.7e\t", ncc, velocity[contact * 3 + ncc]);
}
printf("\n");
}
/* **** Criterium convergence **** */
(*computeError)(problem, reaction , velocity, tolerance, options, &error);
if (verbose > 0)
printf("----------------------------------- FC3D - NSGS_VELOCITY - Iteration %i Error = %14.7e\n", iter, error);
if (error < tolerance) hasNotConverged = 0;
*info = hasNotConverged;
}
printf("----------------------------------- FC3D - NSGS_VELOCITY - # Iteration %i Final Error = %14.7e\n", iter, error);
dparam[0] = tolerance;
dparam[1] = error;
iparam[7] = iter;
/***** Free memory *****/
(*freeSolver)();
}
