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
19 #include "gfc3d_compute_error.h"
20 #include <float.h> // for DBL_EPSILON
21 #include <math.h> // for fabs, sqrt
22 #include <stdlib.h> // for NULL, calloc
23 #include "GlobalFrictionContactProblem.h" // for GlobalFrictionContactProblem
24 #include "NumericsMatrix.h" // for NM_gemv, NM_tgemv, Numeric...
25 #include "SolverOptions.h" // for SolverOptions
26 #include "fc3d_compute_error.h" // for fc3d_unitary_compute_and_a...
27 #include "numerics_verbose.h" // for numerics_error, numerics_w...
28 #include "sanitizer.h" // for cblas_dcopy_msan
29 #include "SiconosBlas.h" // for cblas_dcopy, cblas_dnrm2
30 #include "projectionOnCone.h"
31
32 /* #define DEBUG_NOCOLOR */
33 /* #define DEBUG_STDOUT */
34 /* #define DEBUG_MESSAGES */
35 #include "siconos_debug.h"
36
gfc3d_compute_error(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,double tolerance,SolverOptions * options,double norm_q,double norm_b,double * restrict error)37 int gfc3d_compute_error(GlobalFrictionContactProblem* problem,
38 double* reaction, double* velocity,
39 double* globalVelocity,
40 double tolerance,
41 SolverOptions * options,
42 double norm_q, double norm_b,
43 double* restrict error)
44
45 {
46 DEBUG_BEGIN("gfc3d_compute_error(...)\n");
47 /* Checks inputs */
48 if(problem == NULL || globalVelocity == NULL)
49 numerics_error("gfc3d_compute_error", "null input");
50
51 /* Computes error = dnorm2( GlobalVelocity -M^-1( q + H reaction)*/
52 int nc = problem->numberOfContacts;
53 int m = nc * 3;
54 size_t n = problem->M->size0;
55 double *mu = problem->mu;
56 double *q = problem->q;
57
58 DEBUG_PRINTF("norm_b = %12.8e\n", norm_b);
59 DEBUG_PRINTF("norm_q = %12.8e\n", norm_q);
60 double norm_r= cblas_dnrm2(m,reaction,1);
61 DEBUG_PRINTF("norm of reaction %e\n", cblas_dnrm2(m,reaction,1));
62 DEBUG_PRINTF("norm of global velocity %e\n", cblas_dnrm2(n,globalVelocity,1));
63
64 /* DEBUG_EXPR(NV_display(globalVelocity,n)); */
65 /* DEBUG_EXPR(NV_display(reaction,m)); */
66 /* DEBUG_EXPR(NV_display(velocity,m)); */
67
68 NumericsMatrix *H = problem->H;
69 NumericsMatrix *M = problem->M;
70
71 if(!options->dWork || options->dWorkSize < 2*n)
72 {
73 options->dWork = (double *)calloc(2*n,sizeof(double));
74 options->dWorkSize = 2*n;
75 }
76 double* tmp = options->dWork;
77 double* tmp_1 = &options->dWork[n];
78
79 cblas_dcopy_msan(n, q, 1, tmp_1, 1);
80 if(nc >0)
81 {
82 NM_gemv(1.0, H, reaction, 0.0, tmp);
83 }
84 double norm_Hr = cblas_dnrm2(n,tmp,1);
85 DEBUG_PRINTF("norm of H r %e\n", norm_Hr);
86
87
88 cblas_daxpy(n, 1.0, tmp, 1, tmp_1, 1);
89
90
91 NM_gemv(-1.0, M, globalVelocity, 0.0, tmp);
92 double norm_Mv = cblas_dnrm2(n,tmp,1);
93 DEBUG_PRINTF("norm of M v %e\n", norm_Mv);
94
95 cblas_daxpy(n, 1.0, tmp, 1, tmp_1, 1);
96
97 double relative_scaling = fmax(norm_q, fmax(norm_Mv,norm_Hr));
98 *error = cblas_dnrm2(n,tmp_1,1);
99 DEBUG_PRINTF("absolute error of -M v + H R + q = %e\n", *error);
100 if(fabs(relative_scaling) > DBL_EPSILON)
101 *error = *error/relative_scaling;
102
103 DEBUG_PRINTF("relative error of -M v + H R + q = %e\n", *error);
104
105 /* CHECK_RETURN(!NM_gesv_expert(problem->M, globalVelocity, NM_KEEP_FACTORS)); */
106
107 double error_complementarity =0.0;
108
109 /* Checks inputs */
110 if(reaction == NULL || velocity == NULL)
111 numerics_error("gfc3d_compute_error", "null input");
112
113
114 /* we re-compute local velocity */
115 /* the error in the equation u = H^T v +b is then accuaret to machine precision */
116
117 cblas_dcopy(m, problem->b, 1, velocity, 1);
118 NM_tgemv(1, H, globalVelocity, 1.0, velocity);
119 double norm_u = cblas_dnrm2(m,velocity,1);
120 DEBUG_PRINTF("norm of velocity %e\n", norm_u);
121
122 double worktmp[3];
123 for(int ic = 0 ; ic < nc ; ic++)
124 {
125 fc3d_unitary_compute_and_add_error(&reaction[ic * 3], &velocity[ic * 3], mu[ic],
126 &error_complementarity, worktmp);
127 }
128
129 error_complementarity = sqrt(error_complementarity);
130
131 DEBUG_PRINTF("absolute error in complementarity= %e\n", error_complementarity);
132
133 relative_scaling = fmax(norm_u, norm_r);
134 if(fabs(relative_scaling) > DBL_EPSILON)
135 error_complementarity = error_complementarity/relative_scaling;
136 DEBUG_PRINTF("relative error in complementarity= %e\n", error_complementarity);
137
138
139 *error += error_complementarity;
140 DEBUG_PRINTF("relative error = %e\n", *error);
141 if(*error > tolerance)
142 {
143 DEBUG_END("gfc3d_compute_error(...)\n");
144 return 1;
145 }
146 else
147 {
148 DEBUG_END("gfc3d_compute_error(...)\n");
149 return 0;
150 }
151
152 }
fc3d_unitary_compute_and_add_error_convex(double * restrict r,double * restrict u,double mu,double * restrict error,double * worktmp)153 static inline void fc3d_unitary_compute_and_add_error_convex(double* restrict r, double* restrict u, double mu, double* restrict error, double * worktmp)
154 {
155
156 //double normUT;
157 //double worktmp[3];
158 /* Compute the modified local velocity */
159 /* worktmp[0] = r[0] - u[0] - mu * hypot(u[1], u[2]); */
160 worktmp[0] = r[0] - u[0] ;
161 worktmp[1] = r[1] - u[1] ;
162 worktmp[2] = r[2] - u[2] ;
163 projectionOnCone(worktmp, mu);
164 worktmp[0] = r[0] - worktmp[0];
165 worktmp[1] = r[1] - worktmp[1];
166 worktmp[2] = r[2] - worktmp[2];
167 *error += worktmp[0] * worktmp[0] + worktmp[1] * worktmp[1] + worktmp[2] * worktmp[2];
168
169 }
gfc3d_compute_error_convex(GlobalFrictionContactProblem * problem,double * reaction,double * velocity,double * globalVelocity,double tolerance,SolverOptions * options,double norm_q,double norm_b,double * restrict error)170 int gfc3d_compute_error_convex(GlobalFrictionContactProblem* problem,
171 double* reaction, double* velocity,
172 double* globalVelocity,
173 double tolerance,
174 SolverOptions * options,
175 double norm_q, double norm_b,
176 double* restrict error)
177
178 {
179 DEBUG_BEGIN("gfc3d_compute_error_convex(...)\n");
180 /* Checks inputs */
181 if(problem == NULL || globalVelocity == NULL)
182 numerics_error("gfc3d_compute_error", "null input");
183
184 /* Computes error = dnorm2( GlobalVelocity -M^-1( q + H reaction)*/
185 int nc = problem->numberOfContacts;
186 int m = nc * 3;
187 size_t n = problem->M->size0;
188 double *mu = problem->mu;
189 double *q = problem->q;
190
191 DEBUG_PRINTF("norm_b = %12.8e\n", norm_b);
192 DEBUG_PRINTF("norm_q = %12.8e\n", norm_q);
193 double norm_r= cblas_dnrm2(m,reaction,1);
194 DEBUG_PRINTF("norm of reaction %e\n", cblas_dnrm2(m,reaction,1));
195 DEBUG_PRINTF("norm of global velocity %e\n", cblas_dnrm2(n,globalVelocity,1));
196
197 /* DEBUG_EXPR(NV_display(globalVelocity,n)); */
198 /* DEBUG_EXPR(NV_display(reaction,m)); */
199 /* DEBUG_EXPR(NV_display(velocity,m)); */
200
201 NumericsMatrix *H = problem->H;
202 NumericsMatrix *M = problem->M;
203
204 if(!options->dWork || options->dWorkSize < 2*n)
205 {
206 options->dWork = (double *)calloc(2*n,sizeof(double));
207 options->dWorkSize = 2*n;
208 }
209 double* tmp = options->dWork;
210 double* tmp_1 = &options->dWork[n];
211
212 cblas_dcopy_msan(n, q, 1, tmp_1, 1);
213 if(nc >0)
214 {
215 NM_gemv(1.0, H, reaction, 0.0, tmp);
216 }
217 double norm_Hr = cblas_dnrm2(n,tmp,1);
218 DEBUG_PRINTF("norm of H r %e\n", norm_Hr);
219
220 cblas_daxpy(n, 1.0, tmp, 1, tmp_1, 1);
221
222 NM_gemv(-1.0, M, globalVelocity, 0.0, tmp);
223 double norm_Mv = cblas_dnrm2(n,tmp,1);
224 DEBUG_PRINTF("norm of M v %e\n", norm_Mv);
225
226 cblas_daxpy(n, 1.0, tmp, 1, tmp_1, 1);
227
228
229 double relative_scaling = fmax(norm_q, fmax(norm_Mv,norm_Hr));
230 *error = cblas_dnrm2(n,tmp_1,1);
231 DEBUG_PRINTF("absolute error of -M v + H R + q = %e\n", *error);
232 if(fabs(relative_scaling) > DBL_EPSILON)
233 *error = *error/relative_scaling;
234
235 DEBUG_PRINTF("relative error of -M v + H R + q = %e\n", *error);
236
237 /* CHECK_RETURN(!NM_gesv_expert(problem->M, globalVelocity, NM_KEEP_FACTORS)); */
238
239 double error_complementarity =0.0;
240
241 /* Checks inputs */
242 if(reaction == NULL || velocity == NULL)
243 numerics_error("gfc3d_compute_error", "null input");
244
245
246 /* we re-compute local velocity */
247 /* the error in the equation u = H^T v +b is then accuaret to machine precision */
248
249 cblas_dcopy(m, problem->b, 1, velocity, 1);
250 NM_tgemv(1, H, globalVelocity, 1.0, velocity);
251 double norm_u = cblas_dnrm2(m,velocity,1);
252 DEBUG_PRINTF("norm of velocity %e\n", norm_u);
253
254 double worktmp[3];
255 for(int ic = 0 ; ic < nc ; ic++)
256 {
257 fc3d_unitary_compute_and_add_error_convex(&reaction[ic * 3], &velocity[ic * 3], mu[ic],
258 &error_complementarity, worktmp);
259 }
260
261 error_complementarity = sqrt(error_complementarity);
262
263 DEBUG_PRINTF("absolute error in complementarity= %e\n", error_complementarity);
264
265 relative_scaling = fmax(norm_u, norm_r);
266 if(fabs(relative_scaling) > DBL_EPSILON)
267 error_complementarity = error_complementarity/relative_scaling;
268 DEBUG_PRINTF("relative error in complementarity= %e\n", error_complementarity);
269
270
271 *error += error_complementarity;
272 DEBUG_PRINTF("relative error = %e\n", *error);
273 numerics_printf_verbose(1,"---- GFC3D - Compute Error Convex case");
274 if(*error > tolerance)
275 {
276 DEBUG_END("gfc3d_compute_error_convex(...)\n");
277 return 1;
278 }
279 else
280 {
281 DEBUG_END("gfc3d_compute_error_convex(...)\n");
282 return 0;
283 }
284
285 }
286