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 <stdio.h> // for printf, fclose, fopen
20 #include <stdlib.h> // for malloc, free, exit
21 #include "FrictionContactProblem.h" // for FrictionContactProblem
22 #include "GlobalFrictionContactProblem.h" // for GlobalFrictionContac...
23 #include "NumericsFwd.h" // for NumericsMatrix, Fric...
24 #include "NumericsMatrix.h" // for NumericsMatrix, NM_gemv
25 #include "SiconosBlas.h" // for cblas_dcopy, cblas_d...
26 #include "fc3d_Solvers.h" // for fc3d_DeSaxceFixedPoint
27 #include "fc3d_nonsmooth_Newton_AlartCurnier.h" // for fc3d_nonsmooth_Newto...
28 #include "gfc3d_Solvers.h" // for gfc3d_DeSaxceFixedPo...
29 #include "numerics_verbose.h" // for verbose, numerics_pr...
30 #include "gfc3d_compute_error.h"
31 #include "SolverOptions.h" // for SICONOS_DPARAM_TOL
32
33 /* #define DEBUG_MESSAGES */
34 /* #define DEBUG_STDOUT */
35 #include "siconos_debug.h" // for DEBUG_EXPR, DEBUG_P...
36
37
38 #pragma GCC diagnostic ignored "-Wmissing-prototypes"
39
gfc3d_nsgs_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)40 void gfc3d_nsgs_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
41 {
42
43 /* verbose=1; */
44 DEBUG_BEGIN("gfc3d_nsgs_wr\n");
45 NumericsMatrix *H = problem->H;
46 // We compute only if the local problem has contacts
47 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
48 if(H->size1 > 0)
49 {
50 // Reformulation
51 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
52 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
53 DEBUG_EXPR(frictionContact_display(localproblem););
54 if(verbose)
55 {
56 printf("Call to the fc3d solver ...\n");
57 }
58 // call nsgs solver for the local problem
59 fc3d_nsgs(localproblem, reaction, velocity, info, options);
60
61 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
62 /* Number of contacts */
63 int nc = problem->numberOfContacts;
64 /* Dimension of the problem */
65 int m = 3 * nc;
66 int n = problem->M->size0;
67 double norm_q = cblas_dnrm2(n, problem->q, 1);
68 double norm_b = cblas_dnrm2(m, problem->b, 1);
69 double error;
70 gfc3d_compute_error(problem, reaction, velocity, globalVelocity, options->dparam[SICONOS_DPARAM_TOL], options, norm_q, norm_b, &error);
71
72
73 frictionContactProblem_free(localproblem);
74 }
75 else
76 {
77 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
78 *info = 0 ;
79 }
80 DEBUG_END("gfc3d_nsgs_wr\n");
81 }
82
83
gfc3d_admm_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)84 void gfc3d_admm_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
85 {
86 DEBUG_BEGIN("gfc3d_admm_wr\n");
87 NumericsMatrix *H = problem->H;
88 // We compute only if the local problem has contacts
89 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
90 if(H->size1 > 0)
91 {
92 // Reformulation
93 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
94 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
95 DEBUG_EXPR(frictionContact_display(localproblem););
96
97 if(verbose)
98 {
99 printf("Call to the fc3d solver ...\n");
100 }
101 fc3d_admm(localproblem, reaction, velocity, info, options);
102 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
103
104 frictionContactProblem_free(localproblem);
105 }
106 else
107 {
108 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
109 *info = 0 ;
110 }
111 DEBUG_END("gfc3d_admm_wr\n");
112 }
113
gfc3d_nonsmooth_Newton_AlartCurnier_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)114 void gfc3d_nonsmooth_Newton_AlartCurnier_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
115 {
116 DEBUG_BEGIN("gfc3d_nonsmooth_Newton_AlartCurnier_wr(...)\n");
117 NumericsMatrix *H = problem->H;
118 // We compute only if the local problem has contacts
119 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
120 if(H->size1 > 0)
121 {
122 // Reformulation
123 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
124 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
125 DEBUG_EXPR(frictionContact_display(localproblem););
126
127
128 numerics_printf("gfc3d_nonsmooth_Newton_AlartCurnier_wr - Call to the fc3d solver ...\n");
129
130 fc3d_nonsmooth_Newton_AlartCurnier(localproblem, reaction, velocity, info, options);
131
132 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
133
134 frictionContactProblem_free(localproblem);
135 }
136 else
137 {
138 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
139 *info = 0 ;
140 }
141
142 DEBUG_END("gfc3d_nonsmooth_Newton_AlartCurnier_wr(...)\n")
143
144
145 }
146
gfc3d_nsgs_velocity_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)147 void gfc3d_nsgs_velocity_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
148 {
149 NumericsMatrix *H = problem->H;
150 // We compute only if the local problem has contacts
151 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
152 if(H->size1 > 0)
153 {
154 // Reformulation
155 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
156 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
157 DEBUG_EXPR(frictionContact_display(localproblem););
158 if(verbose)
159 {
160 printf("Call to the fc3d solver ...\n");
161 }
162 fc3d_nsgs_velocity(localproblem, reaction, velocity, info, options);
163
164 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
165
166 frictionContactProblem_free(localproblem);
167 }
168 else
169 {
170 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
171 *info = 0 ;
172 }
173 }
174
gfc3d_proximal_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)175 void gfc3d_proximal_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
176 {
177 NumericsMatrix *H = problem->H;
178 // We compute only if the local problem has contacts
179 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
180 if(H->size1 > 0)
181 {
182 // Reformulation
183 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
184 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
185 DEBUG_EXPR(frictionContact_display(localproblem););
186 DEBUG_EXPR(frictionContact_display(localproblem););
187 if(verbose)
188 {
189 printf("Call to the fc3d solver ...\n");
190 }
191 fc3d_proximal(localproblem, reaction, velocity, info, options);
192
193 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
194
195 frictionContactProblem_free(localproblem);
196 }
197 else
198 {
199 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
200 *info = 0 ;
201 }
202 }
203
gfc3d_DeSaxceFixedPoint_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)204 void gfc3d_DeSaxceFixedPoint_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
205 {
206 NumericsMatrix *H = problem->H;
207 // We compute only if the local problem has contacts
208 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
209 if(H->size1 > 0)
210 {
211 // Reformulation
212 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
213 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
214 DEBUG_EXPR(frictionContact_display(localproblem););
215
216 if(verbose)
217 {
218 printf("Call to the fc3d solver ...\n");
219 }
220 fc3d_DeSaxceFixedPoint(localproblem, reaction, velocity, info, options);
221 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
222
223 frictionContactProblem_free(localproblem);
224 }
225 else
226 {
227 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
228 *info = 0 ;
229 }
230 }
231
gfc3d_TrescaFixedPoint_wr(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,int * info,SolverOptions * options)232 void gfc3d_TrescaFixedPoint_wr(GlobalFrictionContactProblem* problem, double *reaction, double *velocity, double* globalVelocity, int *info, SolverOptions* options)
233 {
234 NumericsMatrix *H = problem->H;
235 // We compute only if the local problem has contacts
236 DEBUG_PRINTF("Number of contacts = %i \n", H->size1/3);
237 if(H->size1 > 0)
238 {
239 // Reformulation
240 numerics_printf_verbose(1,"Reformulation info a reduced problem onto local variables ...\n");
241 FrictionContactProblem* localproblem = globalFrictionContact_reformulation_FrictionContact(problem);
242 DEBUG_EXPR(frictionContact_display(localproblem););
243
244 if(verbose)
245 {
246 printf("Call to the fc3d solver ...\n");
247 }
248 fc3d_TrescaFixedPoint(localproblem, reaction, velocity, info, options);
249 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
250
251 frictionContactProblem_free(localproblem);
252 }
253 else
254 {
255 globalFrictionContact_computeGlobalVelocity(problem, reaction, globalVelocity);
256 *info = 0 ;
257 }
258
259 }
260