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
19 #include <assert.h> // for assert
20 #include <float.h> // for DBL_EPSILON
21 #include <math.h> // for sqrt
22 #include <stdio.h> // for fprintf, printf, NULL, stderr
23 #include <stdlib.h> // for calloc, free, exit, EXIT_FAILURE
24 #include "FrictionContactProblem.h" // for FrictionContactProblem
25 #include "Friction_cst.h" // for SICONOS_FRICTION_3D_NSGS_LOCAL...
26 #include "NumericsFwd.h" // for SolverOptions, FrictionContact...
27 #include "NumericsMatrix.h" // for NumericsMatrix, RawNumericsMatrix
28 #include "SolverOptions.h" // for SolverOptions, solver_options_...
29 #include "SparseBlockMatrix.h" // for SBM_row_prod
30 #include "fc3d_compute_error.h" // for fc3d_Tresca_unitary_compute_an...
31 #include "fc3d_local_problem_tools.h" // for fc3d_local_problem_compute_q
32 #include "fc3d_projection.h" // for fc3d_projectionOnConeWithDiago...
33 #include "numerics_verbose.h" // for numerics_printf, numerics_prin...
34 #include "projectionOnCone.h" // for projectionOnCone
35 #include "projectionOnCylinder.h" // for projectionOnCylinder
36 #include "SiconosBlas.h" // for cblas_ddot
37 #include "fc3d_Solvers.h"
38 /* #define DEBUG_NOCOLOR */
39 /* #define DEBUG_MESSAGES */
40 /* #define DEBUG_STDOUT */
41 #include "siconos_debug.h" // for DEBUG_PRINTF, DEBUG_EXPR, DEBU...
42 #ifdef DEBUG_MESSAGES
43 #include "NumericsVector.h"
44 #endif
45
46 /* Static variables */
47
48 /* The global problem of size n= 3*nc, nc being the number of contacts, is locally saved in MGlobal and qGlobal */
49 /* mu corresponds to the vector of friction coefficients */
50 /* note that either MGlobal or MBGlobal is used, depending on the chosen storage */
51 /* static int n=0; */
52 /* static const NumericsMatrix* MGlobal = NULL; */
53 /* static const double* qGlobal = NULL; */
54 /* static const double* mu = NULL; */
55
56 /* Local problem operators */
57 /* static const int nLocal = 3; */
58 /* static double* MLocal; */
59 /* static int isMAllocatedIn = 0; /\* True if a malloc is done for MLocal, else false *\/ */
60 /* static double qLocal[3]; */
61 /* static double mu_i = 0.0; */
62
63
fc3d_projection_initialize(FrictionContactProblem * problem,FrictionContactProblem * localproblem)64 void fc3d_projection_initialize(FrictionContactProblem * problem, FrictionContactProblem * localproblem)
65 {
66
67 }
68
fc3d_projection_update(int contact,FrictionContactProblem * problem,FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)69 void fc3d_projection_update(int contact, FrictionContactProblem* problem, FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
70 {
71 /* Build a local problem for a specific contact
72 reaction corresponds to the global vector (size n) of the global problem.
73 */
74
75 /* Call the update function which depends on the storage for MGlobal/MBGlobal */
76 /* Build a local problem for a specific contact
77 reaction corresponds to the global vector (size n) of the global problem.
78 */
79
80 /* The part of MGlobal which corresponds to the current block is copied into MLocal */
81 fc3d_local_problem_fill_M(problem, localproblem, contact);
82
83 /**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
84 excluding the block corresponding to the current contact. ****/
85 fc3d_local_problem_compute_q(problem, localproblem, reaction, contact);
86
87 /* Friction coefficient for current block*/
88 localproblem->mu[0] = problem->mu[contact];
89
90 }
91
fc3d_projectionWithDiagonalization_update(int contact,FrictionContactProblem * problem,FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)92 void fc3d_projectionWithDiagonalization_update(int contact, FrictionContactProblem* problem, FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
93 {
94 /* Build a local problem for a specific contact
95 reaction corresponds to the global vector (size n) of the global problem.
96 */
97
98 /* Call the update function which depends on the storage for MGlobal/MBGlobal */
99 /* Build a local problem for a specific contact
100 reaction corresponds to the global vector (size n) of the global problem.
101 */
102
103 /* The part of MGlobal which corresponds to the current block is copied into MLocal */
104 fc3d_local_problem_fill_M(problem, localproblem, contact);
105
106 /**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
107 excluding the block corresponding to the current contact. ****/
108
109 NumericsMatrix * MGlobal = problem->M;
110 double * MLocal = localproblem->M->matrix0;
111
112
113 double *qLocal = localproblem->q;
114 double * qGlobal = problem->q;
115 int n = 3 * problem->numberOfContacts;
116
117
118 int in = 3 * contact, it = in + 1, is = it + 1;
119 /* reaction current block set to zero, to exclude current contact block */
120 /* double rin= reaction[in] ; double rit= reaction[it] ; double ris= reaction[is] ; */
121 /* qLocal computation*/
122 qLocal[0] = qGlobal[in];
123 qLocal[1] = qGlobal[it];
124 qLocal[2] = qGlobal[is];
125
126 if(MGlobal->storageType == NM_DENSE)
127 {
128 double * MM = MGlobal->matrix0;
129 int incx = n, incy = 1;
130 qLocal[0] += cblas_ddot(n, &MM[in], incx, reaction, incy);
131 qLocal[1] += cblas_ddot(n, &MM[it], incx, reaction, incy);
132 qLocal[2] += cblas_ddot(n, &MM[is], incx, reaction, incy);
133 // Substract diagonal term
134 qLocal[0] -= MM[in + n * in] * reaction[in];
135 qLocal[1] -= MM[it + n * it] * reaction[it];
136 qLocal[2] -= MM[is + n * is] * reaction[is];
137 }
138 else if(MGlobal->storageType == NM_SPARSE_BLOCK)
139 {
140 /* qLocal += rowMB * reaction
141 with rowMB the row of blocks of MGlobal which corresponds to the current contact
142 */
143 SBM_row_prod(n, 3, contact, MGlobal->matrix1, reaction, qLocal, 0);
144 // Substract diagonal term
145 qLocal[0] -= MLocal[0] * reaction[in];
146 qLocal[1] -= MLocal[4] * reaction[it];
147 qLocal[2] -= MLocal[8] * reaction[is];
148
149 }
150 else
151 {
152 fprintf(stderr, "fc3d_projectionWithDiagonalization_update :: Unsupported matrix storage)");
153 exit(EXIT_FAILURE);
154 }
155 /* reaction[in] = rin; reaction[it] = rit; reaction[is] = ris; */
156
157 /* Friction coefficient for current block*/
158 localproblem->mu[0] = problem->mu[contact];
159 }
160
161
fc3d_projection_initialize_with_regularization(FrictionContactProblem * problem,FrictionContactProblem * localproblem)162 void fc3d_projection_initialize_with_regularization(FrictionContactProblem * problem, FrictionContactProblem * localproblem)
163 {
164 if(!localproblem->M->matrix0)
165 localproblem->M->matrix0 = (double*)calloc(9, sizeof(double));
166 }
167
fc3d_projection_update_with_regularization(int contact,FrictionContactProblem * problem,FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)168 void fc3d_projection_update_with_regularization(int contact, FrictionContactProblem * problem, FrictionContactProblem * localproblem, double* reaction, SolverOptions* options)
169 {
170
171
172 /* Build a local problem for a specific contact
173 reaction corresponds to the global vector (size n) of the global problem.
174 */
175
176 /* Call the update function which depends on the storage for MGlobal/MBGlobal */
177 /* Build a local problem for a specific contact
178 reaction corresponds to the global vector (size n) of the global problem.
179 */
180
181 /* The part of MGlobal which corresponds to the current block is copied into MLocal */
182
183 NM_copy_diag_block3(problem->M, contact, &localproblem->M->matrix0);
184
185 /**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
186 excluding the block corresponding to the current contact. ****/
187 fc3d_local_problem_compute_q(problem, localproblem, reaction, contact);
188
189 double rho = options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
190 for(int i = 0 ; i < 3 ; i++) localproblem->M->matrix0[i + 3 * i] += rho ;
191
192 double *qLocal = localproblem->q;
193 int in = 3 * contact, it = in + 1, is = it + 1;
194
195 /* qLocal computation*/
196 qLocal[0] -= rho * reaction[in];
197 qLocal[1] -= rho * reaction[it];
198 qLocal[2] -= rho * reaction[is];
199
200 /* Friction coefficient for current block*/
201 localproblem->mu[0] = problem->mu[contact];
202
203
204 }
205
fc3d_projectionWithDiagonalization_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)206 int fc3d_projectionWithDiagonalization_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions * options)
207 {
208
209
210
211 /* Current block position */
212
213 /* Builds local problem for the current contact */
214 /* fc3d_projection_update(contact, reaction); */
215 /* fc3d_projectionWithDiagonalization_update(contact, reaction); */
216
217
218 double * MLocal = localproblem->M->matrix0;
219 double * qLocal = localproblem->q;
220 double mu_i = localproblem->mu[0];
221 int nLocal = 3;
222
223 double mrn, num, mu2 = mu_i * mu_i;
224
225
226 /* projection */
227 if(qLocal[0] > 0.)
228 {
229 reaction[0] = 0.;
230 reaction[1] = 0.;
231 reaction[2] = 0.;
232 }
233 else
234 {
235 if(MLocal[0] < DBL_EPSILON || MLocal[nLocal + 1] < DBL_EPSILON || MLocal[2 * nLocal + 2] < DBL_EPSILON)
236 {
237 fprintf(stderr, "fc3d_projection error: null term on MLocal diagonal.\n");
238 exit(EXIT_FAILURE);
239 }
240
241 reaction[0] = -qLocal[0] / MLocal[0];
242 reaction[1] = -qLocal[1] / MLocal[nLocal + 1];
243 reaction[2] = -qLocal[2] / MLocal[2 * nLocal + 2];
244
245 mrn = reaction[1] * reaction[1] + reaction[2] * reaction[2];
246
247 if(mrn > mu2 * reaction[0]*reaction[0])
248 {
249 num = mu_i * reaction[0] / sqrt(mrn);
250 reaction[1] = reaction[1] * num;
251 reaction[2] = reaction[2] * num;
252 }
253 }
254 return 0;
255 }
256
fc3d_projectionOnConeWithLocalIteration_initialize(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)257 void fc3d_projectionOnConeWithLocalIteration_initialize(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
258 {
259 size_t nc = problem->numberOfContacts;
260 /* printf("fc3d_projectionOnConeWithLocalIteration_initialize. Allocation of dwork\n"); */
261 if(!localsolver_options->dWork
262 || localsolver_options->dWorkSize < nc)
263 {
264 localsolver_options->dWork = (double *)realloc(localsolver_options->dWork,
265 nc * sizeof(double));
266 localsolver_options->dWorkSize = nc ;
267 }
268 for(size_t i = 0; i < nc; i++)
269 {
270 localsolver_options->dWork[i]=1.0;
271 }
272 }
273
fc3d_projectionOnConeWithLocalIteration_free(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)274 void fc3d_projectionOnConeWithLocalIteration_free(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
275 {
276 free(localsolver_options->dWork);
277 localsolver_options->dWork=NULL;
278 }
279
fc3d_projectionOnConeWithLocalIteration_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)280 int fc3d_projectionOnConeWithLocalIteration_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
281 {
282 DEBUG_BEGIN("fc3d_projectionOnConeWithLocalIteration_solve(...)\n");
283
284 DEBUG_EXPR(frictionContact_display(localproblem););
285 /* int and double parameters */
286 int* iparam = options->iparam;
287 double* dparam = options->dparam;
288
289 double * MLocal = localproblem->M->matrix0;
290 double * qLocal = localproblem->q;
291 double mu_i = localproblem->mu[0];
292 /* int nLocal = 3; */
293
294
295 /* /\* Builds local problem for the current contact *\/ */
296 /* fc3d_projection_update(localproblem, reaction); */
297
298
299 /*double an = 1./(MLocal[0]);*/
300 /* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
301 /* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
302 /* double beta = alpha*alpha - 4*det; */
303 /* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
304
305 /* double an = 1. / (MLocal[0]); */
306
307 /* double at = 1.0 / (MLocal[4] + mu_i); */
308 /* double as = 1.0 / (MLocal[8] + mu_i); */
309 /* at = an; */
310 /* as = an; */
311 double rho= options->dWork[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]], rho_k;
312 DEBUG_PRINTF(" Contact options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER] = %i\n",options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]);
313 DEBUG_PRINTF("saved rho = %14.7e\n",rho);
314 assert(rho >0);
315
316
317
318 /* int incx = 1, incy = 1; */
319 int i ;
320
321
322 double velocity[3],velocity_k[3],reaction_k[3],worktmp[3];
323 double normUT;
324 double localerror = 1.0;
325 //printf ("localerror = %14.7e\n",localerror );
326 int localiter = 0;
327 double localtolerance = dparam[SICONOS_DPARAM_TOL];
328
329 /* Variable for Line_search */
330 double a1,a2;
331 int success = 0;
332 double localerror_k;
333 int ls_iter = 0;
334 int ls_itermax = 10;
335
336 double tau=2.0/3.0, tauinv = 3.0/2.0, L= 0.9, Lmin =0.3;
337
338 numerics_printf_verbose(2,"-- fc3d_projectionOnConeWithLocalIteration_solve contact = %i", options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]);
339 numerics_printf_verbose(2,"-- fc3d_projectionOnConeWithLocalIteration_solve | localiter \t| rho \t\t\t| error\t\t\t|");
340 numerics_printf_verbose(2,"-- | %i \t\t| %.10e\t| %.10e\t|", localiter, rho, localerror);
341
342
343
344 /* printf ("localtolerance = %14.7e\n",localtolerance ); */
345 while((localerror > localtolerance) && (localiter < iparam[SICONOS_IPARAM_MAX_ITER]))
346 {
347 DEBUG_PRINT("\n Local iteration starts \n");
348 localiter ++;
349
350 /* printf ("reaction[0] = %14.7e\n",reaction[0]); */
351 /* printf ("reaction[1] = %14.7e\n",reaction[1]); */
352 /* printf ("reaction[2] = %14.7e\n",reaction[2]); */
353
354 /* Store the error */
355 localerror_k = localerror;
356
357 /* store the reaction at the beginning of the iteration */
358 /* cblas_dcopy(nLocal , reaction , 1 , reaction_k, 1); */
359
360 reaction_k[0]=reaction[0];
361 reaction_k[1]=reaction[1];
362 reaction_k[2]=reaction[2];
363 DEBUG_EXPR(NV_display(reaction_k,3););
364 /* /\* velocity_k <- q *\/ */
365 /* cblas_dcopy_msan(nLocal , qLocal , 1 , velocity_k, 1); */
366 /* /\* velocity_k <- q + M * reaction *\/ */
367 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity_k, incy); */
368 for(i = 0; i < 3; i++) velocity_k[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
369 + MLocal[i + 1 * 3] * reaction[1] +
370 + MLocal[i + 2 * 3] * reaction[2] ;
371 DEBUG_EXPR(NV_display(velocity_k,3););
372 ls_iter = 0 ;
373 success =0;
374 rho_k=rho / tau;
375
376 normUT = sqrt(velocity_k[1] * velocity_k[1] + velocity_k[2] * velocity_k[2]);
377 while(!success && (ls_iter < ls_itermax))
378 {
379
380
381 rho_k = rho_k * tau ;
382 DEBUG_PRINTF("rho_k =%f\n", rho_k);
383 reaction[0] = reaction_k[0] - rho_k * (velocity_k[0] + mu_i * normUT);
384 reaction[1] = reaction_k[1] - rho_k * velocity_k[1];
385 reaction[2] = reaction_k[2] - rho_k * velocity_k[2];
386 DEBUG_PRINT("r-rho tilde v before projection")
387 DEBUG_EXPR(NV_display(reaction,3););
388
389 projectionOnCone(&reaction[0], mu_i);
390
391 /* velocity <- q */
392 /* cblas_dcopy(nLocal , qLocal , 1 , velocity, 1); */
393 /* velocity <- q + M * reaction */
394 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity, incy); */
395
396
397 for(i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
398 + MLocal[i + 1 * 3] * reaction[1] +
399 + MLocal[i + 2 * 3] * reaction[2] ;
400
401
402
403 a1 = sqrt((velocity_k[0] - velocity[0]) * (velocity_k[0] - velocity[0]) +
404 (velocity_k[1] - velocity[1]) * (velocity_k[1] - velocity[1]) +
405 (velocity_k[2] - velocity[2]) * (velocity_k[2] - velocity[2]));
406
407 a2 = sqrt((reaction_k[0] - reaction[0]) * (reaction_k[0] - reaction[0]) +
408 (reaction_k[1] - reaction[1]) * (reaction_k[1] - reaction[1]) +
409 (reaction_k[2] - reaction[2]) * (reaction_k[2] - reaction[2]));
410
411
412
413 success = (rho_k*a1 <= L * a2)?1:0;
414
415 DEBUG_PRINTF("rho_k = %12.8e\t", rho_k);
416 DEBUG_PRINTF("a1 = %12.8e\t", a1);
417 DEBUG_PRINTF("a2 = %12.8e\t", a2);
418 DEBUG_PRINTF("norm reaction = %12.8e\t",
419 sqrt(reaction[0] * reaction[0] +
420 reaction[1] * reaction[1] +
421 reaction[2] * reaction[2]
422 ));
423 DEBUG_PRINTF("success = %i\n", success);
424
425 ls_iter++;
426 }
427
428 /* printf("-- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror); */
429
430 /* compute local error */
431 localerror =0.0;
432 fc3d_unitary_compute_and_add_error(reaction, velocity, mu_i, &localerror, worktmp);
433
434
435 /*Update rho*/
436 if((rho_k*a1 < Lmin * a2) && (localerror < localerror_k))
437 {
438 rho =rho_k*tauinv;
439 }
440 else
441 rho =rho_k;
442
443 numerics_printf_verbose(2,"-- | %i \t\t| %.10e\t| %.10e\t|", localiter, rho, localerror);
444
445 }
446 options->dWork[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]] =rho;
447 options->dparam[SICONOS_DPARAM_RESIDU] = localerror ;
448 DEBUG_PRINTF("final rho =%e\n", rho);
449
450 DEBUG_END("fc3d_projectionOnConeWithLocalIteration_solve(...)\n");
451 if(localerror > localtolerance)
452 return 1;
453 return 0;
454
455 }
456
fc3d_projectionOnCylinder_initialize(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * options)457 void fc3d_projectionOnCylinder_initialize(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* options)
458 {
459 assert(localproblem);
460 assert(!localproblem->mu);
461 localproblem->mu = options->dWork;
462 }
463
fc3d_projectionOnCylinder_update(int contact,FrictionContactProblem * problem,FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)464 void fc3d_projectionOnCylinder_update(int contact, FrictionContactProblem* problem, FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
465 {
466 /* Build a local problem for a specific contact
467 reaction corresponds to the global vector (size n) of the global problem.
468 */
469
470 /* Call the update function which depends on the storage for MGlobal/MBGlobal */
471 /* Build a local problem for a specific contact
472 reaction corresponds to the global vector (size n) of the global problem.
473 */
474
475 /* The part of MGlobal which corresponds to the current block is copied into MLocal */
476 fc3d_local_problem_fill_M(problem, localproblem, contact);
477
478 /**** Computation of qLocal = qBlock + sum over a row of blocks in MGlobal of the products MLocal.reactionBlock,
479 excluding the block corresponding to the current contact. ****/
480 fc3d_local_problem_compute_q(problem, localproblem, reaction, contact);
481
482 }
483
484
fc3d_projectionOnCone_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)485 int fc3d_projectionOnCone_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions * options)
486 {
487
488
489 /* /\* Builds local problem for the current contact *\/ */
490 /* fc3d_projection_update(contact, reaction); */
491
492
493
494 double * MLocal = localproblem->M->matrix0;
495 double * qLocal = localproblem->q;
496 double mu_i = localproblem->mu[0];
497 /* int nLocal = 3; */
498
499 /* this part is critical for the success of the projection */
500 /*double an = 1./(MLocal[0]);*/
501 /* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
502 /* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
503 /* double beta = alpha*alpha - 4*det; */
504 /* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
505
506 //double an = 1./(MLocal[0]+mu_i);
507 double an = 1. / (MLocal[0]);
508
509
510 /* int incx = 1, incy = 1; */
511 double worktmp[3];
512 double normUT;
513 /* cblas_dcopy_msan(nLocal , qLocal, incx , worktmp , incy); */
514 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, worktmp, incy); */
515
516 for(int i = 0; i < 3; i++) worktmp[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
517 + MLocal[i + 1 * 3] * reaction[1] +
518 + MLocal[i + 2 * 3] * reaction[2] ;
519
520
521 normUT = sqrt(worktmp[1] * worktmp[1] + worktmp[2] * worktmp[2]);
522 reaction[0] -= an * (worktmp[0] + mu_i * normUT);
523 reaction[1] -= an * worktmp[1];
524 reaction[2] -= an * worktmp[2];
525
526
527 projectionOnCone(reaction, mu_i);
528 return 0;
529
530 }
531
532
533
fc3d_projection_free(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)534 void fc3d_projection_free(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
535 {
536 }
537
fc3d_projection_with_regularization_free(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)538 void fc3d_projection_with_regularization_free(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
539 {
540 free(localproblem->M->matrix0);
541 localproblem->M->matrix0 = NULL;
542 }
543
544
545
fc3d_projectionOnCone_velocity_solve(FrictionContactProblem * localproblem,double * velocity,SolverOptions * options)546 int fc3d_projectionOnCone_velocity_solve(FrictionContactProblem* localproblem, double* velocity, SolverOptions* options)
547 {
548 /* int and double parameters */
549 /* int* iparam = options->iparam; */
550 /* double* dparam = options->dparam; */
551 /* Current block position */
552
553 /* Builds local problem for the current contact */
554 /* fc3d_projection_update(contact, velocity); */
555
556 double * MLocal = localproblem->M->matrix0;
557 double * qLocal = localproblem->q;
558 double mu_i = localproblem->mu[0];
559 /* int nLocal = 3; */
560
561
562 /*double an = 1./(MLocal[0]);*/
563 /* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
564 /* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
565 /* double beta = alpha*alpha - 4*det; */
566 /* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
567
568 double an = 1. / (MLocal[0]);
569 int i;
570 /* int incx = 1, incy = 1; */
571 double worktmp[3];
572 double normUT;
573
574 /* cblas_dcopy(nLocal , qLocal, incx , worktmp , incy); */
575 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, velocity, incx, 1.0, worktmp, incy); */
576 for(i = 0; i < 3; i++) worktmp[i] = MLocal[i + 0 * 3] * velocity[0] + qLocal[i]
577 + MLocal[i + 1 * 3] * velocity[1] +
578 + MLocal[i + 2 * 3] * velocity[2] ;
579
580
581
582 normUT = sqrt(velocity[1] * velocity[1] + velocity[2] * velocity[2]);
583 velocity[0] -= - mu_i * normUT + an * (worktmp[0]);
584 velocity[1] -= an * worktmp[1];
585 velocity[2] -= an * worktmp[2];
586 double invmui = 1.0 / mu_i;
587 projectionOnCone(velocity, invmui);
588
589 normUT = sqrt(velocity[1] * velocity[1] + velocity[2] * velocity[2]);
590 velocity[0] -= mu_i * normUT;
591 return 0;
592 }
593
594
595
fc3d_projectionOnCylinder_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)596 int fc3d_projectionOnCylinder_solve(FrictionContactProblem *localproblem, double* reaction, SolverOptions* options)
597 {
598 /* int and double parameters */
599 /* int* iparam = options->iparam; */
600 /* double* dparam = options->dparam; */
601 double * MLocal = localproblem->M->matrix0;
602 double * qLocal = localproblem->q;
603 /* int nLocal = 3; */
604
605
606 /* Builds local problem for the current contact */
607 /* fc3d_projection_update(contact, reaction); */
608
609
610 /*double an = 1./(MLocal[0]);*/
611 /* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
612 /* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
613 /* double beta = alpha*alpha - 4*det; */
614 /* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
615
616 double an = 1. / (MLocal[0]);
617 int i;
618 /* int incx = 1, incy = 1; */
619 double worktmp[3];
620
621 double R = localproblem->mu[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]];
622 //printf("R=%e\n", R);
623 /* cblas_dcopy(nLocal , qLocal, incx , worktmp , incy); */
624 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, worktmp, incy); */
625 for(i = 0; i < 3; i++) worktmp[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
626 + MLocal[i + 1 * 3] * reaction[1] +
627 + MLocal[i + 2 * 3] * reaction[2] ;
628 reaction[0] -= an * worktmp[0];
629 reaction[1] -= an * worktmp[1];
630 reaction[2] -= an * worktmp[2];
631
632 projectionOnCylinder(reaction, R);
633 return 0;
634
635 }
636
fc3d_projectionOnCylinderWithLocalIteration_initialize(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * options,SolverOptions * localsolver_options)637 void fc3d_projectionOnCylinderWithLocalIteration_initialize(
638 FrictionContactProblem * problem, FrictionContactProblem * localproblem,
639 SolverOptions* options, SolverOptions* localsolver_options)
640 {
641 int nc = problem->numberOfContacts;
642 /* printf("fc3d_projectionOnConeWithLocalIteration_initialize. Allocation of dwork\n"); */
643 if(localproblem->mu)
644 {
645 free(localproblem->mu);
646 }
647
648 localproblem->mu = options->dWork;
649
650 if(!localsolver_options->dWork)
651 {
652 localsolver_options->dWork = (double *)malloc(nc * sizeof(double));
653 localsolver_options->dWorkSize = nc;
654 }
655 else
656 {
657 fprintf(stderr, "Numerics, fc3d_projectionOnCylinderWithLocalIteration_initialize failed. localsolver_options->dWork is different from NULL.\n");
658 exit(EXIT_FAILURE);
659 }
660 for(int i = 0; i < nc; i++)
661 {
662 localsolver_options->dWork[i]=1.0;
663 }
664 }
fc3d_projectionOnCylinderWithLocalIteration_free(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)665 void fc3d_projectionOnCylinderWithLocalIteration_free(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
666 {
667 localproblem->mu = NULL;
668 free(localsolver_options->dWork);
669 localsolver_options->dWork=NULL;
670 }
671
fc3d_projectionOnCylinder_free(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)672 void fc3d_projectionOnCylinder_free(FrictionContactProblem * problem, FrictionContactProblem * localproblem, SolverOptions* localsolver_options)
673 {
674 localproblem->mu = NULL;
675 }
676
fc3d_projectionOnCylinderWithLocalIteration_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)677 int fc3d_projectionOnCylinderWithLocalIteration_solve(FrictionContactProblem* localproblem, double* reaction, SolverOptions* options)
678 {
679 /* int and double parameters */
680 int* iparam = options->iparam;
681 double* dparam = options->dparam;
682
683 double * MLocal = localproblem->M->matrix0;
684 double * qLocal = localproblem->q;
685 /* int nLocal = 3; */
686
687 /* /\* Builds local problem for the current contact *\/ */
688 /* fc3d_projection_update(localproblem, reaction); */
689
690 /*double an = 1./(MLocal[0]);*/
691 /* double alpha = MLocal[nLocal+1] + MLocal[2*nLocal+2]; */
692 /* double det = MLocal[1*nLocal+1]*MLocal[2*nLocal+2] - MLocal[2*nLocal+1] + MLocal[1*nLocal+2]; */
693 /* double beta = alpha*alpha - 4*det; */
694 /* double at = 2*(alpha - beta)/((alpha + beta)*(alpha + beta)); */
695
696 /* double an = 1. / (MLocal[0]); */
697
698 /* double at = 1.0 / (MLocal[4] + mu_i); */
699 /* double as = 1.0 / (MLocal[8] + mu_i); */
700 /* at = an; */
701 /* as = an; */
702 double rho= options->dWork[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]], rho_k;
703 /* printf ("saved rho = %14.7e\n",rho ); */
704 /* printf ("options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER] = %i\n",options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER] ); */
705
706
707
708 /* int incx = 1, incy = 1; */
709 int i;
710 double velocity[3],velocity_k[3],reaction_k[3], worktmp[3];
711
712 double localerror = 1.0;
713 //printf ("localerror = %14.7e\n",localerror );
714 int localiter = 0;
715 double localtolerance = dparam[SICONOS_DPARAM_TOL];
716
717 /* Variable for Line_search */
718 double a1,a2;
719 int success = 0;
720 double localerror_k;
721 int ls_iter = 0;
722 int ls_itermax = 10;
723 double tau=2.0/3.0, tauinv = 3.0/2.0, L= 0.9, Lmin =0.3;
724
725 double R = localproblem->mu[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]];
726
727 /* printf ("R = %14.7e\n",R ); */
728 while((localerror > localtolerance) && (localiter < iparam[SICONOS_IPARAM_MAX_ITER]))
729 {
730 localiter ++;
731
732 /* printf ("reaction[0] = %14.7e\n",reaction[0]); */
733 /* printf ("reaction[1] = %14.7e\n",reaction[1]); */
734 /* printf ("reaction[2] = %14.7e\n",reaction[2]); */
735
736 /* Store the error */
737 localerror_k = localerror;
738
739 /* /\* store the reaction at the beginning of the iteration *\/ */
740 /* cblas_dcopy_msan(nLocal , reaction , 1 , reaction_k, 1); */
741
742 /* /\* velocity_k <- q *\/ */
743 /* cblas_dcopy_msan(nLocal , qLocal , 1 , velocity_k, 1); */
744
745 /* /\* velocity_k <- q + M * reaction *\/ */
746 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity_k, incy); */
747 reaction_k[0]=reaction[0];
748 reaction_k[1]=reaction[1];
749 reaction_k[2]=reaction[2];
750
751 /* /\* velocity_k <- q *\/ */
752 /* cblas_dcopy_msan(nLocal , qLocal , 1 , velocity_k, 1); */
753 /* /\* velocity_k <- q + M * reaction *\/ */
754 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity_k, incy); */
755 for(i = 0; i < 3; i++) velocity_k[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
756 + MLocal[i + 1 * 3] * reaction[1] +
757 + MLocal[i + 2 * 3] * reaction[2] ;
758
759
760 ls_iter = 0 ;
761 success =0;
762 rho_k=rho / tau;
763
764
765 while(!success && (ls_iter < ls_itermax))
766 {
767 rho_k = rho_k * tau ;
768
769 reaction[0] = reaction_k[0] - rho_k * velocity_k[0];
770 reaction[1] = reaction_k[1] - rho_k * velocity_k[1];
771 reaction[2] = reaction_k[2] - rho_k * velocity_k[2];
772
773 projectionOnCylinder(&reaction[0], R);
774
775 /* /\* velocity <- q *\/ */
776 /* cblas_dcopy(nLocal , qLocal , 1 , velocity, 1); */
777 /* /\* velocity <- q + M * reaction *\/ */
778 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocity, incy); */
779
780 for(i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * reaction[0] + qLocal[i]
781 + MLocal[i + 1 * 3] * reaction[1] +
782 + MLocal[i + 2 * 3] * reaction[2] ;
783
784 a1 = sqrt((velocity_k[0] - velocity[0]) * (velocity_k[0] - velocity[0]) +
785 (velocity_k[1] - velocity[1]) * (velocity_k[1] - velocity[1]) +
786 (velocity_k[2] - velocity[2]) * (velocity_k[2] - velocity[2]));
787
788 a2 = sqrt((reaction_k[0] - reaction[0]) * (reaction_k[0] - reaction[0]) +
789 (reaction_k[1] - reaction[1]) * (reaction_k[1] - reaction[1]) +
790 (reaction_k[2] - reaction[2]) * (reaction_k[2] - reaction[2]));
791
792
793
794 success = (rho_k*a1 <= L * a2)?1:0;
795
796 /* printf("rho_k = %12.8e\t", rho_k); */
797 /* printf("a1 = %12.8e\t", a1); */
798 /* printf("a2 = %12.8e\t", a2); */
799 /* printf("norm reaction = %12.8e\t",sqrt(( reaction[0]) * (reaction[0]) + */
800 /* ( reaction[1]) * reaction[1]) + */
801 /* ( reaction[2]) * ( reaction[2])); */
802 /* printf("success = %i\n", success); */
803
804 ls_iter++;
805 }
806 /* if (verbose>2) */
807 /* printf("-- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror); */
808
809 /* compute local error */
810 localerror =0.0;
811 fc3d_Tresca_unitary_compute_and_add_error(reaction, velocity, R, &localerror,worktmp);
812
813
814 /*Update rho*/
815 if((rho_k*a1 < Lmin * a2) && (localerror < localerror_k))
816 {
817 rho =rho_k*tauinv;
818 }
819 else
820 rho =rho_k;
821
822 if(verbose > 1)
823 {
824 printf("-- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror);
825
826 }
827
828 }
829 options->dWork[options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER]] =rho;
830
831
832 if(verbose > 1)
833 {
834 printf("-- localiter = %i\t, rho= %.10e\t, error = %.10e \n", localiter, rho, localerror);
835
836 }
837
838 if(localerror > localtolerance)
839 return 1;
840 return 0;
841
842 }
843
fc3d_poc_set_default(SolverOptions * options)844 void fc3d_poc_set_default(SolverOptions* options)
845 {
846 options->iparam[SICONOS_FRICTION_3D_CURRENT_CONTACT_NUMBER] = 0; // this will be set by external solver
847 options->dparam[SICONOS_FRICTION_3D_NSN_RHO] = 0.; // Used only for ProjectionOnConeWithRegularization
848 }
849