linear_algebra plugged.
[scilab.git] / scilab / modules / linear_algebra / src / c / qr.c
1 /*
2  * Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3  * Copyright (C) 2009 - DIGITEO - Bernard HUGUENEY 
4  *
5  * This file must be used under the terms of the CeCILL.
6  * This source file is licensed as described in the file COPYING, which
7  * you should have received as part of this distribution.  The terms
8  * are also available at
9  * http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
10  *
11  */
12 #include <string.h> /* for memset */
13 #include <stdio.h>
14
15 #include "core_math.h"
16 #include "MALLOC.h"
17 #include "doublecomplex.h"
18 #include "qr.h"
19 #include "elem_common.h"
20
21 #define WORKING_ZQUERIES 0
22
23 extern void C2F(dgeqrf)(int const * piRows, int const * piCols, double* pData, int const * pLDData
24                         , double* pdblTau, double* pdblWork, int const * piWorksize, int * piInfo);
25
26 extern void C2F(zgeqrf)(int const * piRows, int const * piCols, double* pData, int const * pLDData
27                         , double* pdblTau, double* pdblWork, int const * piWorksize, int * piInfo);
28
29
30 extern void C2F(dgeqp3)(int const * piRows, int const * piCols, double* pData, int const * pLDData
31                         , int* piPivot, double* pdblTau, double* pdblWork, int const * piWorksize, int * piInfo);
32
33 extern void C2F(zgeqp3)(int const * piRows, int const * piCols, double* pData, int const * pLDData
34                         , int* piPivot, double* pdblTau, double* pdblWork, int const * piWorksize, double* pdblRWork, int * piInfo);
35
36 extern void C2F(dorgqr)(int const * piRows, int const * piCols, int* piK, double* pData, int const * pLDData
37                         , double* pdblTau, double* pdblWork, int const * piWorksize, int * piInfo);
38
39 extern void C2F(zungqr)(int const * piRows, int const * piCols, int* piK, double* pData, int const * pLDData
40                         , double* pdblTau, double* pdblWork, int const * piWorksize, int * piInfo);
41
42 extern void C2F(dlaset)(char const * uplo /* "U"pper, "L"ower, or full*/, int const * piRows, int const * piCols
43                         , double const * pAlpha, double const * pBeta, double* pData, int const * pLDData);
44
45 extern double C2F(pythag)(double *a, double *b);
46
47 /* /!\ Performance considerations : deprecated lapack subroutines have been replaced by their
48    more recent conterparts (introduced in version 3.0 released on June 30, 1999 )
49    DGEQPF -> DGEQP3, ZGEQPF -> ZGEQP3
50 */
51
52 /* /!\ workSize must be computed in a different way for SAS because :
53    1) we cannot allocate all available memory
54    2) performance is dependant on the amount of allocated memory
55
56    So instead of claiming all available stack memory, we must perform a call on [d|z]geqrf with LWORK (=worksize)
57    to query for optimal worksize.
58    TODO: We could try a more sutble allocation stategy than falling back direclty from max(optimalWorkSizes) to max( minimalWorksizes)
59 */
60 static int optimalWorkSizes(int complexArg, int iRows, int iCols, int iRowsToCompute
61                             , int* opt1, int* opt2, int* opt3);
62 static int optimalWorkSizes(int complexArg, int iRows, int iCols, int iRowsToCompute
63                             , int* opt1, int* opt2, int* opt3)
64 {
65   double optGeqrf, optGeqp3, optGqr;
66   int info=0;
67   int query= -1;
68   int minRowsCols= Min(iRows, iCols); /* even when iRowsToCompute == iRows, [d|z]orgqr uses the min */
69   if( WORKING_ZQUERIES && complexArg) /* /!\ query mode for z lapack functions seems bugged ! */
70     {
71       C2F(zgeqrf)(&iRows, &iCols, /*unused A*/NULL, &iRows, /*unused Tau*/NULL, /*Work used for query*/ &optGeqrf, &query, &info);
72       fprintf(stderr, "info:%d\n",info);
73       info=0;
74       C2F(zgeqp3)(&iRows, &iCols, NULL, &iRows, NULL, NULL, &optGeqp3, &query, NULL, &info);
75       fprintf(stderr, "info:%d\n",info);
76       info=0;
77       C2F(zungqr)(&iRows, &iRowsToCompute, &minRowsCols, NULL, &iRows, NULL, &optGqr, &query, &info);
78       fprintf(stderr, "info:%d\n",info);
79     }
80   else
81     {
82       C2F(dgeqrf)(&iRows, &iCols, NULL, &iRows, NULL, &optGeqrf, &query, &info);
83       C2F(dgeqp3)(&iRows, &iCols, NULL, &iRows, NULL, NULL, &optGeqp3, &query, &info);
84       C2F(dorgqr)(&iRows, &iRowsToCompute, &minRowsCols, NULL, &iRows, NULL, &optGqr, &query, &info);
85     }
86   /* sort sizes in decreasing order */
87   *opt1= (int)Max(optGeqrf, optGeqp3);
88   *opt2= (int)Min(optGeqrf, optGeqp3);
89   if(optGqr > *opt1)
90     {
91       *opt3= *opt2;
92       *opt2= *opt1;
93       *opt1= (int)optGqr;
94     }
95   else
96     {
97       if(optGqr > *opt2)
98         {
99           *opt3= *opt2;
100           *opt2= (int)optGqr;
101         }
102       else
103         {
104           *opt3= (int)optGqr;
105         }
106     }
107   return info;
108 }
109
110 static int minimalWorkSize(int complexArg, int iRows, int iCols, int iRowsToCompute)
111 {
112   /*call ZGEQRF( M, N, zstk(lA), M, //:LWORK >= max(1,N).
113     call ZGEQPF( M, N, zstk(lA), M, istk(lJPVT), zstk(lTAU), ->zqeqp3: LWORK >= N+1.
114     call ZUNGQR( M, min(M,N), min(M,N), zstk(lQ), M, zstk(lTAU),->:LWORK >= max(1,N). ici max (1, iRowsToCompute)
115
116     call DGEQRF( M, N, stk(lA), M, stk(lTAU), stk(lDWORK), LWORK >= max(1,N).
117     DGEQPF( M, N, stk(lA), M, istk(lJPVT), stk(lTAU)->dgeqp3 ,LWORK >= 3*N+1.
118
119     call DORGQR( M, min(M,N), min(M,N), stk(lQ), M, stk(lTAU),LWORK >= max(1,N). ici max iRowsTocompute
120   */
121   int minGeqrf, minGeqp3, minGqr;
122   minGeqrf= Max(1, iCols); /* lifted from original Fortran code, but should be useless after the previous tests 
123                               lifted from the same fortran code that are now in sci_qr. */
124   minGeqp3= (complexArg ? 3 : 1) * iCols +1;
125   minGqr= Max(1, iRowsToCompute);
126
127   return Max( Max(minGeqrf, minGeqp3), minGqr);
128 }
129
130 static int iQr(double* pData, int iRows, int iCols,  int iRowsToCompute, double dblTol
131        , double* pdblQ, double* pdblR, double* pdblE, double* pdblRank
132         , int* piPivot, double* pdblTau, double* pdblWork, int iWorkSize, double* pdblRWork);
133 int iQrM(double* pData, int iRows, int iCols, int /*bool*/ complexArg, int iRowsToCompute, double dblTol
134          , double* pdblQ, double* pdblR, double* pdblE, double* pdblRank)
135 {
136   int ret;
137   int* piPivot= NULL;
138   double*  pdblTau= NULL;
139   double* pdblWork= NULL; /* double ou doublecomplex */
140   double* pdblRWork= NULL; /* used only for comlpexArg */
141   int workSize= 0;
142   { /* tying to alloc the optimal size of temporary memory for algorithms for biggest to smallest, trying minimal amount as last ressort*/
143     int optWS[4];
144     optimalWorkSizes(complexArg, iRows, iCols, iRowsToCompute, optWS, optWS+1, optWS+2);
145     optWS[3]= minimalWorkSize(complexArg, iRows, iCols, iRowsToCompute);
146     {
147       int i;
148       for(i=0; (pdblWork==NULL) && (i!=4); ++i)
149         {
150           workSize= optWS[i];
151           pdblWork= (double*) MALLOC(workSize* (complexArg ? sizeof(doublecomplex): sizeof(double)));
152         }
153     }
154   }
155     if(!(ret=   !( pdblWork /* check previous allocation before trying (if not short-circuited) the next ones */
156                  && (piPivot= (int*) MALLOC(iCols * sizeof(int)))
157                  && (pdblTau= (double*) MALLOC( Min(iCols, iRows) * (complexArg ? sizeof(doublecomplex): sizeof(double))))
158                    && (!complexArg /* last alloc only needed for complex case */
159                      || (pdblRWork= (double*) MALLOC( 2* iCols * sizeof(double))))
160                  )
161        ))
162     {
163       ret= iQr(pData, iRows, iCols, iRowsToCompute, dblTol, pdblQ, pdblR, pdblE, pdblRank
164                , piPivot, pdblTau, pdblWork, workSize, pdblRWork);
165     }
166   FREE(pdblWork);
167   FREE(piPivot);
168   FREE(pdblTau);
169   FREE(pdblRWork);
170   return ret;
171 }
172
173 int iQr(double* pData, int iRows, int iCols,  int iRowsToCompute, double dblTol
174        , double* pdblQ, double* pdblR, double* pdblE, double* pdblRank
175        , int* piPivot, double* pdblTau, double* pdblWork, int iWorkSize, double* pdblRWork)
176 {
177   int lhs= pdblRank ? 4 : (pdblE ? 3 : 2) ;
178   int complexArg= !(pdblRWork == NULL);
179   int economyMode= (iRowsToCompute < iRows);
180   int info;
181
182   memset(pdblR, 0, iRowsToCompute * iCols * (complexArg ? sizeof(doublecomplex) : sizeof(double)));
183   if(lhs == 2)
184     {
185       if(complexArg)
186         {
187           C2F(zgeqrf)(&iRows, &iCols, pData, &iRows, pdblTau, pdblWork, &iWorkSize, &info);
188         }
189       else
190         {
191           C2F(dgeqrf)(&iRows, &iCols, pData, &iRows, pdblTau, pdblWork, &iWorkSize, &info);
192         }
193     }
194   else
195     {
196       memset(piPivot, 0, iCols*sizeof(int));
197       if(complexArg)
198         {
199           C2F(zgeqp3)(&iRows, &iCols, pData, &iRows, piPivot, pdblTau, pdblWork, &iWorkSize, pdblRWork, &info);
200         }
201       else
202         {
203           C2F(dgeqp3)(&iRows, &iCols, pData, &iRows, piPivot, pdblTau,  pdblWork, &iWorkSize, &info);
204         }
205     }
206   if(info == 0)
207     {
208       if(complexArg)
209         {
210           C2F(zlacpy)("U",&iRowsToCompute, &iCols, pData, &iRows, pdblR, &iRowsToCompute);
211         }
212       else
213         {
214           C2F(dlacpy)("U",&iRowsToCompute, &iCols, pData, &iRows, pdblR, &iRowsToCompute);
215         }
216       if( !economyMode && (iRows > iCols) )
217         {
218           if(complexArg)
219             {
220               C2F(zlacpy)("F", &iRows, &iCols, pData, &iRows, pdblQ, &iRows);
221             }
222           else
223             {
224               C2F(dlacpy)("F", &iRows, &iCols, pData, &iRows, pdblQ, &iRows);
225             }
226           {
227             int i, j;
228             for( j= iCols; j != iRows; ++j)
229               {
230                 for( i= 0; i != iRows; ++i)
231                   {
232                     if(complexArg)
233                       {
234                         double* const cplx=pdblQ+ 2*(i+j*iRows) ;
235                         cplx[0]=0.;
236                         cplx[1]=0.;
237                       }
238                     else
239                       {
240                         double* const ptr=pdblQ+ (i+j*iRows) ;
241                         *ptr=0.;
242                         
243                       }
244                   }
245               }
246           }
247         }
248       else
249         {
250           if(complexArg)
251             {
252               C2F(zlacpy)("F", &iRows, &iRowsToCompute, pData, &iRows, pdblQ, &iRows);
253             }
254           else
255             {
256               C2F(dlacpy)("F", &iRows, &iRowsToCompute, pData, &iRows, pdblQ, &iRows);
257             }
258         }
259       {
260         int minRowsCols= Min(iRows, iCols);
261         if(complexArg)
262           {
263             C2F(zungqr)(&iRows, &iRowsToCompute, &minRowsCols, pdblQ, &iRows, pdblTau
264                                     , pdblWork, &iWorkSize, &info);
265           }
266         else
267           {
268             C2F(dorgqr)(&iRows, &iRowsToCompute, &minRowsCols, pdblQ, &iRows, pdblTau
269                                             , pdblWork, &iWorkSize, &info);
270           }
271       }
272       if( lhs > 2 )
273         {
274           memset(pdblE, 0, iCols * iCols * sizeof(double));
275           { 
276             int i, j;
277             for(j= 0; j != iCols; ++j)
278               {
279                 i= piPivot[j]-1;
280                 pdblE[ i + j * iCols ]= 1.;
281               }
282           }
283         }
284       if( lhs > 3) 
285         {
286           double tt= complexArg ? C2F(pythag)(pdblR, pdblR+1) : Abs(*pdblR) ;
287           if( dblTol < 0) /* /!\ original Frotran code does strict fp comparison with -1.0 */
288             {
289             // C2F(dlamch("p")) -> epsilon  * base
290             dblTol = Max(iRows, iCols) * C2F(dlamch)("p",1L) * tt;
291             }
292           {
293             int j, k=0;
294             double* ptrDiagR= pdblR; /* points to elements in the diagonal of R, hence the increment of iRows+1: +1 col & +1 row */
295             int diagIncrement= (iRows +1) * (complexArg ? 2 : 1);
296             for(j= 0; (j != Min(iRows, iCols)) ; ++j, ptrDiagR+= diagIncrement, ++k)
297               {
298                 if((complexArg ? C2F(pythag)(ptrDiagR, ptrDiagR+1) : Abs(*ptrDiagR)) < dblTol)
299                   { 
300                     break;
301                   }
302               }
303             *pdblRank= (double)k;
304           }
305         }
306     }
307   return info;
308 }