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