static
void acceptLocalReactionProjected(FrictionContactProblem *problem,
FrictionContactProblem *localproblem,
SolverPtr local_solver,
SolverOptions *localsolver_options,
unsigned int contact, unsigned int iter,
double *reaction, double localreaction[3])
{
int nan1 = isnan(localsolver_options->dparam[1]) || isinf(localsolver_options->dparam[1]);
if (nan1 || localsolver_options->dparam[1] > 1.0)
{
DEBUG_EXPR(
frictionContact_display(localproblem);
// In case of bad solution, re-solve with a projection-on-cone solver
DEBUG_PRINTF("Discard local reaction for contact %i at iteration %i with local_error = %e\n", contact, iter, localsolver_options->dparam[1]);
memcpy(localreaction, &reaction[contact*3], sizeof(double)*3);
fc3d_projectionOnConeWithLocalIteration_initialize(problem, localproblem, localsolver_options );
fc3d_projectionOnConeWithLocalIteration_solve(localproblem, localreaction, localsolver_options);
int nan2 = isnan(localsolver_options->dparam[1]) || isinf(localsolver_options->dparam[1]);
if (nan2) {
DEBUG_PRINTF("No hope for contact %d, setting to zero.\n", contact);
reaction[3*contact+0] = 0;
reaction[3*contact+1] = 0;
reaction[3*contact+2] = 0;
return;
}
// Save the result in case next step fails
double localreaction_proj[3];
double error_proj = localsolver_options->dparam[1];
memcpy(localreaction_proj, localreaction, sizeof(double)*3);
// Complete it further with the original solver
DEBUG_PRINTF("Try local fc3d_projectionOnConeWithLocalIteration_solve with local_error = %e\n", localsolver_options->dparam[1]);
(*local_solver)(localproblem, localreaction , localsolver_options);
int nan3 = isnan(localsolver_options->dparam[1]) || isinf(localsolver_options->dparam[1]);
DEBUG_PRINTF("Try local another newton solve with local_error = %e\n", localsolver_options->dparam[1]);
// If we produced a bad value doing this, keep the projectionOnCone solution
// (Note: this has been observed, particularly when mu=1.0)
if (nan3)
{
DEBUG_PRINTF("Keep the projectionOnCone local solution = %e\n", error_proj);
memcpy(&reaction[contact*3], localreaction_proj, sizeof(double)*3);
}
else
{
if (nan1 || localsolver_options->dparam[1] <= localsolver_options->dparam[0]
|| localsolver_options->dparam[1] <= error_prev)
{
DEBUG_PRINTF("Keep the new local solution = %e\n", localsolver_options->dparam[1]);
memcpy(&reaction[contact*3], localreaction, sizeof(double)*3);
//getchar();
}
else
{
DEBUG_PRINTF("Keep the previous local solution = %e\n", error_prev);
}
}
);
int main(void)
{
VariationalInequality vi;
variationalInequality_clear(&vi);
//vi.env = &vi;
vi.F = &Ftest;
vi.ProjectionOnX = &PXtest;
vi.normVI = 0.0;
vi.istheNormVIset = 0;
vi.set = NULL;
vi.nabla_F = NULL;
NumericsOptions global_options;
setDefaultNumericsOptions(&global_options);
global_options.verboseMode = 1; // turn verbose mode to off by default
SolverOptions * options = (SolverOptions *) malloc(sizeof(SolverOptions));
int info = variationalInequality_setDefaultSolverOptions(options, SICONOS_VI_FPP);
options->dparam[0]=1e-8;
FILE * finput = fopen("./data/Example1_Fc3D_SBM.dat", "r");
FrictionContactProblem* problem = (FrictionContactProblem *)malloc(sizeof(FrictionContactProblem));
info = frictionContact_newFromFile(problem, finput);
// frictionContact_display(problem);
Problems *pb= (Problems *)malloc(sizeof(Problems));
vi.env = pb;
pb->vi = &vi;
pb->fc3d = problem;
frictionContact_display(pb->fc3d);
int n = problem->numberOfContacts * problem->dimension;
vi.size=n;
double *x = (double*)calloc(n, sizeof(double));
double *w = (double*)calloc(n, sizeof(double));
PXtest(&vi, x,w);
info = variationalInequality_driver(&vi,
x,
w,
options,
&global_options);
int i =0;
for (i =0; i< n ; i++)
{
printf("x[%i]=%f\t",i,x[i]); printf("w[%i]=F[%i]=%f\n",i,i,w[i]);
}
deleteSolverOptions(options);
free(options);
free(problem);
free(x);
free(w);
return info;
}
static
void acceptLocalReactionFiltered(FrictionContactProblem *localproblem,
SolverOptions *localsolver_options,
unsigned int contact, unsigned int iter,
double *reaction, double localreaction[3])
{
if (isnan(localsolver_options->dparam[1])
|| isinf(localsolver_options->dparam[1])
|| localsolver_options->dparam[1] > 1.0)
{
DEBUG_EXPR(frictionContact_display(localproblem));
DEBUG_PRINTF("Discard local reaction for contact %i at iteration %i "
"with local_error = %e\n",
contact, iter, localsolver_options->dparam[1]);
#ifdef FCLIB_OUTPUT
/* printf("step counter value = %i\n", localsolver_options->iparam[19]); */
char fname[256];
fccounter ++;
sprintf(fname, "./local_problem/localproblem_%i_%i.hdf5", contact, localsolver_options->iparam[19]);
if (file_exists(fname))
{
/* printf(" %s already dumped\n", fname); */
}
else
{
printf("Dump %s\n", fname);
int n = 100;
char * title = (char *)malloc(n * sizeof(char));
strcpy(title, "Bad local problem dump in hdf5");
char * description = (char *)malloc(n * sizeof(char));
strcpy(description, "Rewriting in hdf5 from siconos ");
strcat(description, fname);
strcat(description, " in FCLIB format");
char * mathInfo = (char *)malloc(n * sizeof(char));
strcpy(mathInfo, "unknown");
frictionContact_fclib_write(localproblem,
title,
description,
mathInfo,
fname,3);
printf("end of dump %s\n", fname);
free(title);
free(description);
free(mathInfo);
}
#endif
if (verbose > 1)
printf("Discard local reaction for contact %i at iteration %i "
"with local_error = %e\n",
contact, iter, localsolver_options->dparam[1]);
}
else
memcpy(&reaction[contact*3], localreaction, sizeof(double)*3);
}
