1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18 #include <assert.h> // for assert
19 #include <float.h> // for DBL_EPSILON
20 #include <math.h> // for pow, fabs, sqrt, isinf
21 #include <stdlib.h> // for calloc, malloc, srand
22 #include <string.h> // for NULL, memcpy
23 #include "SiconosBlas.h" // for cblas_dnrm2
24 #include "Friction_cst.h" // for SICONOS_FRICTION_3D_IP...
25 #include "NumericsArrays.h" // for uint_shuffle
26 #include "NumericsFwd.h" // for SolverOptions, Rolling...
27 #include "RollingFrictionContactProblem.h" // for RollingFrictionContact...
28 #include "SolverOptions.h" // for SolverOptions, SICONOS...
29 #include "siconos_debug.h" // for DEBUG_PRINTF, DEBUG_BEGIN
30 #include "numerics_verbose.h" // for numerics_printf, numer...
31 #include "rolling_fc_Solvers.h" // for RollingComputeErrorPtr
32 #include "rolling_fc2d_compute_error.h" // for rolling_fc3d_compute_e...
33 #include "rolling_fc2d_local_problem_tools.h" // for rolling_fc3d_local_pro...
34 #include "rolling_fc2d_projection.h" // for rolling_fc3d_projectio...
35
36 //#define FCLIB_OUTPUT
37
38 #ifdef FCLIB_OUTPUT
39 static int fccounter = -1;
40 #include "fclib_interface.h"
41 #endif
42
43
44
45 #pragma GCC diagnostic ignored "-Wmissing-prototypes"
46
47 /* static void rolling_fc2d_nsgs_update(int contact, RollingFrictionContactProblem* problem, RollingFrictionContactProblem* localproblem, double * reaction, SolverOptions* options) */
48 /* { */
49 /* /\* Build a local problem for a specific contact */
50 /* reaction corresponds to the global vector (size n) of the global problem. */
51 /* *\/ */
52 /* /\* Call the update function which depends on the storage for MGlobal/MBGlobal *\/ */
53 /* /\* Build a local problem for a specific contact */
54 /* reaction corresponds to the global vector (size n) of the global problem. */
55 /* *\/ */
56
57 /* /\* The part of MGlobal which corresponds to the current block is copied into MLocal *\/ */
58 /* rolling_fc2d_local_problem_fill_M(problem, localproblem, contact); */
59
60 /* /\**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock, */
61 /* excluding the block corresponding to the current contact. ****\/ */
62 /* rolling_fc2d_local_problem_compute_q(problem, localproblem, reaction, contact); */
63
64 /* /\* Friction coefficient for current block*\/ */
65 /* localproblem->mu[0] = problem->mu[contact]; */
66 /* /\* Rolling Friction coefficient for current block*\/ */
67 /* localproblem->mu_r[0] = problem->mu_r[contact]; */
68
69
70 /* } */
71
rolling_fc2d_nsgs_initialize_local_solver(RollingSolverPtr * solve,RollingUpdatePtr * update,RollingFreeSolverNSGSPtr * freeSolver,RollingComputeErrorPtr * computeError,RollingFrictionContactProblem * problem,RollingFrictionContactProblem * localproblem,SolverOptions * options)72 void rolling_fc2d_nsgs_initialize_local_solver(RollingSolverPtr* solve, RollingUpdatePtr* update,
73 RollingFreeSolverNSGSPtr* freeSolver,
74 RollingComputeErrorPtr* computeError,
75 RollingFrictionContactProblem* problem,
76 RollingFrictionContactProblem* localproblem,
77 SolverOptions * options)
78 {
79 SolverOptions * localsolver_options = options->internalSolvers[0];
80 /** Connect to local solver */
81 switch(localsolver_options->solverId)
82 {
83 case SICONOS_ROLLING_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration:
84 {
85 *solve = &rolling_fc2d_projectionOnConeWithLocalIteration_solve;
86 *update = &rolling_fc2d_projection_update;
87 *freeSolver = (RollingFreeSolverNSGSPtr)&rolling_fc2d_projectionOnConeWithLocalIteration_free;
88 *computeError = (RollingComputeErrorPtr)&rolling_fc2d_compute_error;
89 rolling_fc2d_projectionOnConeWithLocalIteration_initialize(problem, localproblem,localsolver_options);
90 break;
91 }
92 case SICONOS_ROLLING_FRICTION_3D_ONECONTACT_ProjectionOnCone:
93 {
94 *solve = &rolling_fc2d_projectionOnCone_solve;
95 *update = &rolling_fc2d_projection_update;
96 *freeSolver = (RollingFreeSolverNSGSPtr)&rolling_fc2d_projection_free;
97 *computeError = (RollingComputeErrorPtr)&rolling_fc2d_compute_error;
98 rolling_fc2d_projection_initialize(problem, localproblem);
99 break;
100 }
101 default:
102 {
103 numerics_error("rolling_fc2d_nsgs_initialize_local_solver", "Numerics, rolling_fc2d_nsgs failed. Unknown internal solver : %s.\n", solver_options_id_to_name(localsolver_options->solverId));
104 }
105 }
106 }
107
108
109 static
allocShuffledContacts(RollingFrictionContactProblem * problem,SolverOptions * options)110 unsigned int* allocShuffledContacts(RollingFrictionContactProblem *problem,
111 SolverOptions *options)
112 {
113 unsigned int *scontacts = 0;
114 unsigned int nc = problem->numberOfContacts;
115 if(options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE||
116 options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE_EACH_LOOP)
117 {
118 if(options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE_SEED] >0)
119 {
120 srand((unsigned int)options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE_SEED]);
121 }
122 else
123 srand(1);
124 scontacts = (unsigned int *) malloc(nc * sizeof(unsigned int));
125 for(unsigned int i = 0; i < nc ; ++i)
126 {
127 scontacts[i] = i;
128 }
129 uint_shuffle(scontacts, nc);
130 }
131 return scontacts;
132 }
133
134 static
solveLocalReaction(RollingUpdatePtr update_localproblem,RollingSolverPtr local_solver,unsigned int contact,RollingFrictionContactProblem * problem,RollingFrictionContactProblem * localproblem,double * reaction,SolverOptions * localsolver_options,double localreaction[3])135 int solveLocalReaction(RollingUpdatePtr update_localproblem, RollingSolverPtr local_solver,
136 unsigned int contact, RollingFrictionContactProblem *problem,
137 RollingFrictionContactProblem *localproblem, double *reaction,
138 SolverOptions *localsolver_options, double localreaction[3])
139 {
140 (*update_localproblem)(contact, problem, localproblem,
141 reaction, localsolver_options);
142
143 localsolver_options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER] = contact;
144
145 localreaction[0] = reaction[contact*3 + 0];
146 localreaction[1] = reaction[contact*3 + 1];
147 localreaction[2] = reaction[contact*3 + 2];
148
149
150 return (*local_solver)(localproblem, localreaction, localsolver_options);
151 }
152
153 static
performRelaxation(double localreaction[3],double * oldreaction,double omega)154 void performRelaxation(double localreaction[3], double *oldreaction, double omega)
155 {
156 localreaction[0] = omega*localreaction[0]+(1.0-omega)*oldreaction[0];
157 localreaction[1] = omega*localreaction[1]+(1.0-omega)*oldreaction[1];
158 localreaction[2] = omega*localreaction[2]+(1.0-omega)*oldreaction[2];
159 }
160
161 static
accumulateLightErrorSum(double * light_error_sum,double localreaction[3],double * oldreaction)162 void accumulateLightErrorSum(double *light_error_sum, double localreaction[3],
163 double *oldreaction)
164 {
165 *light_error_sum += (pow(oldreaction[0] - localreaction[0], 2) +
166 pow(oldreaction[1] - localreaction[1], 2) +
167 pow(oldreaction[2] - localreaction[2], 2)) ;
168 }
169
170 static
acceptLocalReactionFiltered(RollingFrictionContactProblem * localproblem,SolverOptions * localsolver_options,unsigned int contact,unsigned int iter,double * reaction,double localreaction[3])171 void acceptLocalReactionFiltered(RollingFrictionContactProblem *localproblem,
172 SolverOptions *localsolver_options,
173 unsigned int contact, unsigned int iter,
174 double *reaction, double localreaction[3])
175 {
176 if(isnan(localsolver_options->dparam[SICONOS_DPARAM_RESIDU])
177 || isinf(localsolver_options->dparam[SICONOS_DPARAM_RESIDU])
178 || localsolver_options->dparam[SICONOS_DPARAM_RESIDU] > 1.0)
179 {
180 DEBUG_EXPR(rollingFrictionContact_display(localproblem));
181 DEBUG_PRINTF("Discard local reaction for contact %i at iteration %i "
182 "with local_error = %e\n",
183 contact, iter, localsolver_options->dparam[SICONOS_DPARAM_RESIDU]);
184
185 numerics_printf("Discard local reaction for contact %i at iteration %i "
186 "with local_error = %e",
187 contact, iter, localsolver_options->dparam[SICONOS_DPARAM_RESIDU]);
188 }
189 else
190 memcpy(&reaction[contact*3], localreaction, sizeof(double)*3);
191 }
192
193 static
acceptLocalReactionUnconditionally(unsigned int contact,double * reaction,double localreaction[3])194 void acceptLocalReactionUnconditionally(unsigned int contact,
195 double *reaction, double localreaction[3])
196 {
197 memcpy(&reaction[contact*3], localreaction, sizeof(double)*3);
198 }
199
200 static
calculateLightError(double light_error_sum,unsigned int nc,double * reaction)201 double calculateLightError(double light_error_sum, unsigned int nc, double *reaction)
202 {
203 DEBUG_BEGIN("calculateLightError(...)\n");
204 double error = sqrt(light_error_sum);
205 double norm_r = cblas_dnrm2(nc*3, reaction, 1);
206 if(fabs(norm_r) > DBL_EPSILON)
207 error /= norm_r;
208 DEBUG_PRINTF("error = %f\n", error);
209 DEBUG_END("calculateLightError(...)\n");
210 return error;
211 }
212
213 static
calculateFullErrorAdaptiveInterval(RollingFrictionContactProblem * problem,RollingComputeErrorPtr computeError,SolverOptions * options,int iter,double * reaction,double * velocity,double tolerance,double norm_q)214 double calculateFullErrorAdaptiveInterval(RollingFrictionContactProblem *problem,
215 RollingComputeErrorPtr computeError,
216 SolverOptions *options, int iter,
217 double *reaction, double *velocity,
218 double tolerance, double norm_q)
219 {
220 double error=1e+24;
221 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] >0)
222 {
223 if(iter % options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] == 0)
224 {
225 (*computeError)(problem, reaction, velocity, tolerance, options, norm_q, &error);
226 if(error > tolerance
227 && options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_ADAPTIVE)
228 options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] *= 2;
229 }
230 numerics_printf("--------------- RFC2D - NSGS - Iteration %i "
231 "options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] = %i, options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] = % i",
232 iter, options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY], options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION]);
233 }
234 else
235 (*computeError)(problem, reaction, velocity, tolerance, options, norm_q, &error);
236
237 return error;
238 }
239
240
241
242 static
calculateFullErrorFinal(RollingFrictionContactProblem * problem,SolverOptions * options,RollingComputeErrorPtr computeError,double * reaction,double * velocity,double tolerance,double norm_q)243 double calculateFullErrorFinal(RollingFrictionContactProblem *problem, SolverOptions *options,
244 RollingComputeErrorPtr computeError,
245 double *reaction, double *velocity, double tolerance,
246 double norm_q)
247 {
248 double absolute_error;
249 (*computeError)(problem, reaction, velocity, tolerance,
250 options, norm_q, &absolute_error);
251
252
253
254 if(verbose > 0)
255 {
256 if(absolute_error > options->dparam[SICONOS_DPARAM_TOL])
257 {
258 numerics_printf("------- RFC2D - NSGS - Warning absolute "
259 "Residual = %14.7e is larger than required precision = %14.7e",
260 absolute_error, options->dparam[SICONOS_DPARAM_TOL]);
261 }
262 else
263 {
264 numerics_printf("------- RFC2D - NSGS - absolute "
265 "Residual = %14.7e is smaller than required precision = %14.7e",
266 absolute_error, options->dparam[SICONOS_DPARAM_TOL]);
267 }
268 }
269 return absolute_error;
270 }
271
272
273 static
determine_convergence(double error,double tolerance,int iter,SolverOptions * options)274 int determine_convergence(double error, double tolerance, int iter,
275 SolverOptions *options)
276 {
277 int hasNotConverged = 1;
278 if(error < tolerance)
279 {
280 hasNotConverged = 0;
281 numerics_printf("--------------- RFC2D - NSGS - Iteration %i "
282 "Residual = %14.7e < %7.3e\n", iter, error, tolerance);
283 }
284 else
285 {
286 numerics_printf("--------------- RFC2D - NSGS - Iteration %i "
287 "Residual = %14.7e > %7.3e\n", iter, error, tolerance);
288 }
289 return hasNotConverged;
290 }
291
292 static
determine_convergence_with_full_final(RollingFrictionContactProblem * problem,SolverOptions * options,RollingComputeErrorPtr computeError,double * reaction,double * velocity,double * tolerance,double norm_q,double error,int iter)293 int determine_convergence_with_full_final(RollingFrictionContactProblem *problem, SolverOptions *options,
294 RollingComputeErrorPtr computeError,
295 double *reaction, double *velocity,
296 double *tolerance, double norm_q, double error,
297 int iter)
298 {
299 int hasNotConverged = 1;
300 if(error < *tolerance)
301 {
302 hasNotConverged = 0;
303 numerics_printf("--------------- RFC2D - NSGS - Iteration %i "
304 "Residual = %14.7e < %7.3e", iter, error, *tolerance);
305
306 double absolute_error = calculateFullErrorFinal(problem, options,
307 computeError,
308 reaction, velocity,
309 options->dparam[SICONOS_DPARAM_TOL],
310 norm_q);
311 if(absolute_error > options->dparam[SICONOS_DPARAM_TOL])
312 {
313 *tolerance = error/absolute_error*options->dparam[SICONOS_DPARAM_TOL];
314 assert(*tolerance > 0.0 && "tolerance has to be positive");
315 /* if (*tolerance < DBL_EPSILON) */
316 /* { */
317 /* numerics_warning("determine_convergence_with_full_final", "We try to set a very smal tolerance"); */
318 /* *tolerance = DBL_EPSILON; */
319 /* } */
320 numerics_printf("------- RFC2D - NSGS - We modify the required incremental precision to reach accuracy to %e", *tolerance);
321 hasNotConverged = 1;
322 }
323 else
324 {
325 numerics_printf("------- RFC2D - NSGS - The incremental precision is sufficient to reach accuracy to %e", *tolerance);
326 }
327
328
329
330
331 }
332 else
333 {
334 numerics_printf("--------------- RFC2D - NSGS - Iteration %i "
335 "Residual = %14.7e > %7.3e", iter, error, *tolerance);
336 }
337 return hasNotConverged;
338 }
339
340
341 static
statsIterationCallback(RollingFrictionContactProblem * problem,SolverOptions * options,double * reaction,double * velocity,double error)342 void statsIterationCallback(RollingFrictionContactProblem *problem,
343 SolverOptions *options,
344 double *reaction, double *velocity, double error)
345 {
346 if(options->callback)
347 {
348 options->callback->collectStatsIteration(options->callback->env,
349 problem->numberOfContacts * 3,
350 reaction, velocity,
351 error, NULL);
352 }
353 }
354
355
356
357
rolling_fc2d_nsgs(RollingFrictionContactProblem * problem,double * reaction,double * velocity,int * info,SolverOptions * options)358 void rolling_fc2d_nsgs(RollingFrictionContactProblem* problem, double *reaction,
359 double *velocity, int* info, SolverOptions* options)
360 {
361
362 /* problem->mu_r[0]=0.1; */
363 /* problem->mu[0]=1.0; */
364
365
366 /* verbose=1; */
367 /* int and double parameters */
368 int* iparam = options->iparam;
369 double* dparam = options->dparam;
370
371 /* Number of contacts */
372 unsigned int nc = problem->numberOfContacts;
373
374 /* Maximum number of iterations */
375 int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
376
377 /* Tolerance */
378 double tolerance = dparam[SICONOS_DPARAM_TOL];
379 double norm_q = cblas_dnrm2(nc*3, problem->q, 1);
380 double omega = dparam[SICONOS_FRICTION_3D_NSGS_RELAXATION_VALUE];
381
382 RollingSolverPtr local_solver = NULL;
383 RollingUpdatePtr update_localproblem = NULL;
384 RollingFreeSolverNSGSPtr freeSolver = NULL;
385 RollingComputeErrorPtr computeError = NULL;
386
387 RollingFrictionContactProblem* localproblem;
388 double localreaction[3];
389
390 /***** NSGS Iterations *****/
391 int iter = 0; /* Current iteration number */
392 double error = 1.; /* Current error */
393 int hasNotConverged = 1;
394 unsigned int contact; /* Number of the current row of blocks in M */
395 unsigned int *scontacts = NULL;
396
397 if(*info == 0)
398 return;
399
400 if(options->numberOfInternalSolvers < 1)
401 {
402 numerics_error("rolling_fc2d_nsgs",
403 "The NSGS method needs options for the internal solvers, "
404 "options[0].numberOfInternalSolvers should be >= 1");
405 }
406 SolverOptions * localsolver_options = options->internalSolvers[0];
407
408 /***** Initialize various solver options *****/
409 localproblem = rolling_fc2d_local_problem_allocate(problem);
410
411 rolling_fc2d_nsgs_initialize_local_solver(&local_solver, &update_localproblem,
412 (RollingFreeSolverNSGSPtr *)&freeSolver,
413 &computeError,
414 problem, localproblem, options);
415
416 scontacts = allocShuffledContacts(problem, options);
417
418 /***** Check solver options *****/
419 if(!(iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_FALSE
420 || iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE
421 || iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE_EACH_LOOP))
422 {
423 numerics_error(
424 "rolling_fc2d_nsgs", "iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] must be equal to "
425 "SICONOS_FRICTION_3D_NSGS_SHUFFLE_FALSE (0), "
426 "SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE (1) or "
427 "SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE_EACH_LOOP (2)");
428 return;
429 }
430
431 if(!(iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_FULL
432 || iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL
433 || iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT
434 || iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_ADAPTIVE))
435 {
436 numerics_error(
437 "rolling_fc2d_nsgs", "iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] must be equal to "
438 "SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_FULL (0), "
439 "SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL (1), "
440 "SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT (2) or "
441 "SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_ADAPTIVE (3)");
442 return;
443 }
444
445 /***** NSGS Iterations *****/
446
447 /* A special case for the most common options (should correspond
448 * with mechanics_run.py **/
449 if(iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_FALSE
450 && iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_RELAXATION_FALSE
451 && iparam[SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION] == SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION_TRUE
452 && iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT)
453 {
454 while((iter < itermax) && (hasNotConverged > 0))
455 {
456 ++iter;
457 double light_error_sum = 0.0;
458
459 rolling_fc2d_set_internalsolver_tolerance(problem, options, localsolver_options, error);
460
461 for(unsigned int i = 0 ; i < nc ; ++i)
462 {
463 contact = i;
464
465
466 solveLocalReaction(update_localproblem, local_solver, contact,
467 problem, localproblem, reaction, localsolver_options,
468 localreaction);
469
470 accumulateLightErrorSum(&light_error_sum, localreaction, &reaction[contact*3]);
471
472 /* #if 0 */
473 acceptLocalReactionFiltered(localproblem, localsolver_options,
474 contact, iter, reaction, localreaction);
475 }
476
477 error = calculateLightError(light_error_sum, nc, reaction);
478
479 hasNotConverged = determine_convergence(error, tolerance, iter, options);
480
481 statsIterationCallback(problem, options, reaction, velocity, error);
482 }
483 }
484
485 /* All other cases, we put all the ifs inline.. otherwise, too many
486 * variations to have dedicated loops, but add more if there are
487 * common cases to avoid checking booleans on every iteration. **/
488 else
489 {
490 while((iter < itermax) && (hasNotConverged > 0))
491 {
492 ++iter;
493 double light_error_sum = 0.0;
494 rolling_fc2d_set_internalsolver_tolerance(problem, options, localsolver_options, error);
495
496 for(unsigned int i = 0 ; i < nc ; ++i)
497 {
498 if(iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE
499 || iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE_EACH_LOOP)
500 {
501 if(iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] == SICONOS_FRICTION_3D_NSGS_SHUFFLE_TRUE_EACH_LOOP)
502 uint_shuffle(scontacts, nc);
503 contact = scontacts[i];
504 }
505 else
506 contact = i;
507
508
509 solveLocalReaction(update_localproblem, local_solver, contact,
510 problem, localproblem, reaction, localsolver_options,
511 localreaction);
512
513 if(iparam[SICONOS_FRICTION_3D_NSGS_RELAXATION] == SICONOS_FRICTION_3D_NSGS_RELAXATION_TRUE)
514 performRelaxation(localreaction, &reaction[contact*3], omega);
515
516 accumulateLightErrorSum(&light_error_sum, localreaction, &reaction[contact*3]);
517
518 /* int test =100; */
519 /* if (contact == test) */
520 /* { */
521 /* printf("reaction[%i] = %16.8e\t",3*contact-1,reaction[3*contact]); */
522 /* printf("localreaction[%i] = %16.8e\n",2,localreaction[0]); */
523 /* } */
524
525
526 if(iparam[SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION] == SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION_TRUE)
527 acceptLocalReactionFiltered(localproblem, localsolver_options,
528 contact, iter, reaction, localreaction);
529 else
530 acceptLocalReactionUnconditionally(contact, reaction, localreaction);
531
532
533
534 }
535
536 if(iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT)
537 {
538 error = calculateLightError(light_error_sum, nc, reaction);
539 hasNotConverged = determine_convergence(error, tolerance, iter, options);
540 }
541 else if(iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL)
542 {
543 error = calculateLightError(light_error_sum, nc, reaction);
544 hasNotConverged = determine_convergence_with_full_final(problem, options, computeError,
545 reaction, velocity,
546 &tolerance, norm_q, error,
547 iter);
548
549 }
550 else if(iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_FULL)
551 {
552 error = calculateFullErrorAdaptiveInterval(problem, computeError, options,
553 iter, reaction, velocity,
554 tolerance, norm_q);
555 hasNotConverged = determine_convergence(error, tolerance, iter, options);
556 }
557
558 statsIterationCallback(problem, options, reaction, velocity, error);
559 }
560 }
561
562
563 /* Full criterium */
564 if(iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] == SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL)
565 {
566 error = calculateFullErrorFinal(problem, options, computeError, reaction, velocity,
567 tolerance, norm_q);
568
569 hasNotConverged = determine_convergence(error, dparam[SICONOS_DPARAM_TOL], iter, options);
570
571
572 }
573
574 *info = hasNotConverged;
575
576
577
578 /** return parameter values */
579 /* dparam[SICONOS_DPARAM_TOL] = tolerance; */
580 dparam[SICONOS_DPARAM_RESIDU] = error;
581 iparam[SICONOS_IPARAM_ITER_DONE] = iter;
582
583 /** Free memory **/
584 (*freeSolver)(problem,localproblem,localsolver_options);
585 rolling_fc2d_local_problem_free(localproblem, problem);
586 if(scontacts) free(scontacts);
587 }
588
rfc2d_nsgs_set_default(SolverOptions * options)589 void rfc2d_nsgs_set_default(SolverOptions* options)
590 {
591 options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION] = SICONOS_FRICTION_3D_NSGS_ERROR_EVALUATION_LIGHT_WITH_FULL_FINAL;
592 options->iparam[SICONOS_FRICTION_3D_IPARAM_INTERNAL_ERROR_STRATEGY] = SICONOS_FRICTION_3D_INTERNAL_ERROR_STRATEGY_GIVEN_VALUE;
593 /* options->iparam[SICONOS_FRICTION_3D_IPARAM_INTERNAL_ERROR_STRATEGY] = SICONOS_FRICTION_3D_INTERNAL_ERROR_STRATEGY_ADAPTIVE; */
594 /* options->iparam[SICONOS_FRICTION_3D_IPARAM_INTERNAL_ERROR_STRATEGY] = SICONOS_FRICTION_3D_INTERNAL_ERROR_STRATEGY_ADAPTIVE_N_CONTACT; */
595 options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE] = SICONOS_FRICTION_3D_NSGS_SHUFFLE_FALSE;
596 options->iparam[SICONOS_FRICTION_3D_NSGS_SHUFFLE_SEED] = 0;
597 options->iparam[SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION] = SICONOS_FRICTION_3D_NSGS_FILTER_LOCAL_SOLUTION_FALSE;
598 options->iparam[SICONOS_FRICTION_3D_NSGS_RELAXATION] = SICONOS_FRICTION_3D_NSGS_RELAXATION_FALSE;
599 options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION_FREQUENCY] = 0;
600 options->dparam[SICONOS_DPARAM_TOL] = 1e-4;
601 options->dparam[SICONOS_FRICTION_3D_DPARAM_INTERNAL_ERROR_RATIO] = 10.0;
602
603 assert(options->numberOfInternalSolvers == 1);
604 options->internalSolvers[0] = solver_options_create(SICONOS_ROLLING_FRICTION_3D_ONECONTACT_ProjectionOnConeWithLocalIteration);
605 }
606