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 NULL, fprintf, stderr
20 #include <stdlib.h> // for exit, EXIT_FAILURE
21 #include "Friction_cst.h" // for SICONOS_FRICTION_3D_NCP...
22 #include "NCP_Solvers.h" // for ncp_path
23 #include "NonlinearComplementarityProblem.h" // for NonlinearComplementarit...
24 #include "NumericsFwd.h" // for FrictionContactProblem
25 #include "SolverOptions.h" // for SolverOptions
26 #include "fc3d_2NCP_Glocker.h" // for computeFGlocker, NCPGlo...
27 #include "fc3d_NCPGlockerFixedPoint.h" // for fc3d_Path_computeError
28 #include "SiconosBlas.h" // for cblas_dcopy
29
30 #pragma GCC diagnostic ignored "-Wmissing-prototypes"
31
32 /* Pointer to function used to update the solver, to formalize the local problem for example. */
33 typedef void (*UpdateSolverPtr)(int, double*);
34
35
36 /* size of a block */
37
38 /** writes \f$ F(z) \f$ using Glocker formulation
39 */
F_GlockerPath(void * env,int sizeF,double * reaction,double * FVector)40 void F_GlockerPath(void* env, int sizeF, double* reaction, double* FVector)
41 {
42 /* Glocker formulation */
43 int up2Date = 0;
44 double* FGlocker = NULL;
45 computeFGlocker(&FGlocker, up2Date);
46 /* Note that FGlocker is a static var. in fc3d2NCP_Glocker and thus there is no memory allocation in
47 the present file.
48 */
49
50 /* TMP COPY: review memory management for FGlocker ...*/
51 cblas_dcopy(sizeF, FGlocker, 1, FVector, 1);
52 FGlocker = NULL;
53 }
54
55 /** writes \f$ \nabla_z F(z) \f$ using Glocker formulation and the Fischer-Burmeister function.
56 */
jacobianF_GlockerPath(void * env,int sizeF,double * reaction,NumericsMatrix * jacobianFMatrix)57 void jacobianF_GlockerPath(void* env, int sizeF, double* reaction, NumericsMatrix* jacobianFMatrix)
58 {
59 int up2Date = 0;
60 /* Glocker formulation */
61 double* FGlocker = NULL, *jacobianFGlocker = NULL;
62 computeFGlocker(&FGlocker, up2Date);
63 computeJacobianFGlocker(&jacobianFGlocker, up2Date);
64 /* Note that FGlocker and jacobianFGlocker are static var. in fc3d2NCP_Glocker and thus there is no memory allocation in
65 the present file.
66 */
67
68 FGlocker = NULL;
69 jacobianFGlocker = NULL;
70 }
71
72
fc3d_Path_initialize(FrictionContactProblem * problem,FrictionContactProblem * localproblem,SolverOptions * localsolver_options)73 void fc3d_Path_initialize(FrictionContactProblem* problem, FrictionContactProblem* localproblem, SolverOptions * localsolver_options)
74 {
75
76 /*
77 Initialize solver (Connect F and its jacobian, set local size ...) according to the chosen formulation.
78 */
79
80 /* Glocker formulation */
81 if(localsolver_options->solverId == SICONOS_FRICTION_3D_NCPGlockerFBPATH)
82 {
83 NCPGlocker_initialize(problem, localproblem);
84 }
85 else
86 {
87 fprintf(stderr, "Numerics, fc3d_Path failed. Unknown formulation type.\n");
88 exit(EXIT_FAILURE);
89 }
90 }
91
fc3d_Path_solve(FrictionContactProblem * localproblem,double * reaction,SolverOptions * options)92 int fc3d_Path_solve(FrictionContactProblem * localproblem, double* reaction, SolverOptions * options)
93 {
94
95 NonlinearComplementarityProblem NCP_struct =
96 {
97 5,
98 &F_GlockerPath,
99 &jacobianF_GlockerPath,
100 NULL,
101 NULL
102 };
103
104 double Fvec[5];
105 int info;
106 ncp_path(&NCP_struct, reaction, Fvec, &info, options);
107 if(info > 0)
108 {
109 fprintf(stderr, "Numerics, fc3d_Path failed");
110 exit(EXIT_FAILURE);
111 }
112 return info;
113 /* (*postSolver)(contact,reaction); */
114 }
115
fc3d_Path_free()116 void fc3d_Path_free()
117 {
118 NCPGlocker_free();
119 }
120
fc3d_Path_computeError(int n,double * velocity,double * reaction,double * error)121 void fc3d_Path_computeError(int n, double* velocity, double* reaction, double * error)
122 {
123 /* int numberOfContacts = n/3; */
124 /* int sizeGlobal = numberOfContacts*FSize; */
125 /* // double * FGlobal = (double*)malloc(sizeGlobal*sizeof(*FGlobal)); */
126 /* (*computeFGlobal)(reaction,velocity); */
127 /* int i; */
128 /* double Fz; */
129 /* *error = 0; */
130 /* for(i=0;i<sizeGlobal;++i) */
131 /* { */
132 /* Fz = velocity[i]*reaction[i]; */
133 /* if(Fz>0) */
134 /* *error+=Fz; */
135 /* if(reaction[i]<0) */
136 /* *error+=reaction[i]; */
137 /* if(velocity[i]<0) */
138 /* *error+=velocity[i]; */
139 /* } */
140
141 /* // (*computeVelocity)(FGlobal); */
142
143 /* free(FGlobal); */
144
145 }
146