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 "pinv.h"
19 #include <stdlib.h> // for free, malloc
20 #include "NSSTools.h" // for min
21 #include "SiconosBlas.h" // for cblas_dgemm, CblasColMajor, CblasNoTrans
22 #include "SiconosLapack.h" // for DGESVD, lapack_int
23
24 /**
25 * n : row number
26 * m : col number
27 * Warning n correspond to M in the LAPACK routine, and m to N.
28
29 This routine computes the pseudo inverse of A and returns its conditionning.
30
31 */
pinv(double * A,int n,int m,double tolerance)32 double pinv(double * A, int n, int m, double tolerance)
33 {
34 int dimS = min(n,m);
35 double * S = (double*)malloc(dimS * sizeof(double));
36 int LDU = n;
37 double *U = (double*)malloc(LDU * n * sizeof(double));
38 int LDVT = m;
39 double *VT = (double*)malloc(LDVT * m * sizeof(double));
40 lapack_int InfoDGSVD = -1;
41 double * superb = (double*)malloc((min(m, n) - 1)*sizeof(double));
42 char JOBU = 'A', JOBVT = 'A';
43 DGESVD(JOBU, JOBVT, n, m, A, n, S, U, LDU, VT, LDVT, superb, &InfoDGSVD);
44
45 double conditioning = S[0] / S[dimS - 1];
46 int rank = 0;
47 for(int i = 0; i < dimS ; i++)
48 {
49 if(S[i] > tolerance)
50 {
51 rank ++;
52 S[i] = 1.0 / S[i];
53 }
54 }
55
56 /*Compute the pseudo inverse */
57 /* Costly version with full DGEMM*/
58 double * Utranstmp = (double*)malloc(n * m * sizeof(double));
59 for(int i = 0; i < dimS; i++)
60 {
61 for(int j = 0; j < n; j++)
62 {
63 Utranstmp[i + j * m] = S[i] * U[j + i * n];
64 }
65 }
66 for(int i = dimS; i < m; i++)
67 {
68 for(int j = 0; j < n; j++)
69 {
70 Utranstmp[i + j * m] = 0.0;
71 }
72 }
73
74 cblas_dgemm(CblasColMajor,CblasTrans, CblasNoTrans, m, n, m, 1.0, VT, m, Utranstmp, m, 0.0, A, m);
75 /* for (int i = 0; i < n; i++){ */
76 /* for (int j = 0; j < n; j++) */
77 /* { */
78 /* U[j+i*n] = S[i]*U[j+i*n]; */
79 /* } */
80 /* } */
81
82 /* for (int i = 0; i < rank; i++){ */
83 /* for (int j = 0; j < m; j++) */
84 /* { */
85
86 /* A[i+j*n] =0.0; */
87 /* for (int k = 0; k < rank; k++){ */
88 /* A[i+j*n] += VT[k+i*n]*U[j+k*n]; */
89 /* } */
90 /* } */
91 /* } */
92
93 free(U);
94 free(VT);
95 free(Utranstmp);
96 free(S);
97 free(superb);
98 return conditioning;
99 }
100