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 <math.h> // for sqrt
20 #include "FrictionContactProblem.h" // for FrictionContac...
21 #include "NumericsFwd.h" // for FrictionContac...
22 #include "NumericsMatrix.h" // for NumericsMatrix
23 #include "siconos_debug.h" // for DEBUG_PRINTF
24 #include "fc3d_AlartCurnier_functions.h" // for computeAlartCu...
25 #include "fc3d_onecontact_nonsmooth_Newton_solvers.h" // for computeNonsmoo...
26 #include "numerics_verbose.h" // for numerics_printf
27 #include "op3x3.h" // for SET3, eig_3x3
28
29 #pragma GCC diagnostic ignored "-Wmissing-prototypes"
30
31 extern computeNonsmoothFunction Function;
32 /* #define VERBOSE_DEBUG */
33
34 /* #define AC_STD */
35 /* #define AC_Generated */
36 /* #define AC_JeanMoreau Christensen & Pang */
37
38
39 /*Static variables */
40 /* Local problem operators */
41 /* static const int nLocal = 3; */
42 /* static double* MLocal; */
43 /* static int isMAllocatedIn = 0; /\* True if a malloc is done for MLocal, else false *\/ */
44 /* static double velocityLocal[3]; */
45 /* static double qLocal[3]; */
46 /* static double mu_i = 0.0; */
47
48
49 /* The global problem of size n= 3*nc, nc being the number of contacts, is locally saved in MGlobal and qGlobal */
50 /* mu corresponds to the vector of friction coefficients */
51 /* note that either MGlobal or MBGlobal is used, depending on the chosen storage */
52 /* static int n=0; */
53 /* static const NumericsMatrix* MGlobal = NULL; */
54 /* static const double* qGlobal = NULL; */
55 /* static const double* mu = NULL; */
56
57 /* static FrictionContactProblem* localFC3D = NULL; */
58 /* static FrictionContactProblem* globalFC3D = NULL; */
59
60
61 /* static double an; */
62 /* static double at; */
63 /* static double projN; */
64 /* static double projT; */
65 /* static double projS; */
66 // Set the function for computing F and its gradient
67 // \todo should nbe done in initialization
68
69
70
71 /* /\* Compute function F(Reaction) *\/ */
72 /* void F_AC(int Fsize, double *reaction , double *F, int up2Date) */
73 /* { */
74
75 /* int nLocal = 3; */
76
77
78
79 /* if (F == NULL) */
80 /* { */
81 /* fprintf(stderr, "fc3d_AlartCurnier::F_AC error: F == NULL.\n"); */
82 /* exit(EXIT_FAILURE); */
83 /* } */
84 /* if (Fsize != nLocal) */
85 /* { */
86 /* fprintf(stderr, "fc3d_AlartCurnier::F error, wrong block size.\n"); */
87 /* exit(EXIT_FAILURE); */
88 /* } */
89 /* double * qLocal = localFC3D->q; */
90 /* double * MLocal = localFC3D->M->matrix0; */
91 /* double mu_i = localFC3D->mu[0]; */
92
93
94 /* /\* up2Date = 1 = true if jacobianF(n, reaction,jacobianF) has been called just before jacobianFMatrix(...). In that case the computation of */
95 /* velocityLocal is not required again. */
96 /* *\/ */
97 /* if (up2Date == 0) */
98 /* { */
99 /* /\* velocityLocal = M.reaction + qLocal *\/ */
100 /* int incx = 1, incy = 1; */
101 /* cblas_dcopy(Fsize, qLocal, incx, velocityLocal, incy); */
102 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocityLocal, incy); */
103 /* /\* velocityLocal[0] = MLocal[0]*reaction[0] + MLocal[Fsize]*reaction[1] + MLocal[2*Fsize]*reaction[2] + qLocal[0]; *\/ */
104 /* /\* velocityLocal[1] = MLocal[1]*reaction[0] + MLocal[Fsize+1]*reaction[1] + MLocal[2*Fsize+1]*reaction[2] + qLocal[1]; *\/ */
105 /* /\* velocityLocal[2] = MLocal[2]*reaction[0] + MLocal[Fsize+2]*reaction[1] + MLocal[2*Fsize+2]*reaction[2] + qLocal[2]; *\/ */
106
107 /* an = 1. / MLocal[0]; */
108 /* double alpha = MLocal[Fsize + 1] + MLocal[2 * Fsize + 2]; */
109 /* double det = MLocal[1 * Fsize + 1] * MLocal[2 * Fsize + 2] - MLocal[2 * Fsize + 1] * MLocal[1 * Fsize + 2]; */
110 /* double beta = alpha * alpha - 4 * det; */
111 /* if (beta > 0.) */
112 /* beta = sqrt(beta); */
113 /* else */
114 /* beta = 0.; */
115 /* at = 2 * (alpha - beta) / ((alpha + beta) * (alpha + beta)); */
116
117 /* /\* Projection on [0, +infty[ and on D(0, mu*reactionn) *\/ */
118 /* projN = reaction[0] - an * velocityLocal[0]; */
119 /* projT = reaction[1] - at * velocityLocal[1]; */
120 /* projS = reaction[2] - at * velocityLocal[2]; */
121 /* } */
122
123 /* double num; */
124
125 /* double coef2 = mu_i * mu_i; */
126 /* if (projN > 0) */
127 /* { */
128 /* F[0] = velocityLocal[0]; */
129 /* } */
130 /* else */
131 /* { */
132 /* F[0] = reaction[0] / an; */
133 /* } */
134
135 /* double mrn = projT * projT + projS * projS; */
136 /* if (mrn <= coef2 * reaction[0]*reaction[0]) */
137 /* { */
138 /* F[1] = velocityLocal[1]; */
139 /* F[2] = velocityLocal[2]; */
140 /* } */
141 /* else */
142 /* { */
143 /* num = mu_i / sqrt(mrn); */
144 /* F[1] = (reaction[1] - projT * reaction[0] * num) / at; */
145 /* F[2] = (reaction[2] - projS * reaction[0] * num) / at; */
146 /* } */
147 /* /\* for(int l = 0; l < 3 ; ++l) *\/ */
148 /* /\* printf(" %lf", F[l]); *\/ */
149 /* /\* printf("\n"); *\/ */
150
151 /* } */
152
153 /* /\* Compute Jacobian of function F *\/ */
154 /* void jacobianF_AC(int Fsize, double *reaction, double *jacobianFMatrix, int up2Date) */
155 /* { */
156 /* int nLocal = 3; */
157
158 /* double * qLocal = localFC3D->q; */
159 /* double * MLocal = localFC3D->M->matrix0; */
160 /* double mu_i = localFC3D->mu[0]; */
161
162
163
164 /* if (jacobianFMatrix == NULL) */
165 /* { */
166 /* fprintf(stderr, "fc3d_AlartCurnier::jacobianF_AC error: jacobianMatrix == NULL.\n"); */
167 /* exit(EXIT_FAILURE); */
168 /* } */
169 /* if (Fsize != nLocal) */
170 /* { */
171 /* fprintf(stderr, "fc3d_AlartCurnier::jacobianF_AC error, wrong block size.\n"); */
172 /* exit(EXIT_FAILURE); */
173 /* } */
174
175
176
177
178
179 /* /\* Warning: input reaction is not necessary equal to the last computed value of reactionBlock *\/ */
180
181 /* /\* up2Date = 1 = true if F(n, reaction,F) has been called just before jacobianFMatrix(...). In that case the computation of */
182 /* velocityLocal is not required again. */
183 /* *\/ */
184 /* if (up2Date == 0) */
185 /* { */
186 /* /\* velocityLocal = M.reaction + qLocal *\/ */
187 /* int incx = 1, incy = 1; */
188 /* cblas_dcopy(Fsize, qLocal, incx, velocityLocal, incy); */
189 /* cblas_dgemv(CblasColMajor,CblasNoTrans, nLocal, nLocal, 1.0, MLocal, 3, reaction, incx, 1.0, velocityLocal, incy); */
190
191 /* an = 1. / MLocal[0]; */
192 /* double alpha = MLocal[Fsize + 1] + MLocal[2 * Fsize + 2]; */
193 /* double det = MLocal[1 * Fsize + 1] * MLocal[2 * Fsize + 2] - MLocal[2 * Fsize + 1] * MLocal[1 * Fsize + 2]; */
194 /* double beta = alpha * alpha - 4 * det; */
195 /* if (beta > 0.) */
196 /* beta = sqrt(beta); */
197 /* else */
198 /* beta = 0.; */
199 /* at = 2 * (alpha - beta) / ((alpha + beta) * (alpha + beta)); */
200
201 /* /\* Projection on [0, +infty[ and on D(0, mu*reactionn) *\/ */
202 /* projN = reaction[0] - an * velocityLocal[0]; */
203 /* projT = reaction[1] - at * velocityLocal[1]; */
204 /* projS = reaction[2] - at * velocityLocal[2]; */
205 /* } */
206
207 /* double coef2 = mu_i * mu_i; */
208
209 /* int i, j; */
210 /* if (projN > 0) */
211 /* { */
212 /* for (j = 0; j < Fsize; ++j) */
213 /* jacobianFMatrix[j * Fsize] = MLocal[j * Fsize]; */
214 /* } */
215 /* else */
216 /* { */
217 /* jacobianFMatrix[0] = 1.0 / an; */
218 /* } */
219
220 /* double mrn = projT * projT + projS * projS; */
221 /* double num, rcof, mrn3; */
222 /* double tmp; */
223 /* if (mrn <= coef2 * reaction[0]*reaction[0]) */
224 /* for (i = 1; i < Fsize; ++i) */
225 /* for (j = 0; j < Fsize; ++j) */
226 /* jacobianFMatrix[j * Fsize + i] = MLocal[j * Fsize + i]; */
227 /* else */
228 /* { */
229 /* num = 1. / sqrt(mrn); */
230 /* mrn3 = 1. / sqrt(mrn) * sqrt(mrn) * sqrt(mrn); */
231 /* rcof = mu_i / at; */
232 /* tmp = at * mrn3 * (MLocal[1] * projT + MLocal[2] * projS); */
233 /* jacobianFMatrix[1] = -rcof * (num * projT + reaction[0] * projT * tmp); */
234 /* jacobianFMatrix[2] = -rcof * (num * projS + reaction[0] * projS * tmp); */
235
236 /* tmp = mrn3 * ((1 - at * MLocal[Fsize + 1]) * projT - at * MLocal[Fsize + 2] * projS); */
237 /* jacobianFMatrix[1 * Fsize + 1] = (1 - mu_i * reaction[0] * (num * (1 - at * MLocal[Fsize + 1]) - projT * tmp)) / at; */
238 /* jacobianFMatrix[1 * Fsize + 2] = - rcof * reaction[0] * ((-num * at * MLocal[Fsize + 2]) - projS * tmp); */
239
240 /* tmp = mrn3 * ((1 - at * MLocal[2 * Fsize + 2]) * projS - at * MLocal[2 * Fsize + 1] * projT); */
241 /* jacobianFMatrix[2 * Fsize + 1] = - rcof * reaction[0] * ((-num * at * MLocal[2 * Fsize + 1]) - projT * tmp); */
242 /* jacobianFMatrix[2 * Fsize + 2] = (1 - mu_i * reaction[0] * (num * (1 - at * MLocal[2 * Fsize + 2]) - projS * tmp)) / at; */
243 /* } */
244 /* /\* for(int l = 0; l < 9 ; ++l) *\/ */
245 /* /\* printf(" %lf", jacobianFMatrix[l]); *\/ */
246 /* /\* printf("\n"); *\/ */
247 /* } */
248
249
250 /* void computeFGlobal_AC(double* reaction, double* FGlobal) */
251 /* { */
252
253 /* int numberOfContacts = globalFC3D->numberOfContacts; */
254
255 /* int n = 3 * numberOfContacts; */
256
257 /* NumericsMatrix * MGlobal = globalFC3D->M; */
258 /* double * MLocal = localFC3D->M->matrix0; */
259 /* double * qLocal = localFC3D->q; */
260 /* double *mu = globalFC3D->mu; */
261
262
263 /* int contact, diagPos = 0, position; */
264 /* int in, it, is, inc, incx; */
265 /* double * reactionLocal; */
266 /* double alpha, det, beta, num, coef2, mrn; */
267 /* for (contact = 0; contact < numberOfContacts; ++contact) */
268 /* { */
269 /* position = 3 * contact; */
270 /* if (MGlobal->storageType == 1) /\* Sparse storage *\/ */
271 /* { */
272 /* /\* The part of MGlobal which corresponds to the current block is copied into MLocal *\/ */
273 /* diagPos = numberOfContacts * contact + contact; */
274 /* MLocal = MGlobal->matrix1->block[diagPos]; */
275 /* } */
276 /* else if (MGlobal->storageType == 0) */
277 /* { */
278 /* in = 3 * contact; */
279 /* it = in + 1; */
280 /* is = it + 1; */
281 /* inc = n * in; */
282 /* double *MM = MGlobal->matrix0; */
283 /* /\* The part of MM which corresponds to the current block is copied into MLocal *\/ */
284 /* MLocal[0] = MM[inc + in]; */
285 /* MLocal[1] = MM[inc + it]; */
286 /* MLocal[2] = MM[inc + is]; */
287 /* inc += n; */
288 /* MLocal[3] = MM[inc + in]; */
289 /* MLocal[4] = MM[inc + it]; */
290 /* MLocal[5] = MM[inc + is]; */
291 /* inc += n; */
292 /* MLocal[6] = MM[inc + in]; */
293 /* MLocal[7] = MM[inc + it]; */
294 /* MLocal[8] = MM[inc + is]; */
295 /* } */
296
297 /* reactionLocal = &reaction[3 * contact]; */
298 /* incx = 3; */
299 /* velocityLocal[0] = cblas_ddot(3 , MLocal , incx , reactionLocal , 1) + qLocal[0]; */
300 /* velocityLocal[1] = cblas_ddot(3 , MLocal , incx , reactionLocal , 1) + qLocal[1]; */
301 /* velocityLocal[2] = cblas_ddot(3 , MLocal , incx , reactionLocal , 1) + qLocal[2]; */
302 /* an = 1. / MLocal[0]; */
303 /* alpha = MLocal[4] + MLocal[8]; */
304 /* det = MLocal[4] * MLocal[8] - MLocal[7] + MLocal[5]; */
305 /* beta = alpha * alpha - 4 * det; */
306 /* at = 2 * (alpha - beta) / ((alpha + beta) * (alpha + beta)); */
307 /* projN = reactionLocal[0] - an * velocityLocal[0]; */
308 /* projT = reactionLocal[1] - at * velocityLocal[1]; */
309 /* projS = reactionLocal[2] - at * velocityLocal[2]; */
310 /* coef2 = mu[contact] * mu[contact]; */
311 /* if (projN > 0) */
312 /* { */
313 /* FGlobal[position] = velocityLocal[0]; */
314 /* } */
315 /* else */
316 /* { */
317 /* FGlobal[position] = reactionLocal[0] / an; */
318 /* } */
319
320 /* mrn = projT * projT + projS * projS; */
321 /* if (mrn <= coef2 * reactionLocal[0]*reactionLocal[0]) */
322 /* { */
323 /* FGlobal[position + 1] = velocityLocal[1]; */
324 /* FGlobal[position + 2] = velocityLocal[2]; */
325 /* } */
326 /* else */
327 /* { */
328 /* num = mu[contact] / sqrt(mrn); */
329 /* FGlobal[position + 1] = (reactionLocal[1] - projT * reactionLocal[0] * num) / at; */
330 /* FGlobal[position + 2] = (reactionLocal[2] - projS * reactionLocal[0] * num) / at; */
331 /* } */
332 /* } */
333 /* } */
334
335
336
337 /* Alart & Curnier version (Radius = mu*max(0,RVN)) */
computeAlartCurnierSTDOld(double R[3],double velocity[3],double mu,double rho[3],double F[3],double A[9],double B[9])338 void computeAlartCurnierSTDOld(double R[3], double velocity[3], double mu, double rho[3], double F[3], double A[9], double B[9])
339 {
340 double RVN, RVT, RVS;
341 double RhoN = rho[0];
342 double RhoT = rho[1];
343 double Radius, RV, RV1, RV3, GammaTT, GammaTS, GammaST, GammaSS;
344
345 RVN = R[0] - RhoN * velocity[0];
346 RVT = R[1] - RhoT * velocity[1];
347 RVS = R[2] - RhoT * velocity[2];
348 RV = sqrt(RVT * RVT + RVS * RVS);
349 //Radius = mu*R[0];
350
351 if(A)
352 {
353 A[0 + 3 * 1] = 0.;
354 A[0 + 3 * 2] = 0.;
355 A[1 + 3 * 0] = 0.;
356 A[2 + 3 * 0] = 0.;
357 }
358
359 if(B)
360 {
361 B[0 + 3 * 1] = 0.;
362 B[0 + 3 * 2] = 0.;
363 B[1 + 3 * 0] = 0.;
364 B[2 + 3 * 0] = 0.;
365 }
366
367 if(RVN >= 0.0)
368 {
369 DEBUG_PRINT("Normal part in the cone\n");
370 Radius = mu * RVN;
371 F[0] = RhoN * (velocity[0]);
372 if(A && B)
373 {
374 A[0 + 3 * 0] = RhoN;
375 B[0 + 3 * 0] = 0.0;
376 }
377 }
378 else
379 {
380 DEBUG_PRINT("Normal part out the cone\n");
381 Radius = 0.0;
382 F[0] = R[0];
383 if(A && B)
384 {
385 A[0 + 3 * 0] = 0.0;
386 B[0 + 3 * 0] = 1.0;
387 }
388 }
389
390 // Compute the value of the Alart--Curnier Function and its gradient for the tangential part
391
392
393 DEBUG_PRINTF("Radius=%le\n", Radius);
394 DEBUG_PRINTF("RV=%le\n", RV);
395
396 if(RV <= Radius) // We are in the disk and Radius is positive
397 {
398 DEBUG_PRINT("We are in the disk\n");
399 F[1] = RhoT * (velocity[1]);
400 F[2] = RhoT * (velocity[2]);
401 if(A && B)
402 {
403 A[1 + 3 * 1] = RhoT;
404 A[1 + 3 * 2] = 0.0;
405 A[2 + 3 * 1] = 0.0;
406 A[2 + 3 * 2] = RhoT;
407 B[1 + 3 * 0] = 0.0;
408 B[1 + 3 * 1] = 0.0;
409 B[1 + 3 * 2] = 0.0;
410 B[2 + 3 * 0] = 0.0;
411 B[2 + 3 * 1] = 0.0;
412 B[2 + 3 * 2] = 0.0;
413 }
414 }
415 else if(RV > Radius) // We are out the disk and Radius is postive
416 {
417
418 if(Radius > 0)
419 {
420 DEBUG_PRINT("We are out the disk and Radius is positive\n");
421 RV1 = 1.0 / RV;
422 F[1] = R[1] - Radius * RVT * RV1;
423 F[2] = R[2] - Radius * RVS * RV1;
424 if(A && B)
425 {
426 RV3 = RV1 * RV1 * RV1;
427 GammaTT = RV1 - RVT * RVT * RV3;
428 GammaTS = - RVT * RVS * RV3;
429 GammaST = GammaTS;
430 GammaSS = RV1 - RVS * RVS * RV3;
431
432 A[1 + 3 * 0] = mu * RhoN * RVT * RV1;
433 A[2 + 3 * 0] = mu * RhoN * RVS * RV1;
434
435
436 A[1 + 3 * 1] = GammaTT * RhoT * Radius;
437
438 A[1 + 3 * 2] = GammaTS * RhoT * Radius;
439 A[2 + 3 * 1] = GammaST * RhoT * Radius;
440
441 A[2 + 3 * 2] = GammaSS * RhoT * Radius;
442
443 B[1 + 3 * 0] = -mu * RVT * RV1;
444
445 B[1 + 3 * 1] = 1.0 - GammaTT * Radius ;
446 B[1 + 3 * 2] = - GammaTS * Radius ;
447
448 B[2 + 3 * 0] = -mu * RVS * RV1;
449
450 B[2 + 3 * 1] = - GammaST * Radius;
451 B[2 + 3 * 2] = 1.0 - GammaSS * Radius;
452 }
453 }
454 else
455 {
456 DEBUG_PRINT("We are out the disk and Radius is zero\n");
457
458 F[1] = R[1] ;
459 F[2] = R[2] ;
460 if(A && B)
461 {
462 A[1 + 3 * 1] = 0.0;
463 A[1 + 3 * 2] = 0.0;
464 A[2 + 3 * 1] = 0.0;
465 A[2 + 3 * 2] = 0.0;
466
467 B[1 + 3 * 0] = 0.0;
468 B[1 + 3 * 1] = 1.0;
469 B[1 + 3 * 2] = 0.0;
470 B[2 + 3 * 0] = 0.0;
471 B[2 + 3 * 1] = 0.0;
472 B[2 + 3 * 2] = 1.0;
473 }
474 }
475
476 }
477 /* else // We are out the disk and Radius is negative */
478 /* { */
479 /* #ifdef VERBOSE_DEBUG */
480 /* printf("We are out the disk and Radius is negative\n"); */
481 /* #endif */
482
483 /* /\*Version original *\/ */
484 /* F[1] = R[1] ; */
485 /* F[2] = R[2] ; */
486 /* if (A && B){ */
487 /* A[1+3*1]=0.0; */
488 /* A[1+3*2]=0.0; */
489 /* A[2+3*1]=0.0; */
490 /* A[2+3*2]=0.0; */
491
492 /* B[1+3*0]=0.0; */
493 /* B[1+3*1]=1.0; */
494 /* B[1+3*2]=0.0; */
495 /* B[2+3*0]=0.0; */
496 /* B[2+3*1]=0.0; */
497 /* B[2+3*2]=1.0;} */
498 /* } */
499
500 DEBUG_PRINTF("F[0] = %le\t", F[0]);
501 DEBUG_PRINTF("F[1] = %le\t", F[1]);
502 DEBUG_PRINTF("F[2] = %le\n", F[2]);
503
504 DEBUG_EXPR(if(A) NM_dense_display(A, 3, 3, 3););
505 DEBUG_EXPR(if(B) NM_dense_display(B, 3, 3, 3););
506 DEBUG_EXPR(
507 if(B)
508 {
509 double diago = 0.0;
510 for(int l = 0; l < 3; l++)
511 {
512 for(int k = 0; k < 3; k++)
513 {
514 if(k == l) diago = 1.0;
515 else diago = 0.0;
516 printf("I-B[%i+3*%i] = %le\t", l, k, diago - B[l + 3 * k]);
517 }
518 printf("\n");
519 }
520 }
521 );
522 }
523
524
525 /* Alart & Curnier version (Radius = mu*max(0,RVN)) */
computeAlartCurnierSTD(double R[3],double velocity[3],double mu,double rho[3],double F[3],double A[9],double B[9])526 void computeAlartCurnierSTD(double R[3], double velocity[3], double mu, double rho[3], double F[3], double A[9], double B[9])
527 {
528 DEBUG_PRINT("computeAlartCurnierSTD starts\n");
529 DEBUG_EXPR_WE(for(int i =0 ; i < 3; i++)printf("R[%i]= %12.8e,\t velocity[%i]= %12.8e,\n",i,R[i],i,velocity[i]););
530
531 SET3(R);
532 SET3(velocity);
533 SET3(rho);
534 SET3(F);
535 SET3X3(A);
536 SET3X3(B);
537
538
539 double RVN, RVT, RVS;
540 double RhoN = *rho0;
541 double RhoT = *rho1;
542 double Radius, RV, RV1, RV3, GammaTT, GammaTS, GammaST, GammaSS;
543
544 RVN = *R0 - RhoN * *velocity0;
545 RVT = *R1 - RhoT * *velocity1;
546 RVS = *R2 - RhoT * *velocity2;
547 RV = sqrt(RVT * RVT + RVS * RVS);
548 //Radius = mu*R[0];
549
550 if(A00)
551 {
552 // always true
553 *A01 = 0.;
554 *A02 = 0.;
555
556 // these should appear under conditions
557 *A10 = 0.;
558 *A20 = 0.;
559 }
560
561 if(B00)
562 {
563 // always true
564 *B01 = 0.;
565 *B02 = 0.;
566
567 // these should appear under conditions
568 *B10 = 0.;
569 *B20 = 0.;
570 }
571
572 if(RVN > 0.0)
573 {
574 DEBUG_PRINT("Normal part in the cone\n");
575
576 Radius = mu * RVN;
577 *F0 = RhoN * *velocity0;
578 if(A00 && B00)
579 {
580 *A00 = RhoN;
581 *B00 = 0.0;
582 }
583 }
584 else
585 {
586 DEBUG_PRINT("Normal part out the cone\n");
587 Radius = 0.0;
588 *F0 = *R0;
589 if(A00 && B00)
590 {
591 *A00 = 0.0;
592 *B00 = 1.0;
593 }
594 }
595
596 // Compute the value of the Alart--Curnier Function and its gradient for the tangential part
597
598
599 DEBUG_PRINTF("Radius=%le\n", Radius);
600 DEBUG_PRINTF("RV=%le\n", RV);
601
602 if(RV <= Radius) // We are in the disk and Radius is positive
603 {
604
605 DEBUG_PRINT("We are in the disk\n");
606
607 *F1 = RhoT * *velocity1;
608 *F2 = RhoT * *velocity2;
609 if(A00 && B00)
610 {
611 *A11 = RhoT;
612 *A12 = 0.0;
613 *A21 = 0.0;
614 *A22 = RhoT;
615 *B10 = 0.0;
616 *B11 = 0.0;
617 *B12 = 0.0;
618 *B20 = 0.0;
619 *B21 = 0.0;
620 *B22 = 0.0;
621 }
622 }
623 else if(RV > Radius) // We are out the disk and Radius is positive
624 {
625
626 if(Radius > 0)
627 {
628
629 DEBUG_PRINT("We are out the disk and Radius is positive\n");
630 RV1 = 1.0 / RV;
631 *F1 = *R1 - Radius * RVT * RV1;
632 *F2 = *R2 - Radius * RVS * RV1;
633 if(A00 && B00)
634 {
635 RV3 = RV1 * RV1 * RV1;
636 GammaTT = RV1 - RVT * RVT * RV3;
637 GammaTS = - RVT * RVS * RV3;
638 GammaST = GammaTS;
639 GammaSS = RV1 - RVS * RVS * RV3;
640
641 *A10 = mu * RhoN * RVT * RV1;
642 *A20 = mu * RhoN * RVS * RV1;
643
644
645 *A11 = GammaTT * RhoT * Radius;
646
647 *A12 = GammaTS * RhoT * Radius;
648 *A21 = GammaST * RhoT * Radius;
649
650 *A22 = GammaSS * RhoT * Radius;
651
652 *B10 = -mu * RVT * RV1;
653
654 *B11 = 1.0 - GammaTT * Radius ;
655 *B12 = - GammaTS * Radius ;
656
657 *B20 = -mu * RVS * RV1;
658
659 *B21 = - GammaST * Radius;
660 *B22 = 1.0 - GammaSS * Radius;
661 }
662 }
663 else
664 {
665
666
667 DEBUG_PRINT("We are out the disk and Radius is zero\n");
668
669
670 *F1 = *R1 ;
671 *F2 = *R2 ;
672 if(A00 && B00)
673 {
674 *A11 = 0.0;
675 *A12 = 0.0;
676 *A21 = 0.0;
677 *A22 = 0.0;
678
679 *B10 = 0.0;
680 *B11 = 1.0;
681 *B12 = 0.0;
682 *B20 = 0.0;
683 *B21 = 0.0;
684 *B22 = 1.0;
685 }
686 }
687
688 }
689 }
690
691
692 /* Christensen & Pang version (Radius = mu* R[0])*/
computeAlartCurnierJeanMoreau(double R[3],double velocity[3],double mu,double rho[3],double F[3],double A[9],double B[9])693 void computeAlartCurnierJeanMoreau(double R[3], double velocity[3], double mu, double rho[3], double F[3], double A[9], double B[9])
694 {
695
696 double RVN, RVT, RVS;
697 double RhoN = rho[0];
698 double RhoT = rho[1];
699 double Radius, RV, RV1, RV3, GammaTT, GammaTS, GammaST, GammaSS;
700
701 RVN = R[0] - RhoN * velocity[0];
702 RVT = R[1] - RhoT * velocity[1];
703 RVS = R[2] - RhoT * velocity[2];
704 RV = sqrt(RVT * RVT + RVS * RVS);
705 Radius = mu * R[0];
706
707 // Compute the value of the Alart--Curnier Function and its gradient for the normal part
708 DEBUG_PRINTF("[Numerics] computeAlartCurnierJeanMoreau - RVN = %e\n", RVN);
709 if(RVN >= 0.0)
710 {
711 DEBUG_PRINT("[Numerics] computeAlartCurnierJeanMoreau - Normal part in the cone\n");
712 F[0] = RhoN * (velocity[0]);
713 if(A && B)
714 {
715 A[0 + 3 * 0] = RhoN;
716 B[0 + 3 * 0] = 0.0;
717 }
718 }
719 else
720 {
721 DEBUG_PRINT("[Numerics] computeAlartCurnierJeanMoreau - Normal part out the cone\n");
722 F[0] = R[0];
723 if(A && B)
724 {
725 A[0 + 3 * 0] = 0.0;
726 B[0 + 3 * 0] = 1.0;
727 }
728 }
729
730 // Compute the value of the Alart--Curnier Function and its gradient for the tangential part
731
732 DEBUG_PRINTF("[Numerics] computeAlartCurnierJeanMoreau - Radius=%le\n", Radius);
733 DEBUG_PRINTF("[Numerics] computeAlartCurnierJeanMoreau - RV=%le\n", RV);
734 if(RV < Radius || RV < 1e-20) // We are in the disk
735 {
736
737 DEBUG_PRINT("[Numerics] computeAlartCurnierJeanMoreau - We are in the disk \n");
738
739 F[1] = RhoT * (velocity[1]);
740 F[2] = RhoT * (velocity[2]);
741 if(A && B)
742 {
743 A[1 + 3 * 1] = RhoT;
744 A[1 + 3 * 2] = 0.0;
745 A[2 + 3 * 1] = 0.0;
746 A[2 + 3 * 2] = RhoT;
747 B[1 + 3 * 0] = 0.0;
748 B[1 + 3 * 1] = 0.0;
749 B[1 + 3 * 2] = 0.0;
750 B[2 + 3 * 0] = 0.0;
751 B[2 + 3 * 1] = 0.0;
752 B[2 + 3 * 2] = 0.0;
753 }
754 }
755 else // We are out the disk
756 {
757 DEBUG_PRINT("[Numerics] computeAlartCurnierJeanMoreau - We are out the disk\n");
758
759 /* RV1 = 1.0/RV; */
760 /* F[1] = R[1] - Radius*RVT*RV1; */
761 /* F[2] = R[2] - Radius*RVS*RV1; */
762
763
764 /* RV3 = RV1*RV1*RV1; */
765 /* GammaTT = (RV - RVT*RVT)*RV3; */
766 /* GammaTS = - RVT*RVS*RV3; */
767 /* GammaST = GammaTS; */
768 /* GammaSS = (RV - RVS*RVS)*RV3; */
769
770
771 RV1 = 1.0 / RV;
772 F[1] = R[1] - Radius * RVT * RV1;
773 F[2] = R[2] - Radius * RVS * RV1;
774 if(A && B)
775 {
776 RV3 = RV1 * RV1 * RV1;
777 GammaTT = RV1 - RVT * RVT * RV3;
778 GammaTS = - RVT * RVS * RV3;
779 GammaST = GammaTS;
780 GammaSS = RV1 - RVS * RVS * RV3;
781
782 // come back to r2146
783 // A[0+3*1]= mu * RhoN * RVT*RV1;
784 // A[0+3*2]= mu * RhoN * RVS*RV1;
785
786 A[1 + 3 * 1] = GammaTT * RhoT * Radius;
787
788 A[1 + 3 * 2] = GammaTS * RhoT * Radius;
789
790 A[2 + 3 * 1] = GammaST * RhoT * Radius;
791 A[2 + 3 * 2] = GammaSS * RhoT * Radius;
792
793 B[1 + 3 * 0] = -mu * RVT * RV1;
794
795 B[1 + 3 * 1] = 1.0 - GammaTT * Radius ;
796 B[1 + 3 * 2] = - GammaTS * Radius ;
797
798 B[2 + 3 * 0] = -mu * RVS * RV1;
799
800 B[2 + 3 * 1] = - GammaST * Radius;
801 B[2 + 3 * 2] = 1.0 - GammaSS * Radius;
802 }
803 }
804
805 DEBUG_EXPR(NV_display(F,3););
806
807 DEBUG_EXPR(if(A) NM_dense_display(A, 3, 3, 3););
808 DEBUG_EXPR(if(B) NM_dense_display(B, 3, 3, 3););
809
810 }
811
812
compute_rho_split_spectral_norm_cond(FrictionContactProblem * localproblem,double * rho)813 void compute_rho_split_spectral_norm_cond(FrictionContactProblem* localproblem, double * rho)
814 {
815 double * MLocal = localproblem->M->matrix0;
816 assert(MLocal[0 + 0 * 3] > 0);
817
818 DEBUG_EXPR(NM_dense_display(MLocal,3,3,3););
819 double sw = MLocal[1 + 1 * 3] + MLocal[2 + 2 * 3];
820
821 double dw = sw * sw - 4.0 * (sw - MLocal[2 + 1 * 3] + MLocal[1 + 2 * 3]);
822 DEBUG_PRINTF("dw = %e\n",dw);
823 if(dw > 0.0) dw = sqrt(dw);
824 else dw = 0.0;
825
826 rho[0] = 1.0 / MLocal[0 + 0 * 3];
827 rho[1] = 2.0 * (sw - dw) / ((sw + dw) * (sw + dw));
828 rho[2] = rho[1];
829
830 assert(rho[0] > 0);
831 assert(rho[1] > 0);
832 assert(rho[2] > 0);
833
834 DEBUG_PRINTF("sw=%le\t ", sw);
835 DEBUG_PRINTF("dw=%le\n ", dw);
836 DEBUG_PRINTF("rho[0]=%le\t", rho[0]);
837 DEBUG_PRINTF("rho[1]=%le\t", rho[1]);
838 DEBUG_PRINTF("rho[2]=%le\n", rho[2]);
839
840 }
841
compute_rho_split_spectral_norm(FrictionContactProblem * localproblem,double * rho)842 void compute_rho_split_spectral_norm(FrictionContactProblem* localproblem, double * rho)
843 {
844 double * MLocal = localproblem->M->matrix0;
845 assert(MLocal[0 + 0 * 3] > 0);
846
847 DEBUG_EXPR(NM_dense_display(MLocal,3,3,3););
848 double sw = MLocal[1 + 1 * 3] + MLocal[2 + 2 * 3];
849
850 double dw = sw * sw - 4.0 * (sw - MLocal[2 + 1 * 3] + MLocal[1 + 2 * 3]);
851 DEBUG_PRINTF("dw = %e\n",dw);
852 if(dw > 0.0) dw = sqrt(dw);
853 else dw = 0.0;
854
855 rho[0] = 1.0 / MLocal[0 + 0 * 3];
856
857
858 rho[1] = 2.0/(sw + dw);
859 rho[2] = rho[1];
860
861 assert(rho[0] > 0);
862 assert(rho[1] > 0);
863 assert(rho[2] > 0);
864
865 DEBUG_PRINTF("sw=%le\t ", sw);
866 DEBUG_PRINTF("dw=%le\n ", dw);
867 DEBUG_PRINTF("rho[0]=%le\t", rho[0]);
868 DEBUG_PRINTF("rho[1]=%le\t", rho[1]);
869 DEBUG_PRINTF("rho[2]=%le\n", rho[2]);
870
871 }
872
compute_rho_spectral_norm(FrictionContactProblem * localproblem,double * rho)873 void compute_rho_spectral_norm(FrictionContactProblem* localproblem, double * rho)
874 {
875 double * MLocal = localproblem->M->matrix0;
876 double worktmp[9] = {0.0, 0.0, 0.0,0.0, 0.0, 0.0,0.0, 0.0, 0.0};
877 double eig[3]= {0.0, 0.0, 0.0};
878 if(eig_3x3(MLocal, worktmp, eig))
879 numerics_printf("compute_rho_spectral_norm : failed");
880 DEBUG_PRINTF("eig[0] = %4.2e, eig[1] = %4.2e, eig[2] = %4.2e", eig[0], eig[1], eig[2]);
881 DEBUG_PRINTF("1/eig[0] = %4.2e, 1/eig[1] = %4.2e, 1/eig[2] = %4.2e", 1.0/eig[0], 1.0/eig[1], 1.0/eig[2]);
882 rho[0]=1.0/eig[0];
883 rho[1]=rho[0];
884 rho[2]=rho[0];
885
886 DEBUG_PRINTF("rho[0]=%le\t", rho[0]);
887 DEBUG_PRINTF("rho[1]=%le\t", rho[1]);
888 DEBUG_PRINTF("rho[2]=%le\n", rho[2]);
889
890 }
891