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 <float.h> // for DBL_EPSILON
19 #include <math.h> // for fabs
20 #ifndef __cplusplus
21 #include <stdbool.h> // for false
22 #endif
23 #include <stdio.h> // for printf
24 #include <stdlib.h> // for free, malloc, exit
25 #include "MLCP_Solvers.h" // for mlcp_compute_error
26 #include "MixedLinearComplementarityProblem.h" // for MixedLinearComplement...
27 #include "NumericsFwd.h" // for SolverOptions, MixedL...
28 #include "SiconosBlas.h" // for cblas_ddot, cblas_dcopy
29 #include "SolverOptions.h" // for SolverOptions, SICONO...
30 #include "mlcp_cst.h" // for SICONOS_IPARAM_MLCP_P...
31 #include "NumericsMatrix.h" // for storageType
32 #include "numerics_verbose.h" // for numerics_printf
33
34
35 /*
36 *
37 * double *z : size n+m
38 * double *w : size n+m
39 */
40
mlcp_pgs(MixedLinearComplementarityProblem * problem_orig,double * z,double * w,int * info,SolverOptions * options)41 void mlcp_pgs(MixedLinearComplementarityProblem* problem_orig, double *z, double *w, int *info, SolverOptions* options)
42 {
43 /* verbose=1; */
44
45 MixedLinearComplementarityProblem* problem;
46
47 if(!problem_orig->isStorageType2)
48 {
49 //mixedLinearComplementarity_display(problem_orig);
50 numerics_printf_verbose(0,"mlcp_pgs: Wrong Storage (!isStorageType2) for PGS solver\n");
51 MixedLinearComplementarityProblem* mlcp_abcd = mixedLinearComplementarity_fromMtoABCD(problem_orig);
52 //mixedLinearComplementarity_display(mlcp_abcd);
53 problem = mlcp_abcd;
54 //exit(EXIT_FAILURE);
55 }
56 else
57 {
58 problem =problem_orig;
59 }
60
61 double* A = problem->A;
62 double* B = problem->B;
63 double* C = problem->C;
64 double* D = problem->D;
65 double* a = problem->a;
66 double* b = problem->b;
67 int n = problem->n;
68 int m = problem->m;
69 double *u = &z[0];
70 double *v = &z[n];
71 double *Buf;
72
73 int incx, incy, incAx, incAy, incBx, incBy;
74 int i, iter;
75 int itermax;
76 int pgsExplicit;
77 double err, vi;
78 double tol;
79 double prev;
80 double *diagA, *diagB;
81
82 incx = 1;
83 incy = 1;
84 /* Recup input */
85
86 itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
87 pgsExplicit = options->iparam[SICONOS_IPARAM_MLCP_PGS_EXPLICIT];
88 tol = options->dparam[SICONOS_DPARAM_TOL];
89
90 /* Initialize output */
91
92 options->iparam[SICONOS_IPARAM_ITER_DONE] = 0;
93 options->dparam[SICONOS_DPARAM_RESIDU] = 0.0;
94
95 /* Allocation */
96
97 diagA = (double*)malloc(n * sizeof(double));
98 diagB = (double*)malloc(m * sizeof(double));
99
100
101
102 incx = 1;
103 incy = 1;
104
105 /* Preparation of the diagonal of the inverse matrix */
106
107 for(i = 0 ; i < n ; ++i)
108 {
109 if((fabs(A[i * n + i]) < DBL_EPSILON))
110 {
111 numerics_printf_verbose(1," Vanishing diagonal term A[%i,%i]= %14.8e", i, i, A[i * n + i] );
112 numerics_printf_verbose(1," The local problem cannot be solved");
113
114 *info = 2;
115 free(diagA);
116 free(diagB);
117 *info = 1;
118 if(!problem_orig->isStorageType2)
119 {
120 mixedLinearComplementarity_free(problem);
121 }
122 return;
123 }
124 else
125 {
126 diagA[i] = 1.0 / A[i * n + i];
127 }
128 }
129 for(i = 0 ; i < m ; ++i)
130 {
131 if((fabs(B[i * m + i]) < DBL_EPSILON))
132 {
133
134 numerics_printf_verbose(1," Vanishing diagonal term \n");
135 numerics_printf_verbose(1," The local problem cannot be solved \n");
136
137 *info = 2;
138 free(diagA);
139 free(diagB);
140
141 if(!problem_orig->isStorageType2)
142 {
143 mixedLinearComplementarity_free(problem);
144 }
145 return;
146 }
147 else
148 {
149 diagB[i] = 1.0 / B[i * m + i];
150 }
151 }
152 /*start iterations*/
153
154 iter = 0;
155 err = 1.;
156
157 incx = 1;
158 incy = 1;
159 incAx = n;
160 incAy = 1;
161 incBx = m;
162 incBy = 1;
163
164
165 mlcp_compute_error(problem_orig, z, w, tol, &err);
166
167 while((iter < itermax) && (err > tol))
168 {
169
170 ++iter;
171
172 incx = 1;
173 incy = 1;
174
175 if(pgsExplicit)
176 {
177 /*Use w like a buffer*/
178 cblas_dcopy(n, w, incx, u, incy); //w <- q
179 Buf = w;
180
181 for(i = 0 ; i < n ; ++i)
182 {
183 prev = Buf[i];
184 Buf[i] = 0;
185 //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i];
186 u[i] = - (a[i] + cblas_ddot(n, &A[i], incAx, Buf, incAy) + cblas_ddot(m, &C[i], incAx, v, incBy)) * diagA[i];
187 Buf[i] = prev;
188 }
189 for(i = 0 ; i < m ; ++i)
190 {
191 v[i] = 0.0;
192 //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i];
193 vi = -(b[i] + cblas_ddot(n, &D[i], incBx, u, incAy) + cblas_ddot(m, &B[i], incBx, v, incBy)) * diagB[i];
194
195 if(vi < 0) v[i] = 0.0;
196 else v[i] = vi;
197 }
198 }
199 else
200 {
201
202 for(i = 0 ; i < n ; ++i)
203 {
204 u[i] = 0.0;
205
206 //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i];
207 u[i] = - (a[i] + cblas_ddot(n, &A[i], incAx, u, incAy) + cblas_ddot(m, &C[i], incAx, v, incBy)) * diagA[i];
208 }
209
210 for(i = 0 ; i < m ; ++i)
211 {
212 v[i] = 0.0;
213 //zi = -( q[i] + cblas_ddot( n , &vec[i] , incx , z , incy ))*diag[i];
214 vi = -(b[i] + cblas_ddot(n, &D[i], incBx, u, incAy) + cblas_ddot(m, &B[i], incBx, v, incBy)) * diagB[i];
215
216 if(vi < 0) v[i] = 0.0;
217 else v[i] = vi;
218 }
219 }
220
221 /* **** Criterium convergence compliant with filter_result_MLCP **** */
222
223 mlcp_compute_error(problem_orig, z, w, tol, &err);
224 numerics_printf_verbose(1,"---- MLCP - PGS - Iteration %i residual = %14.7e, tol = %14.7e", iter, err, tol);
225 if(verbose > 1)
226 {
227 for(i = 0 ; i < n ; ++i) printf(" %g", u[i]);
228 for(i = 0 ; i < m ; ++i) printf(" %g", v[i]);
229 for(i = 0 ; i < m ; ++i) printf(" %g", w[i]);
230 printf("\n");
231 }
232
233 /* **** ********************* **** */
234
235 }
236
237 options->iparam[SICONOS_IPARAM_ITER_DONE] = iter;
238 options->dparam[SICONOS_DPARAM_RESIDU] = err;
239
240 if(err > tol)
241 {
242 numerics_printf_verbose(1,"---- MLCP - PGS - No convergence of PGS after %d iterations with error = %14.7e ", iter, err);
243 *info = 1;
244 }
245 else
246 {
247 numerics_printf_verbose(1,"---- MLCP - PGS - Convergence of PGS after %d iterations with error = %14.7e ", iter, err);
248 *info = 0;
249 }
250
251 free(diagA);
252 free(diagB);
253
254 if(!problem_orig->isStorageType2)
255 {
256 mixedLinearComplementarity_free(problem);
257 }
258
259 /* verbose=0; */
260
261 return;
262 }
263
mlcp_pgs_set_default(SolverOptions * options)264 void mlcp_pgs_set_default(SolverOptions* options)
265 {
266 options->filterOn = false;
267 options->iparam[SICONOS_IPARAM_MAX_ITER] = 50000;
268 options->iparam[SICONOS_IPARAM_MLCP_PGS_EXPLICIT] = 0; //implicit
269 }
270