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 malloc, exit, free
21 #include <string.h>
22 #include "MixedLinearComplementarityProblem.h" // for MixedLinearComplement...
23 #include "LinearComplementarityProblem.h" // for LinearComplement...
24 #include "NumericsMatrix.h" // for NM_...
25 #include "numerics_verbose.h" // for verbose */
26 #include "mlcp_to_lcp.h" // for mlcp_to_lcp
27
28 /* #define DEBUG_NOCOLOR */
29 /* #define DEBUG_STDOUT */
30 /* #define DEBUG_MESSAGES */
31 #include "siconos_debug.h"
32
mlcp_to_lcp(MixedLinearComplementarityProblem * problem)33 LinearComplementarityProblem* mlcp_to_lcp(MixedLinearComplementarityProblem* problem)
34 {
35
36 if(!problem->isStorageType2)
37 {
38 //mixedLinearComplementarity_display(problem);
39 numerics_printf_verbose(0,"mlcp_pgs: Wrong Storage (!isStorageType2) for PGS solver\n");
40 MixedLinearComplementarityProblem* mlcp_abcd = mixedLinearComplementarity_fromMtoABCD(problem);
41 //mixedLinearComplementarity_display(mlcp_abcd);
42 problem = mlcp_abcd;
43 //exit(EXIT_FAILURE);
44 }
45
46 int n =problem->n;
47 int m =problem->m;
48
49 /* solve A^{-1} C */
50
51
52 double * A_copy = (double* )calloc(n*n,sizeof(double));
53 memcpy(A_copy, problem->A, n*n*sizeof(double)); /* we preserve the original A */
54 NumericsMatrix * A = NM_new();
55 NM_fill(A, NM_DENSE, n, n, A_copy);
56 double * AinvC = (double* )calloc(n*m,sizeof(double));
57 memcpy(AinvC, problem->C, n*m*sizeof(double));
58 int info = NM_gesv_expert_multiple_rhs(A, AinvC, m, NM_KEEP_FACTORS);
59 DEBUG_PRINTF("GESV info : = %i \n", info);
60 if (info)
61 {
62 numerics_printf_verbose(0,"mlcp_to_lcp: NM_gesv_expert_multiple_rhs failed");
63 free(AinvC);
64 return NULL;
65 }
66
67 /* solve A^{-1} a */
68 double * Ainva = (double* )calloc(n,sizeof(double));
69 memcpy(Ainva, problem->a, n*sizeof(double));
70 NM_gesv_expert(A, Ainva, NM_KEEP_FACTORS);
71
72 LinearComplementarityProblem* lcp= newLCP();
73
74 lcp->size = m;
75 lcp->M = NM_new();
76 double * Mdata = (double* )calloc(m*m,sizeof(double));
77 NM_fill(lcp->M, NM_DENSE, m, m, Mdata);
78
79 lcp->q = (double* )calloc(m,sizeof(double));
80
81 /* compute B-DA^{-1}C */
82 memcpy(lcp->M->matrix0, problem->B, m*m*sizeof(double));
83 DEBUG_EXPR(NM_display(lcp->M));
84
85 NumericsMatrix * D = NM_new();
86 NM_fill(D, NM_DENSE, m, n, problem->D);
87 DEBUG_EXPR(NM_display(D));
88
89 NumericsMatrix * nm_AinvC = NM_new();
90 NM_fill(nm_AinvC, NM_DENSE, n, m, AinvC);
91 NM_gemm(-1.0, D, nm_AinvC, 1.0, lcp->M );
92
93 DEBUG_EXPR(NM_display(lcp->M));
94
95 /* compute b - DA^{-1}a */
96 memcpy(lcp->q, problem->b, m*sizeof(double));
97 NM_gemv(-1.0, D, Ainva, 1.0, lcp->q);
98
99
100
101 NM_clear(A);
102 D->matrix0=NULL;
103 NM_clear(D);
104
105 NM_clear(nm_AinvC);
106 return lcp;
107 }
108