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 "Friction_cst.h" // for SICONOS_GLOBAL_FRICTION_3D...
20 #include "GlobalFrictionContactProblem.h" // for GlobalFrictionContactProblem
21 #include "SolverOptions.h" // for SolverOptions, solver_opti...
22 #include "NumericsSparseMatrix.h" // for NSM_TRIPLET ...
23 #include "numerics_verbose.h" // for numerics_printf_verbose
24 #include "SiconosBlas.h" // for cblas_dcopy, cblas_dscal
25 #include "CSparseMatrix_internal.h" // for NSM_TRIPLET ...
26 #include "gfc3d_balancing.h"
27
28
29 /* #define DEWBUG_STDOUT */
30 /* #define DEBUG_MESSAGES */
31 #include "siconos_debug.h" // for DEBUG_EXPR
32 #ifdef DEBUG_MESSAGES
33 #include "NumericsVector.h"
34 #include "NumericsMatrix.h"
35 #endif
36
gfc3d_rescaling(GlobalFrictionContactProblem * problem,double alpha,double beta,double gamma)37 void gfc3d_rescaling(
38 GlobalFrictionContactProblem* problem,
39 double alpha,
40 double beta,
41 double gamma)
42 {
43 int nc = problem->numberOfContacts;
44 int n = problem->M->size0;
45 int m = 3 * nc;
46
47 /* scaling of M */
48 NM_scal(alpha*gamma*gamma, problem->M);
49 /* scaling of H */
50 NM_scal(beta*gamma, problem->H);
51 /* scaling of q */
52 cblas_dscal(n,alpha*gamma,problem->q,1);
53 /* scaling of b */
54 cblas_dscal(m,beta,problem->b,1);
55
56 }
57
gfc3d_balancing_MHHT(GlobalFrictionContactProblem * problem,BalancingMatrices * B_for_M,BalancingMatrices * B_for_H)58 void gfc3d_balancing_MHHT(
59 GlobalFrictionContactProblem* problem,
60 BalancingMatrices * B_for_M,
61 BalancingMatrices * B_for_H)
62 {
63 assert(B_for_M);
64 assert(B_for_H);
65
66 int nc = problem->numberOfContacts;
67 int n = problem->M->size0;
68 int m = 3 * nc;
69
70 /* scaling of M */
71 NumericsMatrix* M_tmp = NM_create(NM_SPARSE, n, n);
72 NM_triplet_alloc(M_tmp, n);
73 NM_gemm(1.0,problem->M,B_for_M->D2,0.0, M_tmp) ; // M * D2M
74 NM_gemm(1.0,B_for_M->D2,M_tmp, 0.0, problem->M); // D1M *M * D2M
75
76 /* scaling of H */
77 /* Warning the matrix H must be scaled such
78 * that the cone constraint is respected */
79 for (int contact =0, i=0; contact < nc; contact++, i++, i++, i++)
80 {
81 /* A choice among other */
82 NM_triplet(B_for_H->D2)->x[i+1] = NM_triplet(B_for_H->D2)->x[i];
83 NM_triplet(B_for_H->D2)->x[i+2] = NM_triplet(B_for_H->D2)->x[i];
84 }
85 NumericsMatrix* H_tmp = NM_create(NM_SPARSE, n, m);
86 NM_triplet_alloc(H_tmp, n);
87 NM_gemm(1.0,problem->H,B_for_H->D2,0.0, H_tmp); // H * D2H
88 NM_gemm(1.0,B_for_M->D1,H_tmp, 0.0, problem->H); // D1M * H* D2H
89 //NM_gemm(1.0,B_for_M->D1,problem->M, 0.0, M_tmp);
90 //NM_copy(M_tmp, problem->M);
91
92 /* scaling of q */
93 double * q_tmp = (double *) calloc(n,sizeof(double));
94 NM_gemv(1.0, B_for_M->D2, problem->q, 0.0, q_tmp);
95 cblas_dcopy(n, q_tmp, 1, problem->q, 1);
96
97 /* scaling of b */
98 double * b_tmp = (double *) calloc(m,sizeof(double));
99 NM_gemv(1.0, B_for_H->D2, problem->b, 0.0, b_tmp);
100 cblas_dcopy(m, b_tmp, 1, problem->b, 1);
101
102
103 NM_clear(M_tmp);
104 free(M_tmp);
105 NM_clear(H_tmp);
106 free(H_tmp);
107
108 free(q_tmp);
109 free(b_tmp);
110 }
111
112
gfc3d_balancing_M(GlobalFrictionContactProblem * problem,BalancingMatrices * B_for_M)113 void gfc3d_balancing_M(
114 GlobalFrictionContactProblem* problem,
115 BalancingMatrices * B_for_M)
116 {
117 assert(B_for_M);
118
119 NM_compute_balancing_matrices(problem->M, 1e-03, 100, B_for_M);
120
121 int nc = problem->numberOfContacts;
122 int n = problem->M->size0;
123 int m = 3 * nc;
124 /* scaling of M */
125 /* NM_scal(alpha*gamma*gamma, problem->M); */
126 NumericsMatrix* M_tmp = NM_create(NM_SPARSE, n, n);
127 NM_triplet_alloc(M_tmp, n);
128 NM_gemm(1.0,problem->M,B_for_M->D2,0.0, M_tmp);
129 NM_gemm(1.0,B_for_M->D1,M_tmp, 0.0, problem->M);
130
131 /* scaling of H */
132 /* NM_scal(beta*gamma, problem->H);*/
133 /* Warning the matrix H must be scaled such
134 * that the cone constraint is respected */
135 NumericsMatrix* H_tmp = NM_create(NM_SPARSE, n, m);
136 NM_triplet_alloc(H_tmp, n);
137 NM_gemm(1.0, B_for_M->D2, problem->H, 0.0, H_tmp);
138 NM_copy(H_tmp, problem->H);
139
140 /* scaling of q */
141 /* cblas_dscal(n,alpha*gamma,problem->q,1); */
142 double * q_tmp = (double *) calloc(n,sizeof(double));
143 NM_gemv(1.0, B_for_M->D2, problem->q, 0.0, q_tmp);
144 cblas_dcopy(n, q_tmp, 1, problem->q, 1);
145
146 /* scaling of b */
147 /* cblas_dscal(m,beta,problem->b,1); */
148
149 NM_clear(M_tmp);
150 free(M_tmp);
151 NM_clear(H_tmp);
152 free(H_tmp);
153
154 free(q_tmp);
155 //free(b_tmp);
156 }
157
158
gfc3d_balancing_problem(GlobalFrictionContactProblem * problem,SolverOptions * options)159 GlobalFrictionContactProblem* gfc3d_balancing_problem(GlobalFrictionContactProblem* problem,
160 SolverOptions* options)
161 {
162 GlobalFrictionContactProblem * rescaled_problem = NULL;
163 GlobalFrictionContactProblem_balancing_data *data = NULL;
164
165 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]>0)
166 {
167 rescaled_problem = globalFrictionContact_copy(problem);
168 data = gfc3d_balancing_data_new();
169 rescaled_problem->env = (void*) data;
170 data->original_problem = problem;
171 }
172 else
173 {
174 return problem;
175 }
176
177 size_t nc = problem->numberOfContacts;
178 size_t n = problem->M->size0;
179 size_t m = 3 * nc;
180 // double* q = problem->q;
181 // double* b = problem->b;
182 // double* mu = problem->mu;
183
184 NumericsMatrix *M = problem->M;
185 NumericsMatrix *H = problem->H;
186
187
188 data->original_problem = problem;
189
190 double alpha_r=0.0, beta_r=0.0;
191 // BalancingMatrices * B_for_M = NULL;
192 // BalancingMatrices * B_for_H = NULL;
193
194 /* NumericsMatrix *Htrans = NM_transpose(H); */
195 /* /\* Compute M + rho H H^T (storage in W)*\/ */
196 /* NumericsMatrix *W = NM_create(NM_SPARSE,n,n); */
197 /* NM_triplet_alloc(W, n); */
198 /* W->matrix2->origin = NSM_TRIPLET; */
199
200
201 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_SCALAR)
202 {
203 alpha_r = NM_norm_inf(M);
204 beta_r = NM_norm_inf(H);
205 numerics_printf_verbose(1,"---- GFC3D - BALANCING - Scalar rescaling of the problem");
206 numerics_printf_verbose(1,"---- GFC3D - BALANCING - alpha_r = %e\t beta_r= %e\n", alpha_r, beta_r);
207
208 gfc3d_rescaling(rescaled_problem, 1./alpha_r, 1.0/beta_r, 1.0);
209
210 data->alpha= 1.0/alpha_r;
211 data->beta= 1.0/beta_r;
212 data->gamma= 1.0 ;
213 }
214 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_M)
215 {
216 numerics_printf_verbose(1,"---- GFC3D - BALANCING - Rescaling of the problem by balancing M");
217 data->B_for_M = NM_BalancingMatrices_new(problem->M);
218 NM_compute_balancing_matrices(problem->M, 1e-2, 5, data->B_for_M);
219 gfc3d_balancing_M(rescaled_problem, data->B_for_M);
220 }
221 /* else if (options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_H) */
222 /* { */
223 /* numerics_printf_verbose(1,"---- GFC3D - BALANCING - Rescaling of the problem by balancing H"); */
224 /* data->B_for_H = NM_BalancingMatrices_new(problem->H); */
225 /* globalFrictionContact_balancing_H(rescaled_problem, data->B_for_H); */
226 /* } */
227 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_MHHT)
228 {
229
230 numerics_printf_verbose(1,"---- GFC3D - BALANCING - Rescaling of the problem by balancing MHHT");
231
232 /****** compute balancing matrices **************************************/
233 NumericsMatrix * MHHT = NM_create(NM_SPARSE, n+m, n+m);
234 NM_triplet_alloc(MHHT, n+m);
235 MHHT->matrix2->origin = NSM_TRIPLET;
236 NM_insert(MHHT, problem->M, 0, 0);
237 NM_insert(MHHT, problem->H, 0, n);
238 NumericsMatrix *HT = NM_transpose(H);
239 NM_insert(MHHT, HT, n, 0);
240
241 //NM_display(MHHT);
242 BalancingMatrices * B_for_MHHT = NM_BalancingMatrices_new(MHHT);
243 NM_compute_balancing_matrices(MHHT, 1e-2, 5, B_for_MHHT);
244 DEBUG_EXPR(NM_display(B_for_MHHT->D1););
245 DEBUG_EXPR(NM_display(B_for_MHHT->D2););
246
247 /* to simplify the use of the balancing matrices, we split it */
248 data->B_for_M = NM_BalancingMatrices_new(problem->M); // lazy mode
249 data->B_for_H = NM_BalancingMatrices_new(problem->H);
250
251 for(size_t i =0; i < n ; i++)
252 {
253 NM_triplet(data->B_for_M->D1)->x[i] = NM_triplet(B_for_MHHT->D1)->x[i]; // D1M
254 NM_triplet(data->B_for_M->D2)->x[i] = NM_triplet(B_for_MHHT->D2)->x[i]; // D2M
255 }
256
257 for(size_t i =0; i < n ; i++)
258 {
259 NM_triplet(data->B_for_H->D1)->x[i] = NM_triplet(B_for_MHHT->D1)->x[i]; // D1M
260 }
261
262 for(size_t i =0; i < m ; i++)
263 {
264 NM_triplet(data->B_for_H->D2)->x[i] = NM_triplet(B_for_MHHT->D2)->x[i+n]; //D2H
265 }
266
267
268
269 /****** balanced problem ***************************************/
270 gfc3d_balancing_MHHT(rescaled_problem, data->B_for_M, data->B_for_H);
271 }
272 else
273 {
274 numerics_printf_verbose(1,"---- GFC3D - BALANCING - No rescaling of the problem");
275 }
276
277 return rescaled_problem;
278 }
279
280
gfc3d_balancing_go_to_balanced_variables(GlobalFrictionContactProblem * balanced_problem,SolverOptions * options,double * r,double * u,double * v)281 void gfc3d_balancing_go_to_balanced_variables(GlobalFrictionContactProblem* balanced_problem,
282 SolverOptions* options,
283 double *r, double *u, double* v)
284 {
285 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]>0)
286 {
287 size_t nc = balanced_problem->numberOfContacts;
288 size_t n = balanced_problem->M->size0;
289 size_t m = 3 * nc;
290
291 GlobalFrictionContactProblem_balancing_data *data = (GlobalFrictionContactProblem_balancing_data * ) balanced_problem->env;
292 assert(data);
293
294 /* rescale */
295 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_SCALAR)
296 {
297 cblas_dscal(m, data->alpha/data->beta, r, 1);
298 cblas_dscal(m, data->beta, u, 1);
299 cblas_dscal(n, 1.0/data->gamma, v, 1);
300 }
301 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_M)
302 {
303 for(size_t i =0; i < n ; i++)
304 {
305 v[i] = v[i]/NM_triplet(data->B_for_M->D2)->x[i];
306 }
307 //nothing for u and r
308 }
309 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_MHHT)
310 {
311 for(size_t i =0; i < n ; i++)
312 {
313 v[i] = v[i]/NM_triplet(data->B_for_M->D2)->x[i];
314 }
315 for(size_t i =0; i < m ; i++)
316 {
317 r[i] = r[i]/NM_triplet(data->B_for_H->D2)->x[i];
318 u[i] = u[i]*NM_triplet(data->B_for_H->D2)->x[i];
319 }
320 }
321 else
322 numerics_printf_verbose(1,"---- GFC3D - BALANCING - rescaling type is not implemented");
323
324 }
325 //else continue;
326
327 }
gfc3d_balancing_back_to_original_variables(GlobalFrictionContactProblem * balanced_problem,SolverOptions * options,double * r,double * u,double * v)328 void gfc3d_balancing_back_to_original_variables(GlobalFrictionContactProblem* balanced_problem,
329 SolverOptions* options,
330 double *r, double *u, double *v)
331 {
332
333 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]>0)
334 {
335 size_t nc = balanced_problem->numberOfContacts;
336 size_t n = balanced_problem->M->size0;
337 size_t m = 3 * nc;
338
339 GlobalFrictionContactProblem_balancing_data *data = (GlobalFrictionContactProblem_balancing_data * ) balanced_problem->env;
340 assert(data);
341
342 /* rescale */
343 if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_SCALAR)
344 {
345 cblas_dscal(m, data->beta/data->alpha, r, 1);
346 cblas_dscal(m, 1.0/data->beta, u, 1);
347 cblas_dscal(n, data->gamma, v, 1);
348 }
349 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_M)
350 {
351 for(size_t i =0; i < n ; i++)
352 {
353 v[i] = v[i]*NM_triplet(data->B_for_M->D2)->x[i];
354 }
355 //nothing for u and r
356 }
357 else if(options->iparam[SICONOS_FRICTION_3D_IPARAM_RESCALING]==SICONOS_FRICTION_3D_RESCALING_BALANCING_MHHT)
358 {
359 for(size_t i =0; i < n ; i++)
360 {
361 v[i] = v[i]*NM_triplet(data->B_for_M->D2)->x[i];
362 }
363 for(size_t i =0; i < m ; i++)
364 {
365 r[i] = r[i]*NM_triplet(data->B_for_H->D2)->x[i];
366 u[i] = u[i]/NM_triplet(data->B_for_H->D2)->x[i];
367 }
368 }
369 else
370 numerics_printf_verbose(1,"---- GFC3D - BALANCING - rescaling type is not implemented");
371
372 }
373
374 //else continue;
375 }
376
377
gfc3d_balancing_free(GlobalFrictionContactProblem * balanced_problem,SolverOptions * options)378 GlobalFrictionContactProblem* gfc3d_balancing_free(GlobalFrictionContactProblem* balanced_problem,
379 SolverOptions* options)
380 {
381 assert(balanced_problem);
382 GlobalFrictionContactProblem_balancing_data *balancing_data = (GlobalFrictionContactProblem_balancing_data * ) balanced_problem->env;
383 if (balancing_data)
384 {
385 balanced_problem->env = gfc3d_balancing_data_free(balancing_data);
386 globalFrictionContact_free(balanced_problem);
387 return NULL;
388 }
389 else
390 return balanced_problem;
391 }
392
gfc3d_balancing_data_new()393 GlobalFrictionContactProblem_balancing_data * gfc3d_balancing_data_new()
394 {
395 GlobalFrictionContactProblem_balancing_data * data = malloc(sizeof(GlobalFrictionContactProblem_balancing_data));
396 data->B_for_M =NULL;
397 data->B_for_H =NULL;
398 return data;
399 }
400
gfc3d_balancing_data_free(GlobalFrictionContactProblem_balancing_data * data)401 GlobalFrictionContactProblem_balancing_data * gfc3d_balancing_data_free
402 (GlobalFrictionContactProblem_balancing_data * data)
403 {
404 if (data->B_for_M)
405 data->B_for_M = NM_BalancingMatrices_free(data->B_for_M);
406 if (data->B_for_H)
407 data->B_for_H = NM_BalancingMatrices_free(data->B_for_H);
408 free(data);
409 return NULL;
410 }
411