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