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 #define NO_LEXICO_MAT
20 #define WARN_ONLY_SMALL_PIVOT
21 #define LEXICO_TOL 1e3*DBL_EPSILON
22
23 #include <assert.h> // for assert
24 #include <float.h> // for DBL_EPSILON
25 #include <limits.h> // for INT_MAX
26 #include <math.h> // for fabs
27 #include <stdio.h> // for printf, NULL
28 #include <stdlib.h> // for free, malloc, calloc, exit
29 #include <string.h> // for memset
30 #include "LCP_Solvers.h" // for lcp_compute_error, lcp_piv...
31 #include "LinearComplementarityProblem.h" // for LinearComplementarityProblem
32 #include "NumericsFwd.h" // for LinearComplementarityProblem
33 #include "NumericsMatrix.h" // for NumericsMatrix
34 #include "SolverOptions.h" // for SolverOptions, SICONOS_IPA...
35 //#define DEBUG_STDOUT
36 //#define DEBUG_MESSAGES
37 //#define DEBUG_NO_MATRIX
38 #include "siconos_debug.h" // for DEBUG_EXPR_WE, DEBUG_PRINT
39 #include "lcp_cst.h" // for SICONOS_LCP_PIVOT_LEMKE
40 #include "lcp_pivot.h" // for LCP_PATHSEARCH_LEAVING_T
41 #include "lumod_wrapper.h" // for SN_lumod_dense_solve, SN_l...
42 #include "numerics_verbose.h" // for verbose
43 #include "pivot-utils.h" // for do_pivot_lumod
44 #include "SiconosBlas.h" // for cblas_dcopy, cblas_daxpy
45
46 #define BASIS_OFFSET 1
47
48 DEBUG_GLOBAL_VAR_DECL(unsigned * basis_global;);
49
get_q_tilde(double * mat,unsigned n)50 inline static double* get_q_tilde(double* mat, unsigned n)
51 {
52 return mat;
53 }
54
get_driving_col(double * mat,unsigned n)55 inline static double* get_driving_col(double* mat, unsigned n)
56 {
57 return &mat[n];
58 }
59
get_lexico_mat(double * mat,unsigned n)60 inline static double* get_lexico_mat(double* mat, unsigned n)
61 {
62 return &mat[2*n];
63 }
64
get_col_tilde(double * mat,unsigned n)65 inline static double* get_col_tilde(double* mat, unsigned n)
66 {
67 return &mat[(n+2)*n];
68 }
69
get_cov_vec(double * mat,unsigned n)70 inline static double* get_cov_vec(double* mat, unsigned n)
71 {
72 return &mat[(n+3)*n];
73 }
74
lcp_pivot_lumod(LinearComplementarityProblem * problem,double * u,double * s,int * info,SolverOptions * options)75 void lcp_pivot_lumod(LinearComplementarityProblem* problem, double* u, double* s, int *info, SolverOptions* options)
76 {
77 lcp_pivot_lumod_covering_vector(problem, u, s, info, options, NULL);
78 }
79
80
lcp_pivot_lumod_covering_vector(LinearComplementarityProblem * problem,double * restrict u,double * restrict s,int * info,SolverOptions * options,double * restrict cov_vec)81 void lcp_pivot_lumod_covering_vector(LinearComplementarityProblem* problem, double* restrict u, double* restrict s, int *info, SolverOptions* options, double* restrict cov_vec)
82 {
83 /* matrix M of the LCP */
84 assert(problem);
85 assert(problem->M);
86 double* M = problem->M->matrix0;
87 assert(M);
88 assert(problem->q);
89
90
91 unsigned int dim = problem->size;
92 assert(dim>0);
93 /* unsigned int dim2; */
94 /* size of the LCP */
95 DEBUG_EXPR_WE(DEBUG_PRINT("matrix M: ") NM_display(problem->M); DEBUG_PRINT("vector q: ")
96 for(unsigned i = 0; i < dim; ++i)
97 {
98 printf("%e ", problem->q[i]);
99 }
100 printf("\n");
101 if(cov_vec)
102 {
103 DEBUG_PRINT("covering vector: ") for(unsigned i = 0; i < dim; ++i)
104 {
105 printf("%e ", cov_vec[i]);
106 }
107 printf("\n");
108 });
109
110 unsigned drive = dim+1;
111 int bck_drive = -1;
112 int block = -1;
113 unsigned has_sol = 0;
114 unsigned nb_iter = 0;
115 unsigned leaving = 0;
116 unsigned itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
117 unsigned preAlloc = options->iparam[SICONOS_IPARAM_PREALLOC];
118 unsigned pivot_selection_rule = options->iparam[SICONOS_LCP_IPARAM_PIVOTING_METHOD_TYPE];
119
120 assert(itermax > 0 && "lcp_pivot_lumod_covering_vector itermax == 0, the algorithm will not run");
121 double pivot;
122 double tmp;
123 unsigned* basis = (unsigned*) malloc(dim*sizeof(unsigned));
124 DEBUG_EXPR_WE(basis_global = basis;);
125 unsigned* candidate_indx = (unsigned*) malloc(dim*sizeof(unsigned));
126 int basis_init = 0; /* 0 if basis was not initialized, 1 otherwise*/
127 unsigned t_indx = 0;
128 unsigned aux_indx = 0;
129 #if 0
130 double* t_stack = NULL;
131 #endif
132 /* This matrix contains q, the solution to the linear system Hk x = driving_col,
133 * the matrix for the lexicographic ordering and the solution to the linear
134 * system H x = driving_col. */
135 double* mat = (double*) calloc((dim+4)*dim, sizeof(double)); /* XXX memory save */
136 assert(problem->q);
137 cblas_dcopy(dim, problem->q, 1, get_q_tilde(mat, dim), 1);
138
139 if(cov_vec)
140 {
141 cblas_dcopy(dim, cov_vec, 1, get_cov_vec(mat, dim), 1);
142 }
143 else
144 {
145 double* d = get_cov_vec(mat, dim);
146 for(unsigned i = 0; i < dim; ++i) d[i] = 1.;
147 }
148
149 /* Init the lexicographic mat */
150 double* lexico_mat = get_lexico_mat(mat, dim);
151 for(unsigned i = 0; i < dim*dim; i += dim+1) lexico_mat[i] = 1.;
152 DEBUG_PRINT_MAT_ROW_MAJOR_NCOLS_SMALL_STR("lexico_mat", lexico_mat, dim, dim, dim);
153
154 /* Maximum number of columns changed in the matrix */
155 /* TODO: user settable and should not be bigger than the size of the matrix? */
156 unsigned maxmod = 50;
157
158 *info = 0;
159
160 /*output*/
161 options->iparam[SICONOS_IPARAM_ITER_DONE] = 0;
162
163 /* Allocation */
164 SN_lumod_dense_data* lumod_data = SN_lumod_dense_allocate(dim, maxmod);
165
166 /* switch (pivot_selection_rule) */
167 /* { */
168 /* /\* case SICONOS_LCP_PIVOT_BARD: */
169 /* dim2 = 2*dim + 1; */
170 /* break; */
171 /* case SICONOS_LCP_PIVOT_LEAST_INDEX: */
172 /* dim2 = dim + 1; */
173 /* break;*\/ */
174 /* case SICONOS_LCP_PIVOT_LEMKE: */
175 /* case SICONOS_LCP_PIVOT_PATHSEARCH: */
176 /* default: */
177 /* dim2 = 2 * (dim + 1); */
178 /* } */
179
180
181 /* Init basis if necessary */
182 if(!basis_init)
183 {
184 for(unsigned i = 0 ; i < dim ; ++i) basis[i] = i + 1;
185 }
186
187 /* Looking for pivot */
188 switch(pivot_selection_rule)
189 {
190 /* case SICONOS_LCP_PIVOT_BARD:
191 block = pivot_selection_bard(mat, dim);
192 drive = block + dim + 1;
193 break;
194 case SICONOS_LCP_PIVOT_LEAST_INDEX:
195 block = pivot_selection_least_index(mat, dim);
196 drive = block + 1;
197 break;
198 case SICONOS_LCP_PIVOT_PATHSEARCH:
199 block = pivot_init_pathsearch(dim, mat, &t_indx);
200 break;*/
201 case SICONOS_LCP_PIVOT_LEMKE:
202 default:
203 // block = pivot_init_lemke(get_q_tilde(mat, dim), dim);
204 block = pivot_selection_lemke2(dim, get_cov_vec(mat, dim), get_q_tilde(mat, dim), get_lexico_mat(mat, dim), INT_MAX, LEXICO_TOL);
205 }
206
207 if(block < 0)
208 {
209 if(block == -1)
210 {
211 /** exit, the solution is at hand with the current basis */
212 DEBUG_PRINT("Trivial solution\n");
213 goto exit_lcp_pivot;
214 }
215 #if 0
216 else if(block == PIVOT_PATHSEARCH_SUCCESS)
217 {
218 assert(pivot_selection_rule == SICONOS_LCP_PIVOT_PATHSEARCH);
219 DEBUG_PRINTF("lcp_pivot :: path search successful ! t_indx = %d\n", t_indx);
220 bck_drive = t_indx; /* XXX correct ? */
221 t_stack[nb_iter%stack_size] = 1.0;
222 double pivot = 1.0; /* force value of pivot to avoid numerical issues */
223 for(unsigned int i = 0; i < dim; ++i) mat[i] -= mat[i + drive*dim]*pivot;
224 *info = 0;
225 goto exit_lcp_pivot;
226 }
227 else if(block == -LCP_PATHSEARCH_NON_ENTERING_T)
228 {
229 /* exit, t could not become basic */
230 assert(pivot_selection_rule == SICONOS_LCP_PIVOT_PATHSEARCH);
231 DEBUG_PRINT("lcp_pivot :: t could not become basic, exiting\n");
232 *info = LCP_PATHSEARCH_NON_ENTERING_T;
233 goto exit_lcp_pivot;
234 }
235 #endif
236 }
237
238 /* save the position of the auxiliary variable */
239 assert(block >= 0);
240 aux_indx = block;
241
242 /* Update the basis */
243 switch(pivot_selection_rule)
244 {
245 #if 0
246 /* Principal Pivoting Methods */
247 case SICONOS_LCP_PIVOT_BARD:
248 basis[block] = basis[block] <= dim ? block + dim + 2 : block + 1;
249 break;
250 case SICONOS_LCP_PIVOT_LEAST_INDEX:
251 basis[block] = basis[block] <= dim ? block + dim + 2 : block + 1;
252 break;
253 case SICONOS_LCP_PIVOT_PATHSEARCH:
254 DEBUG_PRINTF("t value : %le\n", mat[t_indx]);
255 #endif
256 case SICONOS_LCP_PIVOT_LEMKE:
257 default:
258 /** one basic u is leaving and mu enters the basis */
259 leaving = basis[block];
260 basis[block] = drive;
261 }
262
263 /* Init the LUMOD data and perform the pivot < aux_variable , drive > */
264 switch(pivot_selection_rule)
265 {
266 /* case SICONOS_LCP_PIVOT_BARD:
267 init_M_bard(mat, M, dim, problem->q);
268 break;
269 case SICONOS_LCP_PIVOT_LEAST_INDEX:
270 init_M_least_index(mat, M, dim, problem->q);
271 break;
272 case SICONOS_LCP_PIVOT_PATHSEARCH:
273 init_M_lemke_warm_start(dim, u, mat, M, problem->q, basis, cov_vec);
274 basis_init = 1;
275 DEBUG_EXPR_WE( DEBUG_PRINT("basis after hot start: ")
276 for (unsigned int i = 0; i < dim; ++i)
277 { DEBUG_PRINTF("%i ", basis[i])}
278 DEBUG_PRINT("\n"));
279
280 break;*/
281 case SICONOS_LCP_PIVOT_LEMKE:
282 default:
283 SN_lumod_factorize(lumod_data, basis, problem->M, get_cov_vec(mat, dim));
284 pivot = get_cov_vec(mat, dim)[block];
285 }
286 DEBUG_PRINT("lcp_pivot: init done, starting resolution\n");
287
288 /* XXX Maybe we should compute theta = q_i/pivot */
289 if(fabs(pivot) < DBL_EPSILON)
290 {
291 if(verbose > 0)
292 printf("the pivot is quasi-nul %e, the algorithm cannot be used !\n", pivot);
293 #ifndef WARN_ONLY_SMALL_PIVOT
294 *info = LCP_PIVOT_NUL;
295 goto exit_lcp_pivot;
296 #endif
297 }
298
299 /* Update q
300 * XXX maybe this code should go to pivot-utils*/
301 {
302 double* q = get_q_tilde(mat, dim);
303 double theta = -q[block]/pivot;
304 cblas_daxpy(dim, theta, get_cov_vec(mat, dim), 1, q, 1);
305 q[block] = theta;
306
307 unsigned block_row_indx = block*dim;
308 for(unsigned i = 0, j = 0; i < dim; ++i, j += dim)
309 {
310 if(j == block_row_indx) continue;
311 cblas_daxpy(dim, -get_cov_vec(mat, dim)[i]/pivot, &lexico_mat[block_row_indx], 1, &lexico_mat[j], 1);
312 }
313 cblas_dscal(dim, -1./pivot, &lexico_mat[block_row_indx], 1);
314 DEBUG_PRINT_MAT_ROW_MAJOR_NCOLS_SMALL2_STR("lexico_mat", lexico_mat, dim, dim, dim, get_cov_vec(mat, dim));
315 }
316 DEBUG_PRINT_VEC(get_q_tilde(mat, dim), dim);
317
318
319 DEBUG_EXPR_WE(DEBUG_PRINT("new basis: ")
320 for(unsigned int i = 0; i < dim; ++i)
321 {
322 DEBUG_PRINTF("%i ", basis[i])
323 }
324 DEBUG_PRINT("\n"));
325
326 while(nb_iter < itermax && !has_sol)
327 {
328
329 ++nb_iter;
330 /* Prepare the search for leaving variable */
331 double* driving_col = get_driving_col(mat, dim);
332 if(leaving < dim + BASIS_OFFSET) /* the leaving variable is w_i -> the driving variable is z_i */
333 {
334 drive = leaving + dim + BASIS_OFFSET;
335 cblas_dcopy(dim, &M[dim*(leaving-BASIS_OFFSET)], 1, driving_col, 1);
336 }
337 else if(leaving > dim + BASIS_OFFSET) /* the leaving variable is z_i -> the driving variable is w_i */
338 {
339 drive = leaving - (dim + BASIS_OFFSET);
340 memset(driving_col, 0, sizeof(double) * dim);
341 driving_col[drive - BASIS_OFFSET] = -1.;
342 }
343 else
344 {
345 printf("lcp_pivot_lumod the leaving variable is the auxiliary variable; we should not execute those lines!\n");
346 exit(EXIT_FAILURE);
347 }
348 DEBUG_EXPR_WE(DEBUG_PRINT("basis= "); for(unsigned i = 0; i < dim; ++i)
349 {
350 DEBUG_PRINTF("%s%d ", basis_to_name(basis[i], dim), basis_to_number(basis[i], dim));
351 }
352 DEBUG_PRINT("\n"));
353 int solve_info = SN_lumod_dense_solve(lumod_data, driving_col, get_col_tilde(mat, dim));
354 if(SN_lumod_need_refactorization(solve_info))
355 {
356 DEBUG_PRINT("Refactorizing!\n");
357 SN_lumod_factorize(lumod_data, basis, problem->M, get_cov_vec(mat, dim));
358 if(leaving < dim + BASIS_OFFSET) /* the leaving variable is w_i -> the driving variable is z_i */
359 {
360 drive = leaving + dim + BASIS_OFFSET;
361 cblas_dcopy(dim, &M[dim*(leaving-BASIS_OFFSET)], 1, driving_col, 1);
362 }
363 else if(leaving > dim + BASIS_OFFSET) /* the leaving variable is z_i -> the driving variable is w_i */
364 {
365 drive = leaving - (dim + BASIS_OFFSET);
366 memset(driving_col, 0, sizeof(double) * dim);
367 assert(drive >= BASIS_OFFSET);
368 driving_col[drive - BASIS_OFFSET] = -1.;
369 }
370 solve_info = SN_lumod_dense_solve(lumod_data, driving_col, get_col_tilde(mat, dim));
371 }
372 if(solve_info != 0)
373 {
374 printf("lcp_pivot_lumod :: SN_lumod_dense_solve failed!, info = %d", solve_info);
375 *info = LCP_PIVOT_LUMOD_FAILED;
376 goto exit_lcp_pivot;
377 }
378
379
380 /* Start research of argmin lexico for minimum ratio test */
381
382 /* Looking for pivot */
383 switch(pivot_selection_rule)
384 {
385 /* case SICONOS_LCP_PIVOT_BARD:
386 block = pivot_selection_bard(mat, dim);
387 drive = block + dim + 1;
388 break;
389 case SICONOS_LCP_PIVOT_LEAST_INDEX:
390 block = pivot_selection_least_index(mat, dim);
391 drive = block + 1;
392 break;
393 case SICONOS_LCP_PIVOT_PATHSEARCH:
394 if (leaving < dim + 1)
395 {
396 drive = leaving + dim + 1;
397 }
398 else if (leaving > dim + 1)
399 {
400 drive = leaving - (dim + 1);
401 }
402 else // XXX oulalla
403 {
404 assert(0 && "leaving variable is t");
405 drive = dim + 1;
406 }
407 block = pivot_selection_pathsearch(mat, dim, drive, t_indx);
408 break;*/
409 case SICONOS_LCP_PIVOT_LEMKE:
410 default:
411 #ifndef NO_LEXICO_MAT
412 block = pivot_selection_lemke2(dim, get_driving_col(mat, dim), get_q_tilde(mat, dim), get_lexico_mat(mat, dim), aux_indx, LEXICO_TOL);
413 #else
414 block = pivot_selection_lemke3(dim, get_driving_col(mat, dim), get_q_tilde(mat, dim), get_lexico_mat(mat, dim), basis, candidate_indx, lumod_data, aux_indx, LEXICO_TOL);
415 #endif
416 }
417
418 DEBUG_PRINTF("leaving variable %s%d entering variable %s%d\n", basis_to_name(basis[block], dim), basis_to_number(basis[block], dim), basis_to_name(drive, dim), basis_to_number(drive, dim));
419
420 if(block < 0)
421 {
422
423 /* We stop here: it either mean that the algorithm stops here or that there
424 * is an issue with the LCP */
425 if(block == -1)
426 {
427 switch(pivot_selection_rule)
428 {
429 case SICONOS_LCP_PIVOT_LEMKE:
430 case SICONOS_LCP_PIVOT_PATHSEARCH:
431 *info = LCP_PIVOT_RAY_TERMINATION;
432 DEBUG_PRINT("The pivot column is nonpositive ! We are on ray !\n"
433 "It either means that the algorithm failed or that the LCP is infeasible\n"
434 "Check the class of the M matrix to find out the meaning of this\n");
435 default:
436 bck_drive = drive < dim + 1 ? drive - 1 : drive - dim - 2;
437 }
438 break;
439 }
440 #if 0
441 /* path search was successful, t = 1, we need to update the value of the
442 * basic variable, but we are done here :) */
443 else if(block == PIVOT_PATHSEARCH_SUCCESS)
444 {
445 assert(pivot_selection_rule == SICONOS_LCP_PIVOT_PATHSEARCH);
446 DEBUG_PRINTF("lcp_pivot :: path search successful ! t_indx = %d\n", t_indx);
447 basis[t_indx] = drive;
448 t_stack[nb_iter%stack_size] = 1.0;
449 double pivot = (mat[t_indx] - 1.0)/mat[t_indx + drive*dim];
450 for(unsigned int i = 0; i < dim; ++i) mat[i] -= mat[i + drive*dim]*pivot;
451 mat[t_indx] = pivot;
452 *info = 0;
453 break;
454 }
455 #endif
456
457 }
458
459 if(basis[block] == dim + 1)
460 {
461 if(pivot_selection_rule != SICONOS_LCP_PIVOT_PATHSEARCH)
462 {
463 has_sol = 1;
464 }
465 else
466 {
467 DEBUG_PRINT("t variable leaving !\n");
468 *info = LCP_PATHSEARCH_LEAVING_T;
469 bck_drive = drive < dim + 1 ? drive - 1 : drive - dim - 2;
470 options->dparam[2] = mat[t_indx];
471 goto exit_lcp_pivot;
472 }
473 }
474
475
476 /* Pivot < block , drive > */
477 DEBUG_PRINTF("Pivoting variable at pos %d in basis (%s%d) and (%s%d)\n", block, basis_to_name(basis[block], dim), basis_to_number(basis[block], dim), basis_to_name(drive, dim), basis_to_number(drive, dim));
478
479 pivot = get_driving_col(mat, dim)[block];
480 if(fabs(pivot) < DBL_EPSILON)
481 {
482 if(verbose > 0)
483 printf("the pivot is quasi-nul %e, danger !\nq[block] = %e; z = %e\n", pivot, get_q_tilde(mat, dim)[block], get_q_tilde(mat, dim)[block]/pivot);
484 #ifndef WARN_ONLY_SMALL_PIVOT
485 *info = LCP_PIVOT_NUL;
486 goto exit_lcp_pivot;
487 #endif
488 }
489
490 /* update matrix and q*/
491 switch(pivot_selection_rule)
492 {
493 /* case SICONOS_LCP_PIVOT_BARD:
494 do_pivot_driftless(mat, dim, dim2, block, drive);
495 break;
496 case SICONOS_LCP_PIVOT_LEAST_INDEX:
497 do_pivot(mat, dim, dim2, block, drive);
498 break;*/
499 case SICONOS_LCP_PIVOT_LEMKE:
500 case SICONOS_LCP_PIVOT_PATHSEARCH:
501 default:
502 do_pivot_lumod(lumod_data, problem->M, get_q_tilde(mat, dim), get_lexico_mat(mat, dim), get_driving_col(mat, dim), get_col_tilde(mat, dim), basis, block, drive);
503 }
504 DEBUG_PRINT_VEC(get_q_tilde(mat, dim), dim);
505
506 /* determine leaving variable and update basis */
507 switch(pivot_selection_rule)
508 {
509 #if 0
510 /* Principal Pivoting Methods */
511 case SICONOS_LCP_PIVOT_BARD:
512 basis[block] = basis[block] <= dim ? block + dim + 2 : block + 1;
513 break;
514 case SICONOS_LCP_PIVOT_LEAST_INDEX:
515 basis[block] = basis[block] <= dim ? block + dim + 2 : block + 1;
516 break;
517 case SICONOS_LCP_PIVOT_PATHSEARCH:
518 leaving = basis[block];
519 //memcpy(basis, basis+dim, dim*sizeof(int));
520 //basis += dim;
521 basis[block] = drive;
522 t_stack[nb_iter%stack_size] = mat[t_indx];
523 DEBUG_PRINTF("t value : %2.2e\n", mat[t_indx]);
524 DEBUG_PRINTF("t-1.0 value : %2.2e\n", mat[t_indx]-1.0);
525 #endif /* XXX to test */
526 // if (fabs(mat[t_indx] -1.0) < 1e-8)
527 // {
528 // double pivot = (mat[t_indx] - 1.0)/mat[t_indx + drive*dim];
529 // for (unsigned int i = 0; i < dim; ++i) mat[i] -= mat[i + drive*dim]*pivot;
530 // mat[t_indx] = 0;
531 // *info = 0;
532 // has_sol = 1;
533 // }
534 // break;
535 case SICONOS_LCP_PIVOT_LEMKE:
536 default:
537 /** one basic variable is leaving and the driving one enters the basis */
538 leaving = basis[block];
539 basis[block] = drive;
540 }
541
542 DEBUG_PRINT_VEC_STR("basis value", get_q_tilde(mat, dim), dim);
543
544 DEBUG_EXPR_WE(DEBUG_PRINT("new basis: ")
545 for(unsigned int i = 0; i < dim; ++i)
546 {
547 DEBUG_PRINTF("%i ", basis[i])
548 }
549 DEBUG_PRINT("\n"));
550
551 } /* end while*/
552
553 exit_lcp_pivot:
554
555 DEBUG_EXPR_WE(DEBUG_PRINT("final basis: ")
556 for(unsigned int i = 0; i < dim; ++i)
557 {
558 DEBUG_PRINTF("%i ", basis[i])
559 }
560 DEBUG_PRINT("\n"));
561
562 /* Recover solution */
563 double* finalq = get_q_tilde(mat, dim);
564 for(unsigned int i = 0 ; i < dim; ++i)
565 {
566 drive = basis[i];
567 assert(drive > 0);
568 //assert(drive != dim + 1);
569 if(drive < dim + 1)
570 {
571 u[drive - 1] = 0.0;
572 s[drive - 1] = finalq[i];
573 }
574 else if(drive > dim + 1)
575 {
576 u[drive - dim - 2] = finalq[i];
577 s[drive - dim - 2] = 0.0;
578 }
579 else
580 {
581 if(nb_iter < itermax)
582 {
583 assert(bck_drive >= 0);
584 u[bck_drive] = 0.0;
585 s[bck_drive] = 0.0;
586 }
587 }
588
589 }
590 /* End recover solution */
591
592 DEBUG_PRINT("u s\n");
593 DEBUG_EXPR_WE(for(unsigned int i = 0; i < dim; ++i)
594 {
595 DEBUG_PRINTF("%e %e\n", u[i], s[i])
596 });
597
598 options->iparam[SICONOS_IPARAM_ITER_DONE] = nb_iter;
599
600 /* update info */
601 switch(pivot_selection_rule)
602 {
603 /* Principal Pivoting Methods */
604 case SICONOS_LCP_PIVOT_BARD:
605 case SICONOS_LCP_PIVOT_LEAST_INDEX:
606 *info = lcp_compute_error(problem, u, s, options->dparam[SICONOS_DPARAM_TOL], &tmp);
607 break;
608 case SICONOS_LCP_PIVOT_PATHSEARCH:
609 break; /* info should already be set */
610 case SICONOS_LCP_PIVOT_LEMKE:
611 default:
612 /* info may already be set*/
613 if(*info == 0)
614 {
615 if(has_sol) *info = 0;
616 else *info = 1;
617 }
618 }
619
620 if(*info > 0)
621 {
622 DEBUG_PRINT("No solution found !\n");
623 }
624
625 if(!preAlloc)
626 {
627 free(basis);
628 free(mat);
629 SM_lumod_dense_free(lumod_data);
630 }
631
632 /* XXX Clean that --xhub */
633 free(candidate_indx);
634 }
635
lcp_pivot_lumod_set_default(SolverOptions * options)636 void lcp_pivot_lumod_set_default(SolverOptions* options)
637 {
638 options->iparam[SICONOS_LCP_IPARAM_PIVOTING_METHOD_TYPE] = SICONOS_LCP_PIVOT_LEMKE;
639 }
640