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 <stdio.h> // for printf
20 #include <stdlib.h> // for calloc, free, malloc
21 #include "FrictionContactProblem.h" // for FrictionContactProblem
22 #include "LCP_Solvers.h" // for lcp_compute_error, lcp_enu...
23 #include "LinearComplementarityProblem.h" // for LinearComplementarityProblem
24 #include "NonSmoothDrivers.h" // for linearComplementarity_driver
25 #include "NumericsFwd.h" // for SolverOptions, LinearCompl...
26 #include "SiconosBlas.h" // for cblas_dnrm2
27 #include "SolverOptions.h" // for SolverOptions, SICONOS_DPA...
28 #include "fc2d_Solvers.h" // for fc2d_tolcp, fc2d_enum
29 #include "fc2d_compute_error.h" // for fc2d_compute_error
30 #include "lcp_cst.h" // for SICONOS_LCP_ENUM
31 #include "numerics_verbose.h" // for verbose
32 #include "Friction_cst.h" // for SICONOS_FRICTION_2D_ENUM
33
fc2d_enum(FrictionContactProblem * problem,double * reaction,double * velocity,int * info,SolverOptions * options)34 void fc2d_enum(FrictionContactProblem* problem, double *reaction, double *velocity, int *info, SolverOptions* options)
35 {
36 int i;
37 // conversion into LCP
38 LinearComplementarityProblem* lcp_problem = (LinearComplementarityProblem*)malloc(sizeof(LinearComplementarityProblem));
39
40 fc2d_tolcp(problem, lcp_problem);
41 /* frictionContact_display(problem); */
42 /* linearComplementarity_display(lcp_problem); */
43
44
45
46 double *zlcp = (double*)calloc(lcp_problem->size, sizeof(double));
47 double *wlcp = (double*)calloc(lcp_problem->size, sizeof(double));
48
49 // Call the lcp_solver
50 options->solverId = SICONOS_LCP_ENUM;
51 lcp_enum_init(lcp_problem, options, 1);
52 //}
53 * info = linearComplementarity_driver(lcp_problem, zlcp, wlcp, options);
54 if(options->filterOn > 0)
55 lcp_compute_error(lcp_problem, zlcp, wlcp, options->dparam[SICONOS_DPARAM_TOL], &(options->dparam[SICONOS_DPARAM_RESIDU]));
56 lcp_enum_reset(lcp_problem, options, 1);
57
58 /* printf("\n"); */
59 int nc = problem->numberOfContacts;
60 double norm_q = cblas_dnrm2(nc*2, problem->q, 1);
61 // Conversion of result
62 for(i = 0; i < nc; i++)
63 {
64
65 /* printf("Contact number = %i\n",i); */
66 reaction[2 * i] = zlcp[3 * i];
67 reaction[2 * i + 1] = 1.0 / 2.0 * (zlcp[3 * i + 1] - wlcp[3 * i + 2]);
68 /* printf("reaction[ %i]=%12.10e\n", 2*i, reaction[2*i]); */
69 /* printf("reaction[ %i]=%12.10e\n", 2*i+1, reaction[2*i+1]); */
70
71 velocity[2 * i] = wlcp[3 * i];
72 velocity[2 * i + 1] = wlcp[3 * i + 1] - zlcp[3 * i + 2];
73 /* printf("velocity[ %i]=%12.10e\n", 2*i, velocity[2*i]); */
74 /* printf("velocity[ %i]=%12.10e\n", 2*i+1, velocity[2*i+1]); */
75 }
76
77 /* for (i=0; i< lcp_problem->size; i++){ */
78 /* printf("zlcp[ %i]=%12.10e,\t wlcp[ %i]=%12.10e \n", i, zlcp[i],i, wlcp[i]); */
79 /* } */
80 /* printf("\n"); */
81
82 /* for (i=0; i< problem->size; i++){ */
83 /* printf("z[ %i]=%12.10e,\t w[ %i]=%12.10e\n", i, z[i],i, w[i]); */
84 /* } */
85
86
87 /* printf("\n"); */
88 // back to fc2d for the solver name
89 options->solverId = SICONOS_FRICTION_2D_ENUM;
90
91 if(options->dparam[SICONOS_DPARAM_RESIDU] > options->iparam[SICONOS_DPARAM_TOL])
92 {
93
94 if(verbose > 0)
95 printf("--------------- FC2D - ENUM - No convergence after %i iterations"
96 " residual = %14.7e < %7.3e\n", options->iparam[SICONOS_IPARAM_ITER_DONE],
97 options->dparam[SICONOS_DPARAM_RESIDU],
98 options->dparam[SICONOS_DPARAM_TOL]);
99
100 }
101 else
102 {
103
104 if(verbose > 0)
105 printf("--------------- FC2D - ENUM - Convergence after %i iterations"
106 " residual = %14.7e < %7.3e\n", options->iparam[SICONOS_IPARAM_ITER_DONE],
107 options->dparam[SICONOS_DPARAM_RESIDU], options->dparam[SICONOS_DPARAM_TOL]);
108
109 *info = 0;
110 }
111 double error;
112 *info = fc2d_compute_error(problem, reaction, velocity, options->dparam[SICONOS_DPARAM_TOL], norm_q, &error);
113 free(zlcp);
114 free(wlcp);
115 freeLinearComplementarityProblem(lcp_problem);
116 }
117
118
119
120