Update copyright statements and change license to LGPL
[alexxy/gromacs.git] / src / gmxlib / gmx_lapack / sbdsqr.c
1 /*
2  * This file is part of the GROMACS molecular simulation package.
3  *
4  * Copyright (c) 2012, by the GROMACS development team, led by
5  * David van der Spoel, Berk Hess, Erik Lindahl, and including many
6  * others, as listed in the AUTHORS file in the top-level source
7  * directory and at http://www.gromacs.org.
8  *
9  * GROMACS is free software; you can redistribute it and/or
10  * modify it under the terms of the GNU Lesser General Public License
11  * as published by the Free Software Foundation; either version 2.1
12  * of the License, or (at your option) any later version.
13  *
14  * GROMACS is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17  * Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public
20  * License along with GROMACS; if not, see
21  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
22  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
23  *
24  * If you want to redistribute modifications to GROMACS, please
25  * consider that scientific software is very special. Version
26  * control is crucial - bugs must be traceable. We will be happy to
27  * consider code for inclusion in the official distribution, but
28  * derived work must not be called official GROMACS. Details are found
29  * in the README & COPYING files - if they are missing, get the
30  * official version at http://www.gromacs.org.
31  *
32  * To help us fund GROMACS development, we humbly ask that you cite
33  * the research papers on the package. Check out http://www.gromacs.org.
34  */
35 #include <ctype.h>
36 #include <math.h>
37
38 #include "gmx_blas.h"
39 #include "gmx_lapack.h"
40
41 #include <types/simple.h>
42
43 void 
44 F77_FUNC(sbdsqr,SBDSQR)(const char *uplo,
45                         int *n,
46                         int *ncvt,
47                         int *nru, 
48                         int *ncc, 
49                         float *d__,
50                         float *e,
51                         float *vt, 
52                         int *ldvt,
53                         float *u, 
54                         int *ldu,
55                         float *c__, 
56                         int *ldc,
57                         float *work,
58                         int *info)
59 {
60     const char xuplo = toupper(*uplo);
61     int c_dim1, c_offset, u_dim1, u_offset, vt_dim1, vt_offset, i__1, 
62             i__2;
63     float r__1, r__2, r__3, r__4;
64     float c_b15 = -.125;
65
66     int c__1 = 1;
67     float c_b49 = 1.f;
68     float c_b72 = -1.f;
69
70     float f, g, h__;
71     int i__, j, m;
72     float r__, cs;
73     int ll;
74     float sn, mu;
75     int nm1, nm12, nm13, lll;
76     float eps, sll, tol, abse;
77     int idir;
78     float abss;
79     int oldm;
80     float cosl;
81     int isub, iter;
82     float unfl, sinl, cosr, smin, smax, sinr;
83     float oldcs;
84     int oldll;
85     float shift, sigmn, oldsn;
86     int maxit;
87     float sminl;
88     float sigmx;
89     int lower;
90     float sminoa;
91     float thresh;
92     int rotate;
93     float sminlo, tolmul;
94     int itmp1,itmp2;
95     float ftmp;
96     
97     --d__;
98     --e;
99     vt_dim1 = *ldvt;
100     vt_offset = 1 + vt_dim1;
101     vt -= vt_offset;
102     u_dim1 = *ldu;
103     u_offset = 1 + u_dim1;
104     u -= u_offset;
105     c_dim1 = *ldc;
106     c_offset = 1 + c_dim1;
107     c__ -= c_offset;
108     --work;
109
110     *info = 0;
111     
112     itmp1 = (*n > 1) ? *n : 1;
113     itmp2 = (*nru > 1) ? *nru : 1;
114     
115     lower = (xuplo == 'L');
116     if ( (xuplo!='U') && !lower) {
117         *info = -1;
118     } else if (*n < 0) {
119         *info = -2;
120     } else if (*ncvt < 0) {
121         *info = -3;
122     } else if (*nru < 0) {
123         *info = -4;
124     } else if (*ncc < 0) {
125         *info = -5;
126     } else if ( ((*ncvt == 0) && (*ldvt < 1)) || ((*ncvt > 0) && (*ldvt < itmp1)) ) {
127         *info = -9;
128     } else if (*ldu < itmp2) {
129         *info = -11;
130     } else if ( ((*ncc == 0) && (*ldc < 1)) || ((*ncc > 0) && (*ldc < itmp1))) {
131         *info = -13;
132     }
133     if (*info != 0) {
134         return;
135     }
136     if (*n == 0) {
137         return;
138     }
139     if (*n == 1) {
140         goto L160;
141     }
142
143     rotate = *ncvt > 0 || *nru > 0 || *ncc > 0;
144
145     if (! rotate) {
146         F77_FUNC(slasq1,SLASQ1)(n, &d__[1], &e[1], &work[1], info);
147         return;
148     }
149
150     nm1 = *n - 1;
151     nm12 = nm1 + nm1;
152     nm13 = nm12 + nm1;
153     idir = 0;
154
155     eps = GMX_FLOAT_EPS;
156     unfl = GMX_FLOAT_MIN/GMX_FLOAT_EPS;
157
158     if (lower) {
159         i__1 = *n - 1;
160         for (i__ = 1; i__ <= i__1; ++i__) {
161             F77_FUNC(slartg,SLARTG)(&d__[i__], &e[i__], &cs, &sn, &r__);
162             d__[i__] = r__;
163             e[i__] = sn * d__[i__ + 1];
164             d__[i__ + 1] = cs * d__[i__ + 1];
165             work[i__] = cs;
166             work[nm1 + i__] = sn;
167         }
168
169         if (*nru > 0) {
170             F77_FUNC(slasr,SLASR)("R", "V", "F", nru, n, &work[1], &work[*n], &u[u_offset], 
171                     ldu);
172         }
173         if (*ncc > 0) {
174             F77_FUNC(slasr,SLASR)("L", "V", "F", n, ncc, &work[1], &work[*n], &c__[c_offset],
175                      ldc);
176         }
177     }
178
179     r__3 = 100.f, r__4 = pow(GMX_FLOAT_EPS,c_b15);
180     r__1 = 10.f, r__2 = (r__3<r__4) ? r__3 : r__4;
181     tolmul = (r__1>r__2) ? r__1 : r__2;
182     tol = tolmul * eps;
183     smax = 0.f;
184     i__1 = *n;
185     for (i__ = 1; i__ <= i__1; ++i__) {
186         r__2 = smax, r__3 = (r__1 = d__[i__], fabs(r__1));
187         smax = (r__2>r__3) ? r__2 : r__3;
188     }
189     i__1 = *n - 1;
190     for (i__ = 1; i__ <= i__1; ++i__) {
191         r__2 = smax, r__3 = (r__1 = e[i__], fabs(r__1));
192         smax = (r__2>r__3) ? r__2 : r__3;
193     }
194     sminl = 0.f;
195     if (tol >= 0.f) {
196         sminoa = fabs(d__[1]);
197         if (sminoa == 0.f) {
198             goto L50;
199         }
200         mu = sminoa;
201         i__1 = *n;
202         for (i__ = 2; i__ <= i__1; ++i__) {
203             mu = (r__2 = d__[i__], fabs(r__2)) * (mu / (mu + (r__1 = e[i__ - 
204                     1], fabs(r__1))));
205             sminoa = (sminoa<mu) ? sminoa : mu;
206             if (sminoa == 0.f) {
207                 goto L50;
208             }
209         }
210 L50:
211         sminoa /= sqrt((float) (*n));
212         r__1 = tol * sminoa, r__2 = *n * 6 * *n * unfl;
213         thresh = (r__1>r__2) ? r__1 : r__2;
214     } else {
215         r__1 = fabs(tol) * smax, r__2 = *n * 6 * *n * unfl;
216         thresh = (r__1>r__2) ? r__1 : r__2;
217     }
218     maxit = *n * 6 * *n;
219     iter = 0;
220     oldll = -1;
221     oldm = -1;
222     m = *n;
223
224 L60:
225
226     if (m <= 1) {
227         goto L160;
228     }
229     if (iter > maxit) {
230         goto L200;
231     }
232
233     if (tol < 0.f && (r__1 = d__[m], fabs(r__1)) <= thresh) {
234         d__[m] = 0.f;
235     }
236     smax = (r__1 = d__[m], fabs(r__1));
237     smin = smax;
238     i__1 = m - 1;
239     for (lll = 1; lll <= i__1; ++lll) {
240         ll = m - lll;
241         abss = (r__1 = d__[ll], fabs(r__1));
242         abse = (r__1 = e[ll], fabs(r__1));
243         if (tol < 0.f && abss <= thresh) {
244             d__[ll] = 0.f;
245         }
246         if (abse <= thresh) {
247             goto L80;
248         }
249         smin = (smin<abss) ? smin : abss;
250         r__1 = (smax>abss) ? smax : abss;
251         smax = (r__1>abse) ? r__1 : abse;
252     }
253     ll = 0;
254     goto L90;
255 L80:
256     e[ll] = 0.f;
257     if (ll == m - 1) {
258         --m;
259         goto L60;
260     }
261 L90:
262     ++ll;
263     if (ll == m - 1) {
264         F77_FUNC(slasv2,SLASV2)(&d__[m - 1], &e[m - 1], &d__[m], &sigmn, &sigmx, &sinr, &cosr,
265                  &sinl, &cosl);
266         d__[m - 1] = sigmx;
267         e[m - 1] = 0.f;
268         d__[m] = sigmn;
269         if (*ncvt > 0) {
270             F77_FUNC(srot,SROT)(ncvt, &vt[m - 1 + vt_dim1], ldvt, &vt[m + vt_dim1], ldvt, &
271                     cosr, &sinr);
272         }
273         if (*nru > 0) {
274             F77_FUNC(srot,SROT)(nru, &u[(m - 1) * u_dim1 + 1], &c__1, &u[m * u_dim1 + 1], &
275                     c__1, &cosl, &sinl);
276         }
277         if (*ncc > 0) {
278             F77_FUNC(srot,SROT)(ncc, &c__[m - 1 + c_dim1], ldc, &c__[m + c_dim1], ldc, &
279                     cosl, &sinl);
280         }
281         m += -2;
282         goto L60;
283     }
284     if (ll > oldm || m < oldll) {
285         if ((r__1 = d__[ll], fabs(r__1)) >= (r__2 = d__[m], fabs(r__2))) {
286             idir = 1;
287         } else {
288             idir = 2;
289         }
290     }
291     if (idir == 1) {
292
293         if( (fabs(e[m-1]) <= fabs(tol) * fabs(d__[m])) ||
294             (tol<0.0 && fabs(e[m-1])<=thresh)) {
295             e[m - 1] = 0.f;
296             goto L60;
297         }
298         if (tol >= 0.f) {
299             mu = (r__1 = d__[ll], fabs(r__1));
300             sminl = mu;
301             i__1 = m - 1;
302             for (lll = ll; lll <= i__1; ++lll) {
303                 if ((r__1 = e[lll], fabs(r__1)) <= tol * mu) {
304                     e[lll] = 0.f;
305                     goto L60;
306                 }
307                 sminlo = sminl;
308                 mu = (r__2 = d__[lll + 1], fabs(r__2)) * (mu / (mu + (r__1 = 
309                         e[lll], fabs(r__1))));
310                 sminl = (sminl<mu) ? sminl : mu;
311             }
312         }
313     } else {
314         if( (fabs(e[ll]) <= fabs(tol)*fabs(d__[ll])) ||
315             (tol<0.0 && fabs(e[ll])<=thresh)) {
316             e[ll] = 0.f;
317             goto L60;
318         }
319         if (tol >= 0.f) {
320             mu = (r__1 = d__[m], fabs(r__1));
321             sminl = mu;
322             i__1 = ll;
323             for (lll = m - 1; lll >= i__1; --lll) {
324                 if ((r__1 = e[lll], fabs(r__1)) <= tol * mu) {
325                     e[lll] = 0.f;
326                     goto L60;
327                 }
328                 sminlo = sminl;
329                 mu = (r__2 = d__[lll], fabs(r__2)) * (mu / (mu + (r__1 = e[
330                         lll], fabs(r__1))));
331                 sminl = (sminl<mu) ? sminl : mu;
332             }
333         }
334     }
335     oldll = ll;
336     oldm = m;
337
338     r__1 = eps, r__2 = tol * .01f;
339     if (tol >= 0.f && *n * tol * (sminl / smax) <= ((r__1>r__2) ? r__1 : r__2)) {
340         shift = 0.f;
341     } else {
342         if (idir == 1) {
343             sll = (r__1 = d__[ll], fabs(r__1));
344             F77_FUNC(slas2,SLAS2)(&d__[m - 1], &e[m - 1], &d__[m], &shift, &r__);
345         } else {
346             sll = (r__1 = d__[m], fabs(r__1));
347             F77_FUNC(slas2,SLAS2)(&d__[ll], &e[ll], &d__[ll + 1], &shift, &r__);
348         }
349         if (sll > 0.f) {
350             r__1 = shift / sll;
351             if (r__1 * r__1 < eps) {
352                 shift = 0.f;
353             }
354         }
355     }
356     iter = iter + m - ll;
357     if (shift == 0.f) {
358         if (idir == 1) {
359             cs = 1.f;
360             oldcs = 1.f;
361             i__1 = m - 1;
362             for (i__ = ll; i__ <= i__1; ++i__) {
363                 r__1 = d__[i__] * cs;
364                 F77_FUNC(slartg,SLARTG)(&r__1, &e[i__], &cs, &sn, &r__);
365                 if (i__ > ll) {
366                     e[i__ - 1] = oldsn * r__;
367                 }
368                 r__1 = oldcs * r__;
369                 r__2 = d__[i__ + 1] * sn;
370                 F77_FUNC(slartg,SLARTG)(&r__1, &r__2, &oldcs, &oldsn, &d__[i__]);
371                 work[i__ - ll + 1] = cs;
372                 work[i__ - ll + 1 + nm1] = sn;
373                 work[i__ - ll + 1 + nm12] = oldcs;
374                 work[i__ - ll + 1 + nm13] = oldsn;
375             }
376             h__ = d__[m] * cs;
377             d__[m] = h__ * oldcs;
378             e[m - 1] = h__ * oldsn;
379             if (*ncvt > 0) {
380                 i__1 = m - ll + 1;
381                 F77_FUNC(slasr,SLASR)("L", "V", "F", &i__1, ncvt, &work[1], &work[*n], &vt[
382                         ll + vt_dim1], ldvt);
383             }
384             if (*nru > 0) {
385                 i__1 = m - ll + 1;
386                 F77_FUNC(slasr,SLASR)("R", "V", "F", nru, &i__1, &work[nm12 + 1], &work[nm13 
387                         + 1], &u[ll * u_dim1 + 1], ldu);
388             }
389             if (*ncc > 0) {
390                 i__1 = m - ll + 1;
391                 F77_FUNC(slasr,SLASR)("L", "V", "F", &i__1, ncc, &work[nm12 + 1], &work[nm13 
392                         + 1], &c__[ll + c_dim1], ldc);
393             }
394             if ((r__1 = e[m - 1], fabs(r__1)) <= thresh) {
395                 e[m - 1] = 0.f;
396             }
397         } else {
398             cs = 1.f;
399             oldcs = 1.f;
400             i__1 = ll + 1;
401             for (i__ = m; i__ >= i__1; --i__) {
402                 r__1 = d__[i__] * cs;
403                 F77_FUNC(slartg,SLARTG)(&r__1, &e[i__ - 1], &cs, &sn, &r__);
404                 if (i__ < m) {
405                     e[i__] = oldsn * r__;
406                 }
407                 r__1 = oldcs * r__;
408                 r__2 = d__[i__ - 1] * sn;
409                 F77_FUNC(slartg,SLARTG)(&r__1, &r__2, &oldcs, &oldsn, &d__[i__]);
410                 work[i__ - ll] = cs;
411                 work[i__ - ll + nm1] = -sn;
412                 work[i__ - ll + nm12] = oldcs;
413                 work[i__ - ll + nm13] = -oldsn;
414             }
415             h__ = d__[ll] * cs;
416             d__[ll] = h__ * oldcs;
417             e[ll] = h__ * oldsn;
418             if (*ncvt > 0) {
419                 i__1 = m - ll + 1;
420                 F77_FUNC(slasr,SLASR)("L", "V", "B", &i__1, ncvt, &work[nm12 + 1], &work[
421                         nm13 + 1], &vt[ll + vt_dim1], ldvt);
422             }
423             if (*nru > 0) {
424                 i__1 = m - ll + 1;
425                 F77_FUNC(slasr,SLASR)("R", "V", "B", nru, &i__1, &work[1], &work[*n], &u[ll *
426                          u_dim1 + 1], ldu);
427             }
428             if (*ncc > 0) {
429                 i__1 = m - ll + 1;
430                 F77_FUNC(slasr,SLASR)("L", "V", "B", &i__1, ncc, &work[1], &work[*n], &c__[
431                         ll + c_dim1], ldc);
432             }
433             if ((r__1 = e[ll], fabs(r__1)) <= thresh) {
434                 e[ll] = 0.f;
435             }
436         }
437     } else {
438
439         if (idir == 1) {
440             f = ((r__1 = d__[ll], fabs(r__1)) - shift) * ( ((d__[ll] > 0) ? c_b49 : -c_b49) + shift / d__[ll]);
441             g = e[ll];
442             i__1 = m - 1;
443             for (i__ = ll; i__ <= i__1; ++i__) {
444                 F77_FUNC(slartg,SLARTG)(&f, &g, &cosr, &sinr, &r__);
445                 if (i__ > ll) {
446                     e[i__ - 1] = r__;
447                 }
448                 f = cosr * d__[i__] + sinr * e[i__];
449                 e[i__] = cosr * e[i__] - sinr * d__[i__];
450                 g = sinr * d__[i__ + 1];
451                 d__[i__ + 1] = cosr * d__[i__ + 1];
452                 F77_FUNC(slartg,SLARTG)(&f, &g, &cosl, &sinl, &r__);
453                 d__[i__] = r__;
454                 f = cosl * e[i__] + sinl * d__[i__ + 1];
455                 d__[i__ + 1] = cosl * d__[i__ + 1] - sinl * e[i__];
456                 if (i__ < m - 1) {
457                     g = sinl * e[i__ + 1];
458                     e[i__ + 1] = cosl * e[i__ + 1];
459                 }
460                 work[i__ - ll + 1] = cosr;
461                 work[i__ - ll + 1 + nm1] = sinr;
462                 work[i__ - ll + 1 + nm12] = cosl;
463                 work[i__ - ll + 1 + nm13] = sinl;
464             }
465             e[m - 1] = f;
466
467             if (*ncvt > 0) {
468                 i__1 = m - ll + 1;
469                 F77_FUNC(slasr,SLASR)("L", "V", "F", &i__1, ncvt, &work[1], &work[*n], &vt[
470                         ll + vt_dim1], ldvt);
471             }
472             if (*nru > 0) {
473                 i__1 = m - ll + 1;
474                 F77_FUNC(slasr,SLASR)("R", "V", "F", nru, &i__1, &work[nm12 + 1], &work[nm13 
475                         + 1], &u[ll * u_dim1 + 1], ldu);
476             }
477             if (*ncc > 0) {
478                 i__1 = m - ll + 1;
479                 F77_FUNC(slasr,SLASR)("L", "V", "F", &i__1, ncc, &work[nm12 + 1], &work[nm13 
480                         + 1], &c__[ll + c_dim1], ldc);
481             }
482             if ((r__1 = e[m - 1], fabs(r__1)) <= thresh) {
483                 e[m - 1] = 0.f;
484             }
485         } else {
486
487             f = ((r__1 = d__[m], fabs(r__1)) - shift) * ( ((d__[m] > 0) ? c_b49 : -c_b49) + shift / d__[m]);
488             g = e[m - 1];
489             i__1 = ll + 1;
490             for (i__ = m; i__ >= i__1; --i__) {
491                 F77_FUNC(slartg,SLARTG)(&f, &g, &cosr, &sinr, &r__);
492                 if (i__ < m) {
493                     e[i__] = r__;
494                 }
495                 f = cosr * d__[i__] + sinr * e[i__ - 1];
496                 e[i__ - 1] = cosr * e[i__ - 1] - sinr * d__[i__];
497                 g = sinr * d__[i__ - 1];
498                 d__[i__ - 1] = cosr * d__[i__ - 1];
499                 F77_FUNC(slartg,SLARTG)(&f, &g, &cosl, &sinl, &r__);
500                 d__[i__] = r__;
501                 f = cosl * e[i__ - 1] + sinl * d__[i__ - 1];
502                 d__[i__ - 1] = cosl * d__[i__ - 1] - sinl * e[i__ - 1];
503                 if (i__ > ll + 1) {
504                     g = sinl * e[i__ - 2];
505                     e[i__ - 2] = cosl * e[i__ - 2];
506                 }
507                 work[i__ - ll] = cosr;
508                 work[i__ - ll + nm1] = -sinr;
509                 work[i__ - ll + nm12] = cosl;
510                 work[i__ - ll + nm13] = -sinl;
511             }
512             e[ll] = f;
513
514             if ((r__1 = e[ll], fabs(r__1)) <= thresh) {
515                 e[ll] = 0.f;
516             }
517             if (*ncvt > 0) {
518                 i__1 = m - ll + 1;
519                 F77_FUNC(slasr,SLASR)("L", "V", "B", &i__1, ncvt, &work[nm12 + 1], &work[
520                         nm13 + 1], &vt[ll + vt_dim1], ldvt);
521             }
522             if (*nru > 0) {
523                 i__1 = m - ll + 1;
524                 F77_FUNC(slasr,SLASR)("R", "V", "B", nru, &i__1, &work[1], &work[*n], &u[ll *
525                          u_dim1 + 1], ldu);
526             }
527             if (*ncc > 0) {
528                 i__1 = m - ll + 1;
529                 F77_FUNC(slasr,SLASR)("L", "V", "B", &i__1, ncc, &work[1], &work[*n], &c__[
530                         ll + c_dim1], ldc);
531             }
532         }
533     }
534
535     goto L60;
536
537 L160:
538     i__1 = *n;
539     for (i__ = 1; i__ <= i__1; ++i__) {
540         if (d__[i__] < 0.f) {
541             d__[i__] = -d__[i__];
542
543             if (*ncvt > 0) {
544                 F77_FUNC(sscal,SSCAL)(ncvt, &c_b72, &vt[i__ + vt_dim1], ldvt);
545             }
546         }
547     }
548
549     i__1 = *n - 1;
550     for (i__ = 1; i__ <= i__1; ++i__) {
551
552         isub = 1;
553         smin = d__[1];
554         i__2 = *n + 1 - i__;
555         for (j = 2; j <= i__2; ++j) {
556             if (d__[j] <= smin) {
557                 isub = j;
558                 smin = d__[j];
559             }
560         }
561         if (isub != *n + 1 - i__) {
562             d__[isub] = d__[*n + 1 - i__];
563             d__[*n + 1 - i__] = smin;
564             if (*ncvt > 0) {
565                 F77_FUNC(sswap,SSWAP)(ncvt, &vt[isub + vt_dim1], ldvt, &vt[*n + 1 - i__ + 
566                         vt_dim1], ldvt);
567             }
568             if (*nru > 0) {
569                 F77_FUNC(sswap,SSWAP)(nru, &u[isub * u_dim1 + 1], &c__1, &u[(*n + 1 - i__) * 
570                         u_dim1 + 1], &c__1);
571             }
572             if (*ncc > 0) {
573                 F77_FUNC(sswap,SSWAP)(ncc, &c__[isub + c_dim1], ldc, &c__[*n + 1 - i__ + 
574                         c_dim1], ldc);
575             }
576         }
577     }
578     goto L220;
579
580 L200:
581     *info = 0;
582     i__1 = *n - 1;
583     for (i__ = 1; i__ <= i__1; ++i__) {
584         if (e[i__] != 0.f) {
585             ++(*info);
586         }
587     }
588 L220:
589     return;
590
591 }
592
593