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 <assert.h> // for assert
20 #include <stdio.h> // for printf, fprintf, NULL, stderr
21 #include <stdlib.h> // for exit, EXIT_FAILURE
22 #include "FrictionContactProblem.h" // for FrictionContactProblem
23 #include "Friction_cst.h" // for SICONOS_FRICTION_3D_ONECONTACT_NSN
24 #include "NumericsFwd.h" // for SolverOptions, FrictionContactPr...
25 #include "NumericsMatrix.h" // for NumericsMatrix
26 #include "SiconosBlas.h" // for cblas_dnrm2
27 #include "SolverOptions.h" // for SolverOptions, SICONOS_DPARAM_TOL
28 #include "fc3d_Solvers.h" // for ComputeErrorPtr, FreeSolverPtr
29 #include "fc3d_compute_error.h" // for fc3d_compute_error_velocity
30 #include "fc3d_projection.h" // for fc3d_projection_initialize, fc3d...
31 #include "numerics_verbose.h" // for numerics_error, verbose
32 #include "pinv.h" // for pinv
33
34 #pragma GCC diagnostic ignored "-Wmissing-prototypes"
35
fc3d_nsgs_initialize_local_solver_velocity(SolverPtr * solve,FreeSolverPtr * freeSolver,ComputeErrorPtr * computeError,FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)36 void fc3d_nsgs_initialize_local_solver_velocity(SolverPtr* solve, FreeSolverPtr* freeSolver, ComputeErrorPtr* computeError, FrictionContactProblem* problem, FrictionContactProblem* localproblem, SolverOptions* localsolver_options)
37 {
38
39
40
41
42 /** Connect to local solver */
43 /* Projection */
44 if(localsolver_options->solverId == SICONOS_FRICTION_3D_ONECONTACT_ProjectionOnCone_velocity)
45 {
46 *solve = &fc3d_projectionOnCone_velocity_solve;
47 *freeSolver = (FreeSolverPtr)&fc3d_projection_free;
48 *computeError = (ComputeErrorPtr)&fc3d_compute_error_velocity;
49 fc3d_projection_initialize(problem, localproblem);
50 }
51 else
52 {
53 fprintf(stderr, "Numerics, fc3d_nsgs_velocity failed. Unknown internal solver : %s.\n", solver_options_id_to_name(localsolver_options->solverId));
54 exit(EXIT_FAILURE);
55 }
56 }
57
fc3d_nsgs_velocity(FrictionContactProblem * problem,double * reaction,double * velocity,int * info,SolverOptions * options)58 void fc3d_nsgs_velocity(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
59 {
60 /* int and double parameters */
61 int* iparam = options->iparam;
62 double* dparam = options->dparam;
63
64 /* Number of contacts */
65 int nc = problem->numberOfContacts;
66 NumericsMatrix* M = problem->M;
67 /* Dimension of the problem */
68 int n = 3 * nc;
69 /* Maximum number of iterations */
70 int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
71 /* Tolerance */
72 double tolerance = dparam[SICONOS_DPARAM_TOL];
73 double norm_q = cblas_dnrm2(nc*3, problem->q, 1);
74 /* Check for trivial case */
75 /* *info = fc3d_checkTrivialCase(n, q,velocity, reaction, options); */
76
77 if(*info == 0)
78 return;
79
80 SolverPtr local_solver = NULL;
81 FreeSolverPtr freeSolver = NULL;
82 ComputeErrorPtr computeError = NULL;
83
84
85
86
87
88 if(M->storageType == 0)
89 {
90 /* /\* Inversion of the matrix M *\/ */
91 /* int* ipiv = (int *)malloc(n*sizeof(*ipiv)); */
92 /* int infoDGETRF=0; */
93 /* DGETRF(n,n,M->matrix0,n, ipiv,infoDGETRF ); */
94 /* assert(!infoDGETRF); */
95 /* int infoDGETRI; */
96 /* DGETRI(n,M->matrix0,n, ipiv,infoDGETRI ); */
97 /* assert(!infoDGETRI); */
98 /* double* qtmp = (double*)malloc(n*sizeof(double)); */
99 /* cblas_dcopy(n, q, 1, qtmp, 1); */
100 /* cblas_dgemv(CblasColMajor,CblasNoTrans, n, n, -1.0, M->matrix0 , n, qtmp,1,0.0,q, 1); */
101 /* free(ipiv); */
102 /* free(qtmp); */
103 double tolpinv = 1e-07;
104 pinv(M->matrix0, n, n, tolpinv);
105 }
106 else
107 {
108 fprintf(stderr, "Numerics, fc3d_nsgs_velocity. Not yet implemented for storageType %i\n", M->storageType);
109 exit(EXIT_FAILURE);
110 }
111 if(options->numberOfInternalSolvers < 1)
112 {
113 numerics_error("fc3d_nsgs_velocity", "The NSGS method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
114 }
115
116
117 SolverOptions * localsolver_options = options->internalSolvers[0];
118
119 /* Connect local solver */
120
121
122 FrictionContactProblem* localproblem = 0;
123 fc3d_nsgs_initialize_local_solver_velocity(&local_solver, &freeSolver, &computeError, problem, localproblem, localsolver_options);
124
125 /***** NSGS_VELOCITY Iterations *****/
126 int iter = 0; /* Current iteration number */
127 double error = 1.; /* Current error */
128 int hasNotConverged = 1;
129 int contact; /* Number of the current row of blocks in M */
130
131
132
133 dparam[SICONOS_DPARAM_TOL] = dparam[2]; // set the tolerance for the local solver
134 while((iter < itermax) && (hasNotConverged > 0))
135 {
136 ++iter;
137 /* Loop through the contact points */
138 //cblas_dcopy( n , q , incx , velocity , incy );
139 for(contact = 0 ; contact < nc ; ++contact)
140 {
141
142 (*local_solver)(localproblem, velocity, localsolver_options);
143 for(int ncc = 0; ncc < 3; ncc ++)
144 {
145 printf("velocity[%i]=%14.7e\t", ncc, velocity[contact * 3 + ncc]);
146 }
147 printf("\n");
148 }
149
150
151
152 /* **** Criterium convergence **** */
153 (*computeError)(problem, reaction, velocity, tolerance, options, norm_q, &error);
154
155 if(verbose > 0)
156 printf("--------------- FC3D - NSGS_VELOCITY - Iteration %i Residual = %14.7e\n", iter, error);
157
158 if(error < tolerance) hasNotConverged = 0;
159 *info = hasNotConverged;
160 }
161 printf("--------------- FC3D - NSGS_VELOCITY - # Iteration %i Final Residual = %14.7e\n", iter, error);
162 dparam[SICONOS_DPARAM_TOL] = tolerance;
163 dparam[SICONOS_DPARAM_RESIDU] = error;
164 iparam[7] = iter;
165
166 /***** Free memory *****/
167 (*freeSolver)();
168 }
169
fc3d_nsgs_velocity_set_default(SolverOptions * options)170 void fc3d_nsgs_velocity_set_default(SolverOptions* options)
171 {
172 assert(options->numberOfInternalSolvers == 1);
173 options->internalSolvers[0] = solver_options_create(SICONOS_FRICTION_3D_ONECONTACT_NSN);
174 }
175