int relay_test_function(FILE * f, int solverId)
{
int i, info = 0 ;
RelayProblem* problem = (RelayProblem *)malloc(sizeof(RelayProblem));
info = relay_newFromFile(problem, f);
FILE * foutput = fopen("./relay.verif", "w");
info = relay_printInFile(problem, foutput);
SolverOptions * options = (SolverOptions *)malloc(sizeof(SolverOptions));
relay_setDefaultSolverOptions(problem, options, solverId);
int maxIter = 50000;
double tolerance = 1e-8;
options->iparam[0] = maxIter;
options->dparam[0] = tolerance;
double * z = (double *)calloc(problem->size, sizeof(double));
double * w = (double *)calloc(problem->size, sizeof(double));
info = relay_driver(problem, z , w, options);
for (i = 0 ; i < problem->size ; i++)
{
printf("z[%i] = %12.8e\t,w[%i] = %12.8e\n", i, z[i], i, w[i]);
}
if (!info)
{
printf("test succeeded\n");
}
else
{
printf("test unsuccessful\n");
}
free(z);
free(w);
solver_options_delete(options);
free(options);
freeRelay_problem(problem);
fclose(foutput);
return info;
}
int main(void)
{
int info = 0 ;
char filename[50] = "./data/LMGC_GFC3D_CubeH8.hdf5";
printf("Test on %s\n", filename);
SolverOptions * options = (SolverOptions *) malloc(sizeof(SolverOptions));
info = gfc3d_setDefaultSolverOptions(options, SICONOS_GLOBAL_FRICTION_3D_NSGS);
options->dparam[0] = 1e-08;
options->iparam[0] = 100000;
info = gfc3d_test_function_hdf5(filename, options);
solver_options_delete(options);
free(options);
printf("\nEnd of test on %s\n",filename);
return info;
}
int main(void)
{
printf(" Start tests for MCP solvers.\n");
int info = 0 ;
/* Set solver options */
SolverOptions options;
/* FB solver */
options.solverId = SICONOS_MCP_FB;
/* Create a MixedComplementarityProblem */
MixedComplementarityProblem* problem = (MixedComplementarityProblem *)malloc(sizeof(MixedComplementarityProblem));
problem->sizeEqualities = 2;
problem->sizeInequalities = 3;
problem->computeFmcp = &testF ;
problem->computeNablaFmcp = &testNablaF ;
problem->Fmcp = NULL;
problem->nablaFmcp = NULL;
mixedComplementarity_setDefaultSolverOptions(problem, &options);
int size = 5;
double z[4];
double F[4];
double nablaF[16];
problem->computeFmcp(size, z, F);
problem->computeNablaFmcp(size, z, nablaF);
/* Initialize the solver */
mcp_driver_init(problem, &options) ;
/// TODO : write a real test ... ////
printf("End of MCP solvers test. \n");
mcp_driver_reset(problem, &options);
freeMixedComplementarityProblem(problem);
solver_options_delete(&options);
return info;
}
int main(void)
{
printf("\n Start of test on Default SolverOptions\n");
int info = 0 ;
SolverOptions * options = (SolverOptions *)malloc(sizeof(SolverOptions));
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_NSGS);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_NSGSV);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_PROX);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_TFP);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_DSFP);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_EG);
solver_options_print(options);
solver_options_delete(options);
info = fc3d_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_HP);
solver_options_print(options);
solver_options_delete(options);
free(options);
printf("\n End of test on Default SolverOptions\n");
return info;
}
int main(void)
{
int info = 0 ;
FILE * finput = 0;
SolverOptions * options = (SolverOptions *) malloc(sizeof(SolverOptions));
gmp_setDefaultSolverOptions(options, SICONOS_FRICTION_3D_ONECONTACT_QUARTIC);
options->iparam[0] = 100000000;
//options->iparam[2]=1;
printf("Test on ./data/GMP.dat\n");
finput = fopen("./data/GMP.dat", "r");
info = genericMechanical_test_function(finput, options);
fclose(finput);
solver_options_delete(options);
free(options);
fclose(finput);
printf("\nEnd of test on ./data/GMP.dat\n");
return info;
}
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 main(int argc, char* argv[])
{
// Problem Definition
int info = -1;
int NC = 1;//Number of contacts
int Ndof = 9;//Number of DOF
// Problem Definition
double M11[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
double M22[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
double M33[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // Warning Fortran Storage
/* double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
double H00[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
double H20[9] = { -1, 0, 0, 0, -1, 0, 0, 0, -1};
/* double H[27] = {1, 0, 0, 0, 0, 0, -1, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, -1, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, -1}; */
double q[9] = { -3, -3, -3, -1, 1, 3, -1, 1, 3};
double b[3] = {0, 0, 0};
double mu[1] = {0.1};
/* DSCAL(9,-1.0,q,1); */
/* int NC = 3;//Number of contacts */
/* int Ndof = 9;//Number of DOF */
/* double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double H[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double q[9] = {-1, 1, 3, -1, 1, 3, -1, 1, 3}; */
/* double b[9] = {0, 0, 0,0, 0, 0,0, 0, 0 }; */
/* double mu[3] = {0.1,0.1,0.1}; */
int k;
int m = 3 * NC;
int n = Ndof;
GlobalFrictionContactProblem numericsProblem;
globalFrictionContact_null(&numericsProblem);
numericsProblem.numberOfContacts = NC;
numericsProblem.dimension = 3;
numericsProblem.mu = mu;
numericsProblem.q = q;
numericsProblem.b = b;
numericsProblem.M = newNumericsMatrix();
NumericsMatrix *MM = numericsProblem.M;
MM->storageType = NM_SPARSE_BLOCK;
MM->size0 = Ndof;
MM->size1 = Ndof;
MM->matrix1 = newSBM();
SparseBlockStructuredMatrix *MBlockMatrix = MM->matrix1;
MBlockMatrix->nbblocks = 3;
double * block[3] = {M11, M22, M33};
MBlockMatrix->block = block;
MBlockMatrix->blocknumber0 = 3;
MBlockMatrix->blocknumber1 = 3;
unsigned int blocksize[3] = {3, 6, 9} ;
MBlockMatrix->blocksize0 = blocksize;
MBlockMatrix->blocksize1 = blocksize;
MBlockMatrix->filled1 = 4;
MBlockMatrix->filled2 = 3;
size_t index1_data[4] = {0, 1, 2, 3} ;
size_t index2_data[3] = {0, 1, 2} ;
MBlockMatrix->index1_data = index1_data;
MBlockMatrix->index2_data = index2_data;
numericsProblem.H = newNumericsMatrix();
NumericsMatrix *HH = numericsProblem.H;
HH->storageType = 1;
HH->size0 = Ndof;
HH->size1 = 3 * NC;
HH->matrix1 = (SparseBlockStructuredMatrix*)malloc(sizeof(SparseBlockStructuredMatrix));
SparseBlockStructuredMatrix *HBlockMatrix = HH->matrix1;
HBlockMatrix->nbblocks = 2;
double * hblock[3] = {H00, H20};
HBlockMatrix->block = hblock;
HBlockMatrix->blocknumber0 = 3;
HBlockMatrix->blocknumber1 = 1;
unsigned int blocksize0[3] = {3, 6, 9} ;
unsigned int blocksize1[1] = {3} ;
HBlockMatrix->blocksize0 = blocksize0;
HBlockMatrix->blocksize1 = blocksize1;
HBlockMatrix->filled1 = 4;
HBlockMatrix->filled2 = 2;
size_t hindex1_data[4] = {0, 1, 1, 2} ;
size_t hindex2_data[3] = {0, 0} ;
HBlockMatrix->index1_data = hindex1_data;
HBlockMatrix->index2_data = hindex2_data;
FILE * foutput = fopen("Example_GlobalFrictionContact_SBM.dat", "w");
globalFrictionContact_printInFile(&numericsProblem, foutput);
fclose(foutput);
// Unknown Declaration
double *reaction = (double*)calloc(m, sizeof(double));
double *velocity = (double*)calloc(m, sizeof(double));
double *globalVelocity = (double*)calloc(n, sizeof(double));
// Solver Options
SolverOptions * numerics_solver_options = (SolverOptions *)malloc(sizeof(SolverOptions));
// char solvername[10]= "NSGS";
/*\warning Must be adpated for future globalFrictionContact3D_setDefaultSolverOptions*/
gfc3d_setDefaultSolverOptions(numerics_solver_options, SICONOS_GLOBAL_FRICTION_3D_NSGS);
numerics_solver_options->dparam[0] = 1e-14;
numerics_solver_options->iparam[0] = 100000;
//Driver call
info = gfc3d_driver(&numericsProblem,
reaction , velocity, globalVelocity,
numerics_solver_options);
solver_options_delete(numerics_solver_options);
free(numerics_solver_options);
// Solver output
printf("\n");
for (k = 0 ; k < m; k++) printf("velocity[%i] = %12.8e \t \t reaction[%i] = %12.8e \n ", k, velocity[k], k , reaction[k]);
for (k = 0 ; k < n; k++) printf("globalVelocity[%i] = %12.8e \t \n ", k, globalVelocity[k]);
printf("\n");
free(reaction);
free(velocity);
free(globalVelocity);
// freeSBM(MM->matrix1);
// freeSBM(HH->matrix1);
free(MM->matrix1);
MM->matrix1 = NULL;
free(HH->matrix1);
HH->matrix1 = NULL;
freeNumericsMatrix(MM);
freeNumericsMatrix(HH);
free(MM);
free(HH);
gfc3d_free_workspace(&numericsProblem);
/* while (1) sleep(60); */
return info;
}
int main(void)
{
printf("\n Start of test on Default SolverOptions\n");
int info = 0 ;
SolverOptions * options = (SolverOptions *)malloc(sizeof(SolverOptions));
FILE * finput = fopen("./data/lcp_mmc.dat", "r");
LinearComplementarityProblem* problem = (LinearComplementarityProblem *)malloc(sizeof(LinearComplementarityProblem));
info = linearComplementarity_newFromFile(problem, finput);
fclose(finput);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_NSGS_SBM);
assert(options->internalSolvers);
solver_options_set(options->internalSolvers, SICONOS_LCP_LEMKE);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_PGS);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_RPGS);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_QP);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_NSQP);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_CPG);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_PSOR);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_LATIN);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_LATIN_W);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_LEMKE);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_PATH);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_ENUM);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_NEWTONMIN);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_AVI_CAOFERRIS);
solver_options_print(options);
solver_options_delete(options);
info = linearComplementarity_setDefaultSolverOptions(problem, options, SICONOS_LCP_PIVOT);
solver_options_print(options);
solver_options_delete(options);
freeLinearComplementarityProblem(problem);
free(options);
printf("\n End of test on Default SolverOptions\n");
return info;
}
void ncp_pathsearch(NonlinearComplementarityProblem* problem, double* z, double* F, int *info , SolverOptions* options)
{
/* Main step of the algorithm:
* - compute jacobians
* - call modified lemke
*/
unsigned int n = problem->n;
unsigned int preAlloc = options->iparam[SICONOS_IPARAM_PREALLOC];
int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
double merit_norm = 1.0;
double nn_tol = options->dparam[SICONOS_DPARAM_TOL];
int nbiter = 0;
/* declare a LinearComplementarityProblem on the stack*/
LinearComplementarityProblem lcp_subproblem;
lcp_subproblem.size = n;
/* do some allocation if required
* - nabla_F (used also as M for the LCP subproblem)
* - q for the LCP subproblem
*
* Then fill the LCP subproblem
*/
if (!preAlloc || (preAlloc && !options->internalSolvers))
{
options->internalSolvers = (SolverOptions *) malloc(sizeof(SolverOptions));
solver_options_set(options->internalSolvers, SICONOS_LCP_PIVOT);
options->numberOfInternalSolvers = 1;
SolverOptions * lcp_options = options->internalSolvers;
/* We always allocation once and for all since we are supposed to solve
* many LCPs */
lcp_options->iparam[SICONOS_IPARAM_PREALLOC] = 1;
/* set the right pivot rule */
lcp_options->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
/* set the right stacksize */
lcp_options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE] = options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE];
}
assert(problem->nabla_F);
lcp_subproblem.M = problem->nabla_F;
if (!preAlloc || (preAlloc && !options->dWork))
{
options->dWork = (double *) malloc(4*n*sizeof(double));
}
lcp_subproblem.q = options->dWork;
double* x = &options->dWork[n];
double* x_plus = &options->dWork[2*n];
double* r = &options->dWork[3*n];
NMS_data* data_NMS;
functions_LSA* functions;
if (!preAlloc || (preAlloc && !options->solverData))
{
options->solverData = malloc(sizeof(pathsearch_data));
pathsearch_data* solverData = (pathsearch_data*) options->solverData;
/* do all the allocation */
solverData->data_NMS = create_NMS_data(n, NM_DENSE, options->iparam, options->dparam);
solverData->lsa_functions = (functions_LSA*) malloc(sizeof(functions_LSA));
solverData->data_NMS->set = malloc(sizeof(positive_orthant));
data_NMS = solverData->data_NMS;
functions = solverData->lsa_functions;
/* for use in NMS; only those 3 functions are called */
init_lsa_functions(functions, &FB_compute_F_ncp, &ncp_FB);
functions->compute_H = &FB_compute_H_ncp;
set_set_id(data_NMS->set, SICONOS_SET_POSITIVE_ORTHANT);
/* fill ls_data */
data_NMS->ls_data->compute_F = functions->compute_F;
data_NMS->ls_data->compute_F_merit = functions->compute_F_merit;
data_NMS->ls_data->z = NULL; /* XXX to check -- xhub */
data_NMS->ls_data->zc = NMS_get_generic_workV(data_NMS->workspace, n);
data_NMS->ls_data->F = NMS_get_F(data_NMS->workspace, n);
data_NMS->ls_data->F_merit = NMS_get_F_merit(data_NMS->workspace, n);
data_NMS->ls_data->desc_dir = NMS_get_dir(data_NMS->workspace, n);
/** \todo this value should be settable by the user with a default value*/
data_NMS->ls_data->alpha_min = fmin(data_NMS->alpha_min_watchdog, data_NMS->alpha_min_pgrad);
data_NMS->ls_data->data = (void*)problem;
data_NMS->ls_data->set = data_NMS->set;
data_NMS->ls_data->sigma = options->dparam[SICONOS_DPARAM_NMS_SIGMA];
/* data_NMS->ls_data->searchtype is set in the NMS code */
}
else
{
pathsearch_data* solverData = (pathsearch_data*) options->solverData;
data_NMS = solverData->data_NMS;
functions = solverData->lsa_functions;
}
/* initial value for ref_merit */
problem->compute_F(problem->env, n, z, F);
functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);
data_NMS->ref_merit = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);
data_NMS->merit_bestpoint = data_NMS->ref_merit;
cblas_dcopy(n, z, 1, NMS_checkpoint_0(data_NMS, n), 1);
cblas_dcopy(n, z, 1, NMS_checkpoint_T(data_NMS, n), 1);
cblas_dcopy(n, z, 1, NMS_bestpoint(data_NMS, n), 1);
/* -------------------- end init ---------------------------*/
int nms_failed = 0;
double err = 10*nn_tol;
/* to check the solution */
LinearComplementarityProblem lcp_subproblem_check;
int check_lcp_solution = 1; /* XXX add config for that */
double normal_norm2_newton_point;
/* F is already computed here at z */
while ((err > nn_tol) && (nbiter < itermax) && !nms_failed)
{
int force_watchdog_step = 0;
int force_d_step_merit_check = 0;
double check_ratio = 0.0;
nbiter++;
/* update M, q and r */
/* First find x */
ncp_pathsearch_compute_x_from_z(n, z, F, x);
pos_part(n, x, x_plus); /* update x_plus */
ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);
if (check_lcp_solution)
{
lcp_subproblem_check.size = n;
lcp_subproblem_check.M = problem->nabla_F;
lcp_subproblem_check.q = lcp_subproblem.q;
//cblas_dcopy(n, x, 1, lcp_subproblem_check.q , 1);
//prodNumericsMatrix(n, n, -1.0, problem->nabla_F, x_plus, 0.0, lcp_subproblem.q);
}
double norm_r2 = cblas_ddot(n, r, 1, r, 1);
if (norm_r2 < DBL_EPSILON*DBL_EPSILON) /* ||r|| < 1e-15 */
{
DEBUG_PRINTF("ncp_pathsearch :: ||r|| = %e < %e; path search procedure was successful!\n", norm_r2, DBL_EPSILON*DBL_EPSILON);
(*info) = 0;
ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
break;
}
/* end update M, q and r */
lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);
switch (*info)
{
case LCP_PIVOT_SUCCESS:
DEBUG_PRINT("ncp_pathsearch :: path search procedure was successful!\n");
if (check_lcp_solution)
{
double err_lcp = 0.0;
cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
//assert(err_lcp < local_tol && "ncp_pathsearch :: lcp solved with very bad precision");
if (err_lcp > local_tol)
{
printf("ncp_pathsearch :: lcp solved with very bad precision\n");
NM_display(lcp_subproblem.M);
printf("z r q x_plus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
printf("ncp_pathsearch :: lcp resolved with error = %e; local_tol = %e\n", err_lcp, local_tol);
}
/* XXX missing recompute x ?*/
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
//check_ratio = norm_r2/normal_norm2_newton_point;
}
if (50*normal_norm2_newton_point < norm_r2)
{
force_d_step_merit_check = 1;
}
else if (10*normal_norm2_newton_point < norm_r2)
{
// check_ratio = sqrt(norm_r2/normal_norm2_newton_point);
}
}
break;
case LCP_PIVOT_RAY_TERMINATION:
DEBUG_PRINT("ncp_pathsearch :: ray termination, let's fastened your seat belt!\n");
break;
case LCP_PATHSEARCH_LEAVING_T:
DEBUG_PRINT("ncp_pathsearch :: leaving t, fastened your seat belt!\n");
DEBUG_PRINTF("ncp_pathsearch :: max t value = %e\n", options->internalSolvers->dparam[2]); /* XXX fix 2 */
/* try to retry solving the problem */
/* XXX keep or not ? */
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
check_ratio = 5.0*norm_r2/normal_norm2_newton_point;
}
if (options->internalSolvers->dparam[2] > 1e-5) break;
memset(x_plus, 0, sizeof(double) * n);
problem->compute_F(problem->env, n, x_plus, r);
ncp_pathsearch_compute_x_from_z(n, x_plus, r, x);
ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);
lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);
if (*info == LCP_PIVOT_SUCCESS)
{
DEBUG_PRINT("ncp_pathsearch :: Lemke start worked !\n");
double err_lcp = 0.0;
cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
assert(err_lcp < local_tol);
}
else
{
NM_display(lcp_subproblem.M);
printf("z r q x_plus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
DEBUG_PRINT("ncp_pathsearch :: Lemke start did not succeeded !\n");
lcp_pivot_diagnose_info(*info);
if (*info == LCP_PATHSEARCH_LEAVING_T)
{
DEBUG_PRINTF("ncp_pathsearch :: max t value after Lemke start = %e\n", options->internalSolvers->dparam[2]);
}
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
double err_lcp = 0.0;
lcp_compute_error(&lcp_subproblem, x_plus, x, 1e-14, &err_lcp);
printf("ncp_pathsearch :: lemke start resolved with info = %d; error = %e\n", *info, err_lcp);
printf("x_plus x_minus\n");
for (unsigned i = 0; i < n; ++i) printf("%e %e\n", x_plus[i], x[i]);
/* recompute the normal norm */
problem->compute_F(problem->env, n, x_plus, r);
cblas_daxpy(n, -1.0, x, 1, r, 1);
double normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
if (normal_norm2_newton_point > norm_r2)
{
printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
//assert(normal_norm2_newton_point <= norm_r2);
}
else
{
printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %.*e < %.*e\n", DECIMAL_DIG, normal_norm2_newton_point, DECIMAL_DIG, norm_r2);
}
if (100*normal_norm2_newton_point < norm_r2)
{
force_d_step_merit_check = 1;
}
}
break;
case LCP_PIVOT_NUL:
printf("ncp_pathsearch :: kaboom, kaboom still more work needs to be done\n");
lcp_pivot_diagnose_info(*info);
// exit(EXIT_FAILURE);
force_watchdog_step = 1;
break;
case LCP_PATHSEARCH_NON_ENTERING_T:
DEBUG_PRINT("ncp_pathsearch :: non entering t, something is wrong here. Fix the f****** code!\n");
assert(0 && "ncp_pathsearch :: non entering t, something is wrong here\n");
force_watchdog_step = 1;
break;
default:
printf("ncp_pathsearch :: unknown code returned by the path search\n");
exit(EXIT_FAILURE);
}
nms_failed = NMS(data_NMS, problem, functions, z, x_plus, force_watchdog_step, force_d_step_merit_check, check_ratio);
/* at this point z has been updated */
/* recompute the normal norm */
problem->compute_F(problem->env, n, z, F);
functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);
/* XXX is this correct ? */
merit_norm = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);
ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
DEBUG_PRINTF("ncp_pathsearch :: iter = %d, ncp_error = %e; merit_norm^2 = %e\n", nbiter, err, merit_norm);
}
options->iparam[1] = nbiter;
options->dparam[1] = err;
if (nbiter == itermax)
{
*info = 1;
}
else if (nms_failed)
{
*info = 2;
}
else
{
*info = 0;
}
DEBUG_PRINTF("ncp_pathsearch procedure finished :: info = %d; iter = %d; ncp_error = %e; merit_norm^2 = %e\n", *info, nbiter, err, merit_norm);
if (!preAlloc)
{
freeNumericsMatrix(problem->nabla_F);
free(problem->nabla_F);
problem->nabla_F = NULL;
free(options->dWork);
options->dWork = NULL;
solver_options_delete(options->internalSolvers);
free(options->internalSolvers);
options->internalSolvers = NULL;
free_NMS_data(data_NMS);
free(functions);
free(options->solverData);
options->solverData = NULL;
}
}
int main(int argc, char* argv[])
{
// Problem Definition
int info = -1;
int NC = 1;//Number of contacts
int m = 3;
int n = 9;
double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1
};
double H[27] = {1, 0, 0, 0, 0, 0, -1, 0, 0,
0, 1, 0, 0, 0, 0, 0, -1, 0,
0, 0, 1, 0, 0, 0, 0, 0, -1
};
double q[9] = { -3, -3, -3, -1, 1, 3, -1, 1, 3};
double b[3] = {0, 0, 0};
double mu[1] = {0.1};
/* int NC = 3;//Number of contacts */
/* int Ndof = 9;//Number of DOF */
/* double M[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double H[81] = {1, 0, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 1, 0, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 1, 0, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 1, 0, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 1, 0, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 1, 0, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 1, 0, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 1, 0, */
/* 0, 0, 0, 0, 0, 0, 0, 0, 1}; */
/* double q[9] = {-1, 1, 3, -1, 1, 3, -1, 1, 3}; */
/* double b[9] = {0, 0, 0,0, 0, 0,0, 0, 0 }; */
/* double mu[3] = {0.1,0.1,0.1}; */
int i = 0, k = 0;
GlobalFrictionContactProblem numericsProblem;
globalFrictionContact_null(&numericsProblem);
numericsProblem.numberOfContacts = NC;
numericsProblem.dimension = 3;
numericsProblem.mu = mu;
numericsProblem.q = q;
numericsProblem.b = b;
numericsProblem.M = newNumericsMatrix();
NumericsMatrix *MM = numericsProblem.M ;
MM->storageType = NM_DENSE;
MM->matrix0 = M;
MM->size0 = n;
MM->size1 = n;
numericsProblem.H = newNumericsMatrix();
NumericsMatrix *HH = numericsProblem.H;
HH->storageType = NM_DENSE;
HH->matrix0 = H;
HH->size0 = n;
HH->size1 = m;
/* FILE * foutput = fopen("Example_GlobalFrictionContact.dat", "w"); */
/* globalFrictionContact_printInFile(&numericsProblem, foutput ); */
/* fclose(foutput); */
// Unknown Declaration
double *reaction = (double*)calloc(m, sizeof(double));
double *velocity = (double*)calloc(m, sizeof(double));
double *globalVelocity = (double*)calloc(n, sizeof(double));
// Solver Options
SolverOptions * numerics_solver_options = (SolverOptions *)malloc(sizeof(SolverOptions)) ;
//char solvername[10]= "NSGS";
/*\warning Must be adpated for future globalFrictionContact3D_setDefaultSolverOptions*/
gfc3d_setDefaultSolverOptions(numerics_solver_options, SICONOS_GLOBAL_FRICTION_3D_NSGS);
numerics_solver_options->dparam[0] = 1e-14;
numerics_solver_options->iparam[0] = 100000;
//Driver call
i = 0;
info = gfc3d_driver(&numericsProblem,
reaction , velocity, globalVelocity,
numerics_solver_options);
solver_options_delete(numerics_solver_options);
// Solver output
printf("\n");
for (k = 0 ; k < m; k++) printf("velocity[%i] = %12.8e \t \t reaction[%i] = %12.8e \n ", k, velocity[k], k , reaction[k]);
for (k = 0 ; k < n; k++) printf("globalVelocity[%i] = %12.8e \t \n ", k, globalVelocity[k]);
printf("\n");
free(numerics_solver_options);
free(reaction);
free(velocity);
free(globalVelocity);
free(numericsProblem.M);
free(numericsProblem.H);
gfc3d_free_workspace(&numericsProblem);
return info;
}
void fc3d_nonsmooth_Newton_AlartCurnier2(
FrictionContactProblem* problem,
double *reaction,
double *velocity,
int *info,
SolverOptions *options)
{
assert(problem);
assert(reaction);
assert(velocity);
assert(info);
assert(options);
assert(problem->dimension == 3);
assert(options->iparam);
assert(options->dparam);
assert(problem->q);
assert(problem->mu);
assert(problem->M);
assert(!options->iparam[4]); // only host
AlartCurnierParams acparams;
switch (options->iparam[10])
{
case 0:
{
acparams.computeACFun3x3 = &computeAlartCurnierSTD;
break;
}
case 1:
{
acparams.computeACFun3x3 = &computeAlartCurnierJeanMoreau;
break;
};
case 2:
{
acparams.computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
break;
}
case 3:
{
acparams.computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
break;
}
}
fc3d_nonsmooth_Newton_solvers equation;
equation.problem = problem;
equation.data = (void *) &acparams;
equation.function = &nonsmoothEqnAlartCurnierFun;
/*************************************************************************
* START NEW STUFF
*/
size_t problemSize = problem->M->size0;
size_t _3problemSize = problemSize + problemSize + problemSize;
FC3D_Newton_data opaque_data;
opaque_data.problem = problem;
opaque_data.equation = &equation;
opaque_data.rho = (double*)calloc(problemSize, sizeof(double));
for (size_t i = 0; i < problemSize; ++i) opaque_data.rho[i] = 1.;
opaque_data.Ax = (double*)calloc(_3problemSize, sizeof(double));
opaque_data.Bx = (double*)calloc(_3problemSize, sizeof(double));
opaque_data.normq = cblas_dnrm2(problemSize, problem->q, 1);
opaque_data.AwpB_data_computed = false;
functions_LSA functions_AC;
init_lsa_functions(&functions_AC, &FC3D_compute_F, &FC3D_compute_F_merit);
functions_AC.compute_H = &FC3D_compute_AWpB;
functions_AC.compute_error = &FC3D_compute_error;
functions_AC.get_set_from_problem_data = NULL;
set_lsa_params_data(options, problem->M);
newton_LSA_param* params = (newton_LSA_param*) options->solverParameters;
params->check_dir_quality = false;
options->iparam[SICONOS_IPARAM_LSA_SEARCH_CRITERION] = SICONOS_LSA_GOLDSTEIN;
// options->iparam[SICONOS_IPARAM_LSA_SEARCH_CRITERION] = SICONOS_LSA_ARMIJO;
/*************************************************************************
* END NEW STUFF
*/
if(options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] == SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_VI_EG_NSN)
{
SolverOptions * options_vi_eg =(SolverOptions *)malloc(sizeof(SolverOptions));
fc3d_VI_ExtraGradient_setDefaultSolverOptions(options_vi_eg);
options_vi_eg->iparam[0] = 50;
options_vi_eg->dparam[0] = sqrt(options->dparam[0]);
options_vi_eg->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] = SICONOS_VI_ERROR_EVALUATION_LIGHT;
fc3d_VI_ExtraGradient(problem, reaction , velocity , info , options_vi_eg);
solver_options_delete(options_vi_eg);
free(options_vi_eg);
newton_LSA(problemSize, reaction, velocity, info, (void *)&opaque_data, options, &functions_AC);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] == SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_NO)
{
newton_LSA(problemSize, reaction, velocity, info, (void *)&opaque_data, options, &functions_AC);
}
else
{
numerics_error("fc3d_nonsmooth_Newton_AlartCurnier","Unknown nsn hybrid solver");
}
free(opaque_data.rho);
free(opaque_data.Ax);
free(opaque_data.Bx);
}
void fc3d_nonsmooth_Newton_AlartCurnier(
FrictionContactProblem* problem,
double *reaction,
double *velocity,
int *info,
SolverOptions *options)
{
/* verbose=1; */
assert(problem);
assert(reaction);
assert(velocity);
assert(info);
assert(options);
assert(problem->dimension == 3);
assert(options->iparam);
assert(options->dparam);
assert(problem->q);
assert(problem->mu);
assert(problem->M);
assert(!options->iparam[4]); // only host
AlartCurnierParams acparams;
switch (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION])
{
case 0:
{
acparams.computeACFun3x3 = &computeAlartCurnierSTD;
break;
}
case 1:
{
acparams.computeACFun3x3 = &computeAlartCurnierJeanMoreau;
break;
};
case 2:
{
acparams.computeACFun3x3 = &fc3d_AlartCurnierFunctionGenerated;
break;
}
case 3:
{
acparams.computeACFun3x3 = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;
break;
}
}
fc3d_nonsmooth_Newton_solvers equation;
equation.problem = problem;
equation.data = (void *) &acparams;
equation.function = &nonsmoothEqnAlartCurnierFun;
if(options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] == SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_VI_EG_NSN)
{
SolverOptions * options_vi_eg =(SolverOptions *)malloc(sizeof(SolverOptions));
fc3d_VI_ExtraGradient_setDefaultSolverOptions(options_vi_eg);
options_vi_eg->iparam[SICONOS_IPARAM_MAX_ITER] = 50;
options_vi_eg->dparam[SICONOS_DPARAM_TOL] = sqrt(options->dparam[0]);
options_vi_eg->iparam[SICONOS_VI_IPARAM_ERROR_EVALUATION] = SICONOS_VI_ERROR_EVALUATION_LIGHT;
fc3d_VI_ExtraGradient(problem, reaction , velocity , info , options_vi_eg);
fc3d_nonsmooth_Newton_solvers_solve(&equation, reaction, velocity, info, options);
solver_options_delete(options_vi_eg);
free(options_vi_eg);
}
else if (options->iparam[SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY] == SICONOS_FRICTION_3D_NSN_HYBRID_STRATEGY_NO)
{
fc3d_nonsmooth_Newton_solvers_solve(&equation, reaction, velocity, info, options);
}
else
{
numerics_error("fc3d_nonsmooth_Newton_AlartCurnier","Unknown nsn hybrid solver");
}
}
void fc3d_VI_FixedPointProjection(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Dimension of the problem */
int n = 3 * nc;
VariationalInequality *vi = (VariationalInequality *)malloc(sizeof(VariationalInequality));
//vi.self = &vi;
vi->F = &Function_VI_FC3D;
vi->ProjectionOnX = &Projection_VI_FC3D;
int iter=0;
double error=1e24;
FrictionContactProblem_as_VI *fc3d_as_vi= (FrictionContactProblem_as_VI*)malloc(sizeof(FrictionContactProblem_as_VI));
vi->env =fc3d_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;
fc3d_as_vi->vi = vi;
fc3d_as_vi->fc3d = problem;
/* frictionContact_display(fc3d_as_vi->fc3d); */
SolverOptions * visolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));
variationalInequality_setDefaultSolverOptions(visolver_options,
SICONOS_VI_FPP);
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 prolem in fc3d_VI_FixedPointProjection\n");
}
if (dsize != vi_dsize )
{
printf("size prolem in fc3d_VI_FixedPointProjection\n");
}
int i;
for (i = 0; i < isize; i++)
{
if (options->iparam[i] != 0 )
visolver_options->iparam[i] = options->iparam[i] ;
}
for (i = 0; i < dsize; i++)
{
if (fabs(options->dparam[i]) >= 1e-24 )
visolver_options->dparam[i] = options->dparam[i] ;
}
variationalInequality_FixedPointProjection(vi, reaction, velocity , info , visolver_options);
/* **** Criterium convergence **** */
double normq = cblas_dnrm2(nc*3 , problem->q , 1);
fc3d_compute_error(problem, reaction , velocity, options->dparam[0], options, normq, &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[1];
iter = visolver_options->iparam[7];
options->dparam[1] = error;
options->dparam[3] = visolver_options->dparam[3];
options->iparam[7] = iter;
if (verbose > 0)
{
printf("----------------------------------- FC3D - VI Fixed Point Projection (VI_FPP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
free(vi);
solver_options_delete(visolver_options);
free(visolver_options);
free(fc3d_as_vi);
}
void fc3d_ConvexQP_ProjectedGradient_Cylinder(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
{
/* Number of contacts */
int nc = problem->numberOfContacts;
/* Dimension of the problem */
int n = 3 * nc;
ConvexQP *cqp = (ConvexQP *)malloc(sizeof(ConvexQP));
cqp->size=n;
cqp->M = problem->M;
cqp->q = problem->q;
cqp->A = NULL; /* The A matrix is the identity and b is equal to zero */
cqp->b = NULL;
cqp->ProjectionOnC = &Projection_ConvexQP_FC3D_Cylinder;
int iter=0;
double error=1e24;
FrictionContactProblem_as_ConvexQP *fc3d_as_cqp= (FrictionContactProblem_as_ConvexQP*)malloc(sizeof(FrictionContactProblem_as_ConvexQP));
cqp->env = fc3d_as_cqp ;
cqp->size = n;
/*set the norm of the ConvexQP to the norm of problem->q */
double norm_q = cblas_dnrm2(nc*3 , problem->q , 1);
cqp->normConvexQP= norm_q;
cqp->istheNormConvexQPset=1;
fc3d_as_cqp->cqp = cqp;
fc3d_as_cqp->fc3d = problem;
fc3d_as_cqp->options = options;
/* frictionContact_display(fc3d_as_cqp->fc3d); */
SolverOptions * cqpsolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));
convexQP_ProjectedGradient_setDefaultSolverOptions(cqpsolver_options);
int isize = options->iSize;
int dsize = options->dSize;
int cqp_isize = cqpsolver_options->iSize;
int cqp_dsize = cqpsolver_options->dSize;
if (isize != cqp_isize )
{
printf("Warning: options->iSize in fc3d_ConvexQP_FixedPointProjection is not consitent with options->iSize in ConvexQP_FPP\n");
}
if (dsize != cqp_dsize )
{
printf("Warning: options->iSize in fc3d_ConvexQP_FixedPointProjection is not consitent with options->iSize in ConvexQP_FPP\n");
}
int i;
for (i = 0; i < min(isize,cqp_isize); i++)
{
if (options->iparam[i] != 0 )
cqpsolver_options->iparam[i] = options->iparam[i] ;
}
for (i = 0; i < min(dsize,cqp_dsize); i++)
{
if (fabs(options->dparam[i]) >= 1e-24 )
cqpsolver_options->dparam[i] = options->dparam[i] ;
}
convexQP_ProjectedGradient(cqp, reaction, velocity , info , cqpsolver_options);
/* **** Criterium convergence **** */
fc3d_Tresca_compute_error(problem, reaction , velocity, options->dparam[0], options, norm_q, &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 = cqpsolver_options->dparam[1];
iter = cqpsolver_options->iparam[7];
options->dparam[SICONOS_DPARAM_RESIDU] = error;
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (verbose > 0)
{
printf("--------------- FC3D - ConvexQP Fixed Point Projection (ConvexQP_FPP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
free(cqp);
solver_options_delete(cqpsolver_options);
free(cqpsolver_options);
free(fc3d_as_cqp);
}
void convexQP_VI_solver(ConvexQP* problem, double *z, double *w, int* info, SolverOptions* options)
{
NumericsMatrix* A = problem->A;
if (A)
{
numerics_error("ConvexQP_VI_Solver", "This solver does not support a specific matrix A different from the identity");
}
/* Dimension of the problem */
int n = problem->size;
VariationalInequality *vi = (VariationalInequality *)malloc(sizeof(VariationalInequality));
//vi.self = &vi;
vi->F = &Function_VI_CQP;
vi->ProjectionOnX = &Projection_VI_CQP;
int iter=0;
double error=1e24;
ConvexQP_as_VI *convexQP_as_vi= (ConvexQP_as_VI*)malloc(sizeof(ConvexQP_as_VI));
vi->env =convexQP_as_vi ;
vi->size = n;
/*set the norm of the VI to the norm of problem->q */
double norm_q = cblas_dnrm2(n, problem->q , 1);
vi->normVI= norm_q;
vi->istheNormVIset=1;
convexQP_as_vi->vi = vi;
convexQP_as_vi->cqp = problem;
SolverOptions * visolver_options = (SolverOptions *) malloc(sizeof(SolverOptions));
if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
{
variationalInequality_setDefaultSolverOptions(visolver_options, SICONOS_VI_FPP);
}
else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
{
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("Warning: options->iSize in convexQP_VI_solver is not consitent with options->iSize in VI solver\n");
}
if (dsize != vi_dsize )
{
printf("Warning: options->iSize in convexQP_VI_solver is not consitent with options->iSize in VI solver\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] ;
}
if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
{
variationalInequality_FixedPointProjection(vi, z, w , info , visolver_options);
}
else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
{
variationalInequality_ExtraGradient(vi, z, w , info , visolver_options);
}
/* **** Criterium convergence **** */
convexQP_compute_error_reduced(problem, z , w, options->dparam[0], options, norm_q, &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[3] = visolver_options->dparam[SICONOS_VI_EG_DPARAM_RHO];
options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
if (verbose > 0)
{
if (options->solverId==SICONOS_CONVEXQP_VI_FPP)
{
printf("--------------- CONVEXQP - VI solver (VI_FPP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
else if (options->solverId==SICONOS_CONVEXQP_VI_EG)
{
printf("--------------- CONVEXQP - VI solver (VI_EG) - #Iteration %i Final Residual = %14.7e\n", iter, error);
}
}
free(vi);
solver_options_delete(visolver_options);
free(visolver_options);
free(convexQP_as_vi);
}
