Free cookie consent management tool by TermsFeed Policy Generator

source: branches/Persistence Test/ALGLIB/bdsvd.cs @ 4383

Last change on this file since 4383 was 2430, checked in by gkronber, 15 years ago

Moved ALGLIB code into a separate plugin. #783
File size: 50.3 KB
Line 
1/*************************************************************************
2Copyright (c) 1992-2007 The University of Tennessee.  All rights reserved.
3
4Contributors:
5    * Sergey Bochkanov (ALGLIB project). Translation from FORTRAN to
6      pseudocode.
7
8See subroutines comments for additional copyrights.
9
10>>> SOURCE LICENSE >>>
11This program is free software; you can redistribute it and/or modify
12it under the terms of the GNU General Public License as published by
13the Free Software Foundation (www.fsf.org); either version 2 of the
14License, or (at your option) any later version.
15
16This program is distributed in the hope that it will be useful,
17but WITHOUT ANY WARRANTY; without even the implied warranty of
18MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19GNU General Public License for more details.
20
21A copy of the GNU General Public License is available at
22http://www.fsf.org/licensing/licenses
23
24>>> END OF LICENSE >>>
25*************************************************************************/
26
27using System;
28
29namespace alglib
30{
31    public class bdsvd
32    {
33        /*************************************************************************
34        Singular value decomposition of a bidiagonal matrix (extended algorithm)
35
36        The algorithm performs the singular value decomposition  of  a  bidiagonal
37        matrix B (upper or lower) representing it as B = Q*S*P^T, where Q and  P -
38        orthogonal matrices, S - diagonal matrix with non-negative elements on the
39        main diagonal, in descending order.
40
41        The  algorithm  finds  singular  values.  In  addition,  the algorithm can
42        calculate  matrices  Q  and P (more precisely, not the matrices, but their
43        product  with  given  matrices U and VT - U*Q and (P^T)*VT)).  Of  course,
44        matrices U and VT can be of any type, including identity. Furthermore, the
45        algorithm can calculate Q'*C (this product is calculated more  effectively
46        than U*Q,  because  this calculation operates with rows instead  of matrix
47        columns).
48
49        The feature of the algorithm is its ability to find  all  singular  values
50        including those which are arbitrarily close to 0  with  relative  accuracy
51        close to  machine precision. If the parameter IsFractionalAccuracyRequired
52        is set to True, all singular values will have high relative accuracy close
53        to machine precision. If the parameter is set to False, only  the  biggest
54        singular value will have relative accuracy  close  to  machine  precision.
55        The absolute error of other singular values is equal to the absolute error
56        of the biggest singular value.
57
58        Input parameters:
59            D       -   main diagonal of matrix B.
60                        Array whose index ranges within [0..N-1].
61            E       -   superdiagonal (or subdiagonal) of matrix B.
62                        Array whose index ranges within [0..N-2].
63            N       -   size of matrix B.
64            IsUpper -   True, if the matrix is upper bidiagonal.
65            IsFractionalAccuracyRequired -
66                        accuracy to search singular values with.
67            U       -   matrix to be multiplied by Q.
68                        Array whose indexes range within [0..NRU-1, 0..N-1].
69                        The matrix can be bigger, in that case only the  submatrix
70                        [0..NRU-1, 0..N-1] will be multiplied by Q.
71            NRU     -   number of rows in matrix U.
72            C       -   matrix to be multiplied by Q'.
73                        Array whose indexes range within [0..N-1, 0..NCC-1].
74                        The matrix can be bigger, in that case only the  submatrix
75                        [0..N-1, 0..NCC-1] will be multiplied by Q'.
76            NCC     -   number of columns in matrix C.
77            VT      -   matrix to be multiplied by P^T.
78                        Array whose indexes range within [0..N-1, 0..NCVT-1].
79                        The matrix can be bigger, in that case only the  submatrix
80                        [0..N-1, 0..NCVT-1] will be multiplied by P^T.
81            NCVT    -   number of columns in matrix VT.
82
83        Output parameters:
84            D       -   singular values of matrix B in descending order.
85            U       -   if NRU>0, contains matrix U*Q.
86            VT      -   if NCVT>0, contains matrix (P^T)*VT.
87            C       -   if NCC>0, contains matrix Q'*C.
88
89        Result:
90            True, if the algorithm has converged.
91            False, if the algorithm hasn't converged (rare case).
92
93        Additional information:
94            The type of convergence is controlled by the internal  parameter  TOL.
95            If the parameter is greater than 0, the singular values will have
96            relative accuracy TOL. If TOL<0, the singular values will have
97            absolute accuracy ABS(TOL)*norm(B).
98            By default, |TOL| falls within the range of 10*Epsilon and 100*Epsilon,
99            where Epsilon is the machine precision. It is not  recommended  to  use
100            TOL less than 10*Epsilon since this will  considerably  slow  down  the
101            algorithm and may not lead to error decreasing.
102        History:
103            * 31 March, 2007.
104                changed MAXITR from 6 to 12.
105
106          -- LAPACK routine (version 3.0) --
107             Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
108             Courant Institute, Argonne National Lab, and Rice University
109             October 31, 1999.
110        *************************************************************************/
111        public static bool rmatrixbdsvd(ref double[] d,
112            double[] e,
113            int n,
114            bool isupper,
115            bool isfractionalaccuracyrequired,
116            ref double[,] u,
117            int nru,
118            ref double[,] c,
119            int ncc,
120            ref double[,] vt,
121            int ncvt)
122        {
123            bool result = new bool();
124            double[] d1 = new double[0];
125            double[] e1 = new double[0];
126            int i_ = 0;
127            int i1_ = 0;
128
129            e = (double[])e.Clone();
130
131            d1 = new double[n+1];
132            i1_ = (0) - (1);
133            for(i_=1; i_<=n;i_++)
134            {
135                d1[i_] = d[i_+i1_];
136            }
137            if( n>1 )
138            {
139                e1 = new double[n-1+1];
140                i1_ = (0) - (1);
141                for(i_=1; i_<=n-1;i_++)
142                {
143                    e1[i_] = e[i_+i1_];
144                }
145            }
146            result = bidiagonalsvddecompositioninternal(ref d1, e1, n, isupper, isfractionalaccuracyrequired, ref u, 0, nru, ref c, 0, ncc, ref vt, 0, ncvt);
147            i1_ = (1) - (0);
148            for(i_=0; i_<=n-1;i_++)
149            {
150                d[i_] = d1[i_+i1_];
151            }
152            return result;
153        }
154
155
156        /*************************************************************************
157        Obsolete 1-based subroutine. See RMatrixBDSVD for 0-based replacement.
158
159        History:
160            * 31 March, 2007.
161                changed MAXITR from 6 to 12.
162
163          -- LAPACK routine (version 3.0) --
164             Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
165             Courant Institute, Argonne National Lab, and Rice University
166             October 31, 1999.
167        *************************************************************************/
168        public static bool bidiagonalsvddecomposition(ref double[] d,
169            double[] e,
170            int n,
171            bool isupper,
172            bool isfractionalaccuracyrequired,
173            ref double[,] u,
174            int nru,
175            ref double[,] c,
176            int ncc,
177            ref double[,] vt,
178            int ncvt)
179        {
180            bool result = new bool();
181
182            e = (double[])e.Clone();
183
184            result = bidiagonalsvddecompositioninternal(ref d, e, n, isupper, isfractionalaccuracyrequired, ref u, 1, nru, ref c, 1, ncc, ref vt, 1, ncvt);
185            return result;
186        }
187
188
189        /*************************************************************************
190        Internal working subroutine for bidiagonal decomposition
191        *************************************************************************/
192        private static bool bidiagonalsvddecompositioninternal(ref double[] d,
193            double[] e,
194            int n,
195            bool isupper,
196            bool isfractionalaccuracyrequired,
197            ref double[,] u,
198            int ustart,
199            int nru,
200            ref double[,] c,
201            int cstart,
202            int ncc,
203            ref double[,] vt,
204            int vstart,
205            int ncvt)
206        {
207            bool result = new bool();
208            int i = 0;
209            int idir = 0;
210            int isub = 0;
211            int iter = 0;
212            int j = 0;
213            int ll = 0;
214            int lll = 0;
215            int m = 0;
216            int maxit = 0;
217            int oldll = 0;
218            int oldm = 0;
219            double abse = 0;
220            double abss = 0;
221            double cosl = 0;
222            double cosr = 0;
223            double cs = 0;
224            double eps = 0;
225            double f = 0;
226            double g = 0;
227            double h = 0;
228            double mu = 0;
229            double oldcs = 0;
230            double oldsn = 0;
231            double r = 0;
232            double shift = 0;
233            double sigmn = 0;
234            double sigmx = 0;
235            double sinl = 0;
236            double sinr = 0;
237            double sll = 0;
238            double smax = 0;
239            double smin = 0;
240            double sminl = 0;
241            double sminlo = 0;
242            double sminoa = 0;
243            double sn = 0;
244            double thresh = 0;
245            double tol = 0;
246            double tolmul = 0;
247            double unfl = 0;
248            double[] work0 = new double[0];
249            double[] work1 = new double[0];
250            double[] work2 = new double[0];
251            double[] work3 = new double[0];
252            int maxitr = 0;
253            bool matrixsplitflag = new bool();
254            bool iterflag = new bool();
255            double[] utemp = new double[0];
256            double[] vttemp = new double[0];
257            double[] ctemp = new double[0];
258            double[] etemp = new double[0];
259            bool rightside = new bool();
260            bool fwddir = new bool();
261            double tmp = 0;
262            int mm1 = 0;
263            int mm0 = 0;
264            bool bchangedir = new bool();
265            int uend = 0;
266            int cend = 0;
267            int vend = 0;
268            int i_ = 0;
269
270            e = (double[])e.Clone();
271
272            result = true;
273            if( n==0 )
274            {
275                return result;
276            }
277            if( n==1 )
278            {
279                if( d[1]<0 )
280                {
281                    d[1] = -d[1];
282                    if( ncvt>0 )
283                    {
284                        for(i_=vstart; i_<=vstart+ncvt-1;i_++)
285                        {
286                            vt[vstart,i_] = -1*vt[vstart,i_];
287                        }
288                    }
289                }
290                return result;
291            }
292           
293            //
294            // init
295            //
296            work0 = new double[n-1+1];
297            work1 = new double[n-1+1];
298            work2 = new double[n-1+1];
299            work3 = new double[n-1+1];
300            uend = ustart+Math.Max(nru-1, 0);
301            vend = vstart+Math.Max(ncvt-1, 0);
302            cend = cstart+Math.Max(ncc-1, 0);
303            utemp = new double[uend+1];
304            vttemp = new double[vend+1];
305            ctemp = new double[cend+1];
306            maxitr = 12;
307            rightside = true;
308            fwddir = true;
309           
310            //
311            // resize E from N-1 to N
312            //
313            etemp = new double[n+1];
314            for(i=1; i<=n-1; i++)
315            {
316                etemp[i] = e[i];
317            }
318            e = new double[n+1];
319            for(i=1; i<=n-1; i++)
320            {
321                e[i] = etemp[i];
322            }
323            e[n] = 0;
324            idir = 0;
325           
326            //
327            // Get machine constants
328            //
329            eps = AP.Math.MachineEpsilon;
330            unfl = AP.Math.MinRealNumber;
331           
332            //
333            // If matrix lower bidiagonal, rotate to be upper bidiagonal
334            // by applying Givens rotations on the left
335            //
336            if( !isupper )
337            {
338                for(i=1; i<=n-1; i++)
339                {
340                    rotations.generaterotation(d[i], e[i], ref cs, ref sn, ref r);
341                    d[i] = r;
342                    e[i] = sn*d[i+1];
343                    d[i+1] = cs*d[i+1];
344                    work0[i] = cs;
345                    work1[i] = sn;
346                }
347               
348                //
349                // Update singular vectors if desired
350                //
351                if( nru>0 )
352                {
353                    rotations.applyrotationsfromtheright(fwddir, ustart, uend, 1+ustart-1, n+ustart-1, ref work0, ref work1, ref u, ref utemp);
354                }
355                if( ncc>0 )
356                {
357                    rotations.applyrotationsfromtheleft(fwddir, 1+cstart-1, n+cstart-1, cstart, cend, ref work0, ref work1, ref c, ref ctemp);
358                }
359            }
360           
361            //
362            // Compute singular values to relative accuracy TOL
363            // (By setting TOL to be negative, algorithm will compute
364            // singular values to absolute accuracy ABS(TOL)*norm(input matrix))
365            //
366            tolmul = Math.Max(10, Math.Min(100, Math.Pow(eps, -0.125)));
367            tol = tolmul*eps;
368            if( !isfractionalaccuracyrequired )
369            {
370                tol = -tol;
371            }
372           
373            //
374            // Compute approximate maximum, minimum singular values
375            //
376            smax = 0;
377            for(i=1; i<=n; i++)
378            {
379                smax = Math.Max(smax, Math.Abs(d[i]));
380            }
381            for(i=1; i<=n-1; i++)
382            {
383                smax = Math.Max(smax, Math.Abs(e[i]));
384            }
385            sminl = 0;
386            if( tol>=0 )
387            {
388               
389                //
390                // Relative accuracy desired
391                //
392                sminoa = Math.Abs(d[1]);
393                if( sminoa!=0 )
394                {
395                    mu = sminoa;
396                    for(i=2; i<=n; i++)
397                    {
398                        mu = Math.Abs(d[i])*(mu/(mu+Math.Abs(e[i-1])));
399                        sminoa = Math.Min(sminoa, mu);
400                        if( sminoa==0 )
401                        {
402                            break;
403                        }
404                    }
405                }
406                sminoa = sminoa/Math.Sqrt(n);
407                thresh = Math.Max(tol*sminoa, maxitr*n*n*unfl);
408            }
409            else
410            {
411               
412                //
413                // Absolute accuracy desired
414                //
415                thresh = Math.Max(Math.Abs(tol)*smax, maxitr*n*n*unfl);
416            }
417           
418            //
419            // Prepare for main iteration loop for the singular values
420            // (MAXIT is the maximum number of passes through the inner
421            // loop permitted before nonconvergence signalled.)
422            //
423            maxit = maxitr*n*n;
424            iter = 0;
425            oldll = -1;
426            oldm = -1;
427           
428            //
429            // M points to last element of unconverged part of matrix
430            //
431            m = n;
432           
433            //
434            // Begin main iteration loop
435            //
436            while( true )
437            {
438               
439                //
440                // Check for convergence or exceeding iteration count
441                //
442                if( m<=1 )
443                {
444                    break;
445                }
446                if( iter>maxit )
447                {
448                    result = false;
449                    return result;
450                }
451               
452                //
453                // Find diagonal block of matrix to work on
454                //
455                if( tol<0 & Math.Abs(d[m])<=thresh )
456                {
457                    d[m] = 0;
458                }
459                smax = Math.Abs(d[m]);
460                smin = smax;
461                matrixsplitflag = false;
462                for(lll=1; lll<=m-1; lll++)
463                {
464                    ll = m-lll;
465                    abss = Math.Abs(d[ll]);
466                    abse = Math.Abs(e[ll]);
467                    if( tol<0 & abss<=thresh )
468                    {
469                        d[ll] = 0;
470                    }
471                    if( abse<=thresh )
472                    {
473                        matrixsplitflag = true;
474                        break;
475                    }
476                    smin = Math.Min(smin, abss);
477                    smax = Math.Max(smax, Math.Max(abss, abse));
478                }
479                if( !matrixsplitflag )
480                {
481                    ll = 0;
482                }
483                else
484                {
485                   
486                    //
487                    // Matrix splits since E(LL) = 0
488                    //
489                    e[ll] = 0;
490                    if( ll==m-1 )
491                    {
492                       
493                        //
494                        // Convergence of bottom singular value, return to top of loop
495                        //
496                        m = m-1;
497                        continue;
498                    }
499                }
500                ll = ll+1;
501               
502                //
503                // E(LL) through E(M-1) are nonzero, E(LL-1) is zero
504                //
505                if( ll==m-1 )
506                {
507                   
508                    //
509                    // 2 by 2 block, handle separately
510                    //
511                    svdv2x2(d[m-1], e[m-1], d[m], ref sigmn, ref sigmx, ref sinr, ref cosr, ref sinl, ref cosl);
512                    d[m-1] = sigmx;
513                    e[m-1] = 0;
514                    d[m] = sigmn;
515                   
516                    //
517                    // Compute singular vectors, if desired
518                    //
519                    if( ncvt>0 )
520                    {
521                        mm0 = m+(vstart-1);
522                        mm1 = m-1+(vstart-1);
523                        for(i_=vstart; i_<=vend;i_++)
524                        {
525                            vttemp[i_] = cosr*vt[mm1,i_];
526                        }
527                        for(i_=vstart; i_<=vend;i_++)
528                        {
529                            vttemp[i_] = vttemp[i_] + sinr*vt[mm0,i_];
530                        }
531                        for(i_=vstart; i_<=vend;i_++)
532                        {
533                            vt[mm0,i_] = cosr*vt[mm0,i_];
534                        }
535                        for(i_=vstart; i_<=vend;i_++)
536                        {
537                            vt[mm0,i_] = vt[mm0,i_] - sinr*vt[mm1,i_];
538                        }
539                        for(i_=vstart; i_<=vend;i_++)
540                        {
541                            vt[mm1,i_] = vttemp[i_];
542                        }
543                    }
544                    if( nru>0 )
545                    {
546                        mm0 = m+ustart-1;
547                        mm1 = m-1+ustart-1;
548                        for(i_=ustart; i_<=uend;i_++)
549                        {
550                            utemp[i_] = cosl*u[i_,mm1];
551                        }
552                        for(i_=ustart; i_<=uend;i_++)
553                        {
554                            utemp[i_] = utemp[i_] + sinl*u[i_,mm0];
555                        }
556                        for(i_=ustart; i_<=uend;i_++)
557                        {
558                            u[i_,mm0] = cosl*u[i_,mm0];
559                        }
560                        for(i_=ustart; i_<=uend;i_++)
561                        {
562                            u[i_,mm0] = u[i_,mm0] - sinl*u[i_,mm1];
563                        }
564                        for(i_=ustart; i_<=uend;i_++)
565                        {
566                            u[i_,mm1] = utemp[i_];
567                        }
568                    }
569                    if( ncc>0 )
570                    {
571                        mm0 = m+cstart-1;
572                        mm1 = m-1+cstart-1;
573                        for(i_=cstart; i_<=cend;i_++)
574                        {
575                            ctemp[i_] = cosl*c[mm1,i_];
576                        }
577                        for(i_=cstart; i_<=cend;i_++)
578                        {
579                            ctemp[i_] = ctemp[i_] + sinl*c[mm0,i_];
580                        }
581                        for(i_=cstart; i_<=cend;i_++)
582                        {
583                            c[mm0,i_] = cosl*c[mm0,i_];
584                        }
585                        for(i_=cstart; i_<=cend;i_++)
586                        {
587                            c[mm0,i_] = c[mm0,i_] - sinl*c[mm1,i_];
588                        }
589                        for(i_=cstart; i_<=cend;i_++)
590                        {
591                            c[mm1,i_] = ctemp[i_];
592                        }
593                    }
594                    m = m-2;
595                    continue;
596                }
597               
598                //
599                // If working on new submatrix, choose shift direction
600                // (from larger end diagonal element towards smaller)
601                //
602                // Previously was
603                //     "if (LL>OLDM) or (M<OLDLL) then"
604                // fixed thanks to Michael Rolle < m@rolle.name >
605                // Very strange that LAPACK still contains it.
606                //
607                bchangedir = false;
608                if( idir==1 & Math.Abs(d[ll])<1.0E-3*Math.Abs(d[m]) )
609                {
610                    bchangedir = true;
611                }
612                if( idir==2 & Math.Abs(d[m])<1.0E-3*Math.Abs(d[ll]) )
613                {
614                    bchangedir = true;
615                }
616                if( ll!=oldll | m!=oldm | bchangedir )
617                {
618                    if( Math.Abs(d[ll])>=Math.Abs(d[m]) )
619                    {
620                       
621                        //
622                        // Chase bulge from top (big end) to bottom (small end)
623                        //
624                        idir = 1;
625                    }
626                    else
627                    {
628                       
629                        //
630                        // Chase bulge from bottom (big end) to top (small end)
631                        //
632                        idir = 2;
633                    }
634                }
635               
636                //
637                // Apply convergence tests
638                //
639                if( idir==1 )
640                {
641                   
642                    //
643                    // Run convergence test in forward direction
644                    // First apply standard test to bottom of matrix
645                    //
646                    if( Math.Abs(e[m-1])<=Math.Abs(tol)*Math.Abs(d[m]) | tol<0 & Math.Abs(e[m-1])<=thresh )
647                    {
648                        e[m-1] = 0;
649                        continue;
650                    }
651                    if( tol>=0 )
652                    {
653                       
654                        //
655                        // If relative accuracy desired,
656                        // apply convergence criterion forward
657                        //
658                        mu = Math.Abs(d[ll]);
659                        sminl = mu;
660                        iterflag = false;
661                        for(lll=ll; lll<=m-1; lll++)
662                        {
663                            if( Math.Abs(e[lll])<=tol*mu )
664                            {
665                                e[lll] = 0;
666                                iterflag = true;
667                                break;
668                            }
669                            sminlo = sminl;
670                            mu = Math.Abs(d[lll+1])*(mu/(mu+Math.Abs(e[lll])));
671                            sminl = Math.Min(sminl, mu);
672                        }
673                        if( iterflag )
674                        {
675                            continue;
676                        }
677                    }
678                }
679                else
680                {
681                   
682                    //
683                    // Run convergence test in backward direction
684                    // First apply standard test to top of matrix
685                    //
686                    if( Math.Abs(e[ll])<=Math.Abs(tol)*Math.Abs(d[ll]) | tol<0 & Math.Abs(e[ll])<=thresh )
687                    {
688                        e[ll] = 0;
689                        continue;
690                    }
691                    if( tol>=0 )
692                    {
693                       
694                        //
695                        // If relative accuracy desired,
696                        // apply convergence criterion backward
697                        //
698                        mu = Math.Abs(d[m]);
699                        sminl = mu;
700                        iterflag = false;
701                        for(lll=m-1; lll>=ll; lll--)
702                        {
703                            if( Math.Abs(e[lll])<=tol*mu )
704                            {
705                                e[lll] = 0;
706                                iterflag = true;
707                                break;
708                            }
709                            sminlo = sminl;
710                            mu = Math.Abs(d[lll])*(mu/(mu+Math.Abs(e[lll])));
711                            sminl = Math.Min(sminl, mu);
712                        }
713                        if( iterflag )
714                        {
715                            continue;
716                        }
717                    }
718                }
719                oldll = ll;
720                oldm = m;
721               
722                //
723                // Compute shift.  First, test if shifting would ruin relative
724                // accuracy, and if so set the shift to zero.
725                //
726                if( tol>=0 & n*tol*(sminl/smax)<=Math.Max(eps, 0.01*tol) )
727                {
728                   
729                    //
730                    // Use a zero shift to avoid loss of relative accuracy
731                    //
732                    shift = 0;
733                }
734                else
735                {
736                   
737                    //
738                    // Compute the shift from 2-by-2 block at end of matrix
739                    //
740                    if( idir==1 )
741                    {
742                        sll = Math.Abs(d[ll]);
743                        svd2x2(d[m-1], e[m-1], d[m], ref shift, ref r);
744                    }
745                    else
746                    {
747                        sll = Math.Abs(d[m]);
748                        svd2x2(d[ll], e[ll], d[ll+1], ref shift, ref r);
749                    }
750                   
751                    //
752                    // Test if shift negligible, and if so set to zero
753                    //
754                    if( sll>0 )
755                    {
756                        if( AP.Math.Sqr(shift/sll)<eps )
757                        {
758                            shift = 0;
759                        }
760                    }
761                }
762               
763                //
764                // Increment iteration count
765                //
766                iter = iter+m-ll;
767               
768                //
769                // If SHIFT = 0, do simplified QR iteration
770                //
771                if( shift==0 )
772                {
773                    if( idir==1 )
774                    {
775                       
776                        //
777                        // Chase bulge from top to bottom
778                        // Save cosines and sines for later singular vector updates
779                        //
780                        cs = 1;
781                        oldcs = 1;
782                        for(i=ll; i<=m-1; i++)
783                        {
784                            rotations.generaterotation(d[i]*cs, e[i], ref cs, ref sn, ref r);
785                            if( i>ll )
786                            {
787                                e[i-1] = oldsn*r;
788                            }
789                            rotations.generaterotation(oldcs*r, d[i+1]*sn, ref oldcs, ref oldsn, ref tmp);
790                            d[i] = tmp;
791                            work0[i-ll+1] = cs;
792                            work1[i-ll+1] = sn;
793                            work2[i-ll+1] = oldcs;
794                            work3[i-ll+1] = oldsn;
795                        }
796                        h = d[m]*cs;
797                        d[m] = h*oldcs;
798                        e[m-1] = h*oldsn;
799                       
800                        //
801                        // Update singular vectors
802                        //
803                        if( ncvt>0 )
804                        {
805                            rotations.applyrotationsfromtheleft(fwddir, ll+vstart-1, m+vstart-1, vstart, vend, ref work0, ref work1, ref vt, ref vttemp);
806                        }
807                        if( nru>0 )
808                        {
809                            rotations.applyrotationsfromtheright(fwddir, ustart, uend, ll+ustart-1, m+ustart-1, ref work2, ref work3, ref u, ref utemp);
810                        }
811                        if( ncc>0 )
812                        {
813                            rotations.applyrotationsfromtheleft(fwddir, ll+cstart-1, m+cstart-1, cstart, cend, ref work2, ref work3, ref c, ref ctemp);
814                        }
815                       
816                        //
817                        // Test convergence
818                        //
819                        if( Math.Abs(e[m-1])<=thresh )
820                        {
821                            e[m-1] = 0;
822                        }
823                    }
824                    else
825                    {
826                       
827                        //
828                        // Chase bulge from bottom to top
829                        // Save cosines and sines for later singular vector updates
830                        //
831                        cs = 1;
832                        oldcs = 1;
833                        for(i=m; i>=ll+1; i--)
834                        {
835                            rotations.generaterotation(d[i]*cs, e[i-1], ref cs, ref sn, ref r);
836                            if( i<m )
837                            {
838                                e[i] = oldsn*r;
839                            }
840                            rotations.generaterotation(oldcs*r, d[i-1]*sn, ref oldcs, ref oldsn, ref tmp);
841                            d[i] = tmp;
842                            work0[i-ll] = cs;
843                            work1[i-ll] = -sn;
844                            work2[i-ll] = oldcs;
845                            work3[i-ll] = -oldsn;
846                        }
847                        h = d[ll]*cs;
848                        d[ll] = h*oldcs;
849                        e[ll] = h*oldsn;
850                       
851                        //
852                        // Update singular vectors
853                        //
854                        if( ncvt>0 )
855                        {
856                            rotations.applyrotationsfromtheleft(!fwddir, ll+vstart-1, m+vstart-1, vstart, vend, ref work2, ref work3, ref vt, ref vttemp);
857                        }
858                        if( nru>0 )
859                        {
860                            rotations.applyrotationsfromtheright(!fwddir, ustart, uend, ll+ustart-1, m+ustart-1, ref work0, ref work1, ref u, ref utemp);
861                        }
862                        if( ncc>0 )
863                        {
864                            rotations.applyrotationsfromtheleft(!fwddir, ll+cstart-1, m+cstart-1, cstart, cend, ref work0, ref work1, ref c, ref ctemp);
865                        }
866                       
867                        //
868                        // Test convergence
869                        //
870                        if( Math.Abs(e[ll])<=thresh )
871                        {
872                            e[ll] = 0;
873                        }
874                    }
875                }
876                else
877                {
878                   
879                    //
880                    // Use nonzero shift
881                    //
882                    if( idir==1 )
883                    {
884                       
885                        //
886                        // Chase bulge from top to bottom
887                        // Save cosines and sines for later singular vector updates
888                        //
889                        f = (Math.Abs(d[ll])-shift)*(extsignbdsqr(1, d[ll])+shift/d[ll]);
890                        g = e[ll];
891                        for(i=ll; i<=m-1; i++)
892                        {
893                            rotations.generaterotation(f, g, ref cosr, ref sinr, ref r);
894                            if( i>ll )
895                            {
896                                e[i-1] = r;
897                            }
898                            f = cosr*d[i]+sinr*e[i];
899                            e[i] = cosr*e[i]-sinr*d[i];
900                            g = sinr*d[i+1];
901                            d[i+1] = cosr*d[i+1];
902                            rotations.generaterotation(f, g, ref cosl, ref sinl, ref r);
903                            d[i] = r;
904                            f = cosl*e[i]+sinl*d[i+1];
905                            d[i+1] = cosl*d[i+1]-sinl*e[i];
906                            if( i<m-1 )
907                            {
908                                g = sinl*e[i+1];
909                                e[i+1] = cosl*e[i+1];
910                            }
911                            work0[i-ll+1] = cosr;
912                            work1[i-ll+1] = sinr;
913                            work2[i-ll+1] = cosl;
914                            work3[i-ll+1] = sinl;
915                        }
916                        e[m-1] = f;
917                       
918                        //
919                        // Update singular vectors
920                        //
921                        if( ncvt>0 )
922                        {
923                            rotations.applyrotationsfromtheleft(fwddir, ll+vstart-1, m+vstart-1, vstart, vend, ref work0, ref work1, ref vt, ref vttemp);
924                        }
925                        if( nru>0 )
926                        {
927                            rotations.applyrotationsfromtheright(fwddir, ustart, uend, ll+ustart-1, m+ustart-1, ref work2, ref work3, ref u, ref utemp);
928                        }
929                        if( ncc>0 )
930                        {
931                            rotations.applyrotationsfromtheleft(fwddir, ll+cstart-1, m+cstart-1, cstart, cend, ref work2, ref work3, ref c, ref ctemp);
932                        }
933                       
934                        //
935                        // Test convergence
936                        //
937                        if( Math.Abs(e[m-1])<=thresh )
938                        {
939                            e[m-1] = 0;
940                        }
941                    }
942                    else
943                    {
944                       
945                        //
946                        // Chase bulge from bottom to top
947                        // Save cosines and sines for later singular vector updates
948                        //
949                        f = (Math.Abs(d[m])-shift)*(extsignbdsqr(1, d[m])+shift/d[m]);
950                        g = e[m-1];
951                        for(i=m; i>=ll+1; i--)
952                        {
953                            rotations.generaterotation(f, g, ref cosr, ref sinr, ref r);
954                            if( i<m )
955                            {
956                                e[i] = r;
957                            }
958                            f = cosr*d[i]+sinr*e[i-1];
959                            e[i-1] = cosr*e[i-1]-sinr*d[i];
960                            g = sinr*d[i-1];
961                            d[i-1] = cosr*d[i-1];
962                            rotations.generaterotation(f, g, ref cosl, ref sinl, ref r);
963                            d[i] = r;
964                            f = cosl*e[i-1]+sinl*d[i-1];
965                            d[i-1] = cosl*d[i-1]-sinl*e[i-1];
966                            if( i>ll+1 )
967                            {
968                                g = sinl*e[i-2];
969                                e[i-2] = cosl*e[i-2];
970                            }
971                            work0[i-ll] = cosr;
972                            work1[i-ll] = -sinr;
973                            work2[i-ll] = cosl;
974                            work3[i-ll] = -sinl;
975                        }
976                        e[ll] = f;
977                       
978                        //
979                        // Test convergence
980                        //
981                        if( Math.Abs(e[ll])<=thresh )
982                        {
983                            e[ll] = 0;
984                        }
985                       
986                        //
987                        // Update singular vectors if desired
988                        //
989                        if( ncvt>0 )
990                        {
991                            rotations.applyrotationsfromtheleft(!fwddir, ll+vstart-1, m+vstart-1, vstart, vend, ref work2, ref work3, ref vt, ref vttemp);
992                        }
993                        if( nru>0 )
994                        {
995                            rotations.applyrotationsfromtheright(!fwddir, ustart, uend, ll+ustart-1, m+ustart-1, ref work0, ref work1, ref u, ref utemp);
996                        }
997                        if( ncc>0 )
998                        {
999                            rotations.applyrotationsfromtheleft(!fwddir, ll+cstart-1, m+cstart-1, cstart, cend, ref work0, ref work1, ref c, ref ctemp);
1000                        }
1001                    }
1002                }
1003               
1004                //
1005                // QR iteration finished, go back and check convergence
1006                //
1007                continue;
1008            }
1009           
1010            //
1011            // All singular values converged, so make them positive
1012            //
1013            for(i=1; i<=n; i++)
1014            {
1015                if( d[i]<0 )
1016                {
1017                    d[i] = -d[i];
1018                   
1019                    //
1020                    // Change sign of singular vectors, if desired
1021                    //
1022                    if( ncvt>0 )
1023                    {
1024                        for(i_=vstart; i_<=vend;i_++)
1025                        {
1026                            vt[i+vstart-1,i_] = -1*vt[i+vstart-1,i_];
1027                        }
1028                    }
1029                }
1030            }
1031           
1032            //
1033            // Sort the singular values into decreasing order (insertion sort on
1034            // singular values, but only one transposition per singular vector)
1035            //
1036            for(i=1; i<=n-1; i++)
1037            {
1038               
1039                //
1040                // Scan for smallest D(I)
1041                //
1042                isub = 1;
1043                smin = d[1];
1044                for(j=2; j<=n+1-i; j++)
1045                {
1046                    if( d[j]<=smin )
1047                    {
1048                        isub = j;
1049                        smin = d[j];
1050                    }
1051                }
1052                if( isub!=n+1-i )
1053                {
1054                   
1055                    //
1056                    // Swap singular values and vectors
1057                    //
1058                    d[isub] = d[n+1-i];
1059                    d[n+1-i] = smin;
1060                    if( ncvt>0 )
1061                    {
1062                        j = n+1-i;
1063                        for(i_=vstart; i_<=vend;i_++)
1064                        {
1065                            vttemp[i_] = vt[isub+vstart-1,i_];
1066                        }
1067                        for(i_=vstart; i_<=vend;i_++)
1068                        {
1069                            vt[isub+vstart-1,i_] = vt[j+vstart-1,i_];
1070                        }
1071                        for(i_=vstart; i_<=vend;i_++)
1072                        {
1073                            vt[j+vstart-1,i_] = vttemp[i_];
1074                        }
1075                    }
1076                    if( nru>0 )
1077                    {
1078                        j = n+1-i;
1079                        for(i_=ustart; i_<=uend;i_++)
1080                        {
1081                            utemp[i_] = u[i_,isub+ustart-1];
1082                        }
1083                        for(i_=ustart; i_<=uend;i_++)
1084                        {
1085                            u[i_,isub+ustart-1] = u[i_,j+ustart-1];
1086                        }
1087                        for(i_=ustart; i_<=uend;i_++)
1088                        {
1089                            u[i_,j+ustart-1] = utemp[i_];
1090                        }
1091                    }
1092                    if( ncc>0 )
1093                    {
1094                        j = n+1-i;
1095                        for(i_=cstart; i_<=cend;i_++)
1096                        {
1097                            ctemp[i_] = c[isub+cstart-1,i_];
1098                        }
1099                        for(i_=cstart; i_<=cend;i_++)
1100                        {
1101                            c[isub+cstart-1,i_] = c[j+cstart-1,i_];
1102                        }
1103                        for(i_=cstart; i_<=cend;i_++)
1104                        {
1105                            c[j+cstart-1,i_] = ctemp[i_];
1106                        }
1107                    }
1108                }
1109            }
1110            return result;
1111        }
1112
1113
1114        private static double extsignbdsqr(double a,
1115            double b)
1116        {
1117            double result = 0;
1118
1119            if( b>=0 )
1120            {
1121                result = Math.Abs(a);
1122            }
1123            else
1124            {
1125                result = -Math.Abs(a);
1126            }
1127            return result;
1128        }
1129
1130
1131        private static void svd2x2(double f,
1132            double g,
1133            double h,
1134            ref double ssmin,
1135            ref double ssmax)
1136        {
1137            double aas = 0;
1138            double at = 0;
1139            double au = 0;
1140            double c = 0;
1141            double fa = 0;
1142            double fhmn = 0;
1143            double fhmx = 0;
1144            double ga = 0;
1145            double ha = 0;
1146
1147            fa = Math.Abs(f);
1148            ga = Math.Abs(g);
1149            ha = Math.Abs(h);
1150            fhmn = Math.Min(fa, ha);
1151            fhmx = Math.Max(fa, ha);
1152            if( fhmn==0 )
1153            {
1154                ssmin = 0;
1155                if( fhmx==0 )
1156                {
1157                    ssmax = ga;
1158                }
1159                else
1160                {
1161                    ssmax = Math.Max(fhmx, ga)*Math.Sqrt(1+AP.Math.Sqr(Math.Min(fhmx, ga)/Math.Max(fhmx, ga)));
1162                }
1163            }
1164            else
1165            {
1166                if( ga<fhmx )
1167                {
1168                    aas = 1+fhmn/fhmx;
1169                    at = (fhmx-fhmn)/fhmx;
1170                    au = AP.Math.Sqr(ga/fhmx);
1171                    c = 2/(Math.Sqrt(aas*aas+au)+Math.Sqrt(at*at+au));
1172                    ssmin = fhmn*c;
1173                    ssmax = fhmx/c;
1174                }
1175                else
1176                {
1177                    au = fhmx/ga;
1178                    if( au==0 )
1179                    {
1180                       
1181                        //
1182                        // Avoid possible harmful underflow if exponent range
1183                        // asymmetric (true SSMIN may not underflow even if
1184                        // AU underflows)
1185                        //
1186                        ssmin = fhmn*fhmx/ga;
1187                        ssmax = ga;
1188                    }
1189                    else
1190                    {
1191                        aas = 1+fhmn/fhmx;
1192                        at = (fhmx-fhmn)/fhmx;
1193                        c = 1/(Math.Sqrt(1+AP.Math.Sqr(aas*au))+Math.Sqrt(1+AP.Math.Sqr(at*au)));
1194                        ssmin = fhmn*c*au;
1195                        ssmin = ssmin+ssmin;
1196                        ssmax = ga/(c+c);
1197                    }
1198                }
1199            }
1200        }
1201
1202
1203        private static void svdv2x2(double f,
1204            double g,
1205            double h,
1206            ref double ssmin,
1207            ref double ssmax,
1208            ref double snr,
1209            ref double csr,
1210            ref double snl,
1211            ref double csl)
1212        {
1213            bool gasmal = new bool();
1214            bool swp = new bool();
1215            int pmax = 0;
1216            double a = 0;
1217            double clt = 0;
1218            double crt = 0;
1219            double d = 0;
1220            double fa = 0;
1221            double ft = 0;
1222            double ga = 0;
1223            double gt = 0;
1224            double ha = 0;
1225            double ht = 0;
1226            double l = 0;
1227            double m = 0;
1228            double mm = 0;
1229            double r = 0;
1230            double s = 0;
1231            double slt = 0;
1232            double srt = 0;
1233            double t = 0;
1234            double temp = 0;
1235            double tsign = 0;
1236            double tt = 0;
1237            double v = 0;
1238
1239            ft = f;
1240            fa = Math.Abs(ft);
1241            ht = h;
1242            ha = Math.Abs(h);
1243           
1244            //
1245            // PMAX points to the maximum absolute element of matrix
1246            //  PMAX = 1 if F largest in absolute values
1247            //  PMAX = 2 if G largest in absolute values
1248            //  PMAX = 3 if H largest in absolute values
1249            //
1250            pmax = 1;
1251            swp = ha>fa;
1252            if( swp )
1253            {
1254               
1255                //
1256                // Now FA .ge. HA
1257                //
1258                pmax = 3;
1259                temp = ft;
1260                ft = ht;
1261                ht = temp;
1262                temp = fa;
1263                fa = ha;
1264                ha = temp;
1265            }
1266            gt = g;
1267            ga = Math.Abs(gt);
1268            if( ga==0 )
1269            {
1270               
1271                //
1272                // Diagonal matrix
1273                //
1274                ssmin = ha;
1275                ssmax = fa;
1276                clt = 1;
1277                crt = 1;
1278                slt = 0;
1279                srt = 0;
1280            }
1281            else
1282            {
1283                gasmal = true;
1284                if( ga>fa )
1285                {
1286                    pmax = 2;
1287                    if( fa/ga<AP.Math.MachineEpsilon )
1288                    {
1289                       
1290                        //
1291                        // Case of very large GA
1292                        //
1293                        gasmal = false;
1294                        ssmax = ga;
1295                        if( ha>1 )
1296                        {
1297                            v = ga/ha;
1298                            ssmin = fa/v;
1299                        }
1300                        else
1301                        {
1302                            v = fa/ga;
1303                            ssmin = v*ha;
1304                        }
1305                        clt = 1;
1306                        slt = ht/gt;
1307                        srt = 1;
1308                        crt = ft/gt;
1309                    }
1310                }
1311                if( gasmal )
1312                {
1313                   
1314                    //
1315                    // Normal case
1316                    //
1317                    d = fa-ha;
1318                    if( d==fa )
1319                    {
1320                        l = 1;
1321                    }
1322                    else
1323                    {
1324                        l = d/fa;
1325                    }
1326                    m = gt/ft;
1327                    t = 2-l;
1328                    mm = m*m;
1329                    tt = t*t;
1330                    s = Math.Sqrt(tt+mm);
1331                    if( l==0 )
1332                    {
1333                        r = Math.Abs(m);
1334                    }
1335                    else
1336                    {
1337                        r = Math.Sqrt(l*l+mm);
1338                    }
1339                    a = 0.5*(s+r);
1340                    ssmin = ha/a;
1341                    ssmax = fa*a;
1342                    if( mm==0 )
1343                    {
1344                       
1345                        //
1346                        // Note that M is very tiny
1347                        //
1348                        if( l==0 )
1349                        {
1350                            t = extsignbdsqr(2, ft)*extsignbdsqr(1, gt);
1351                        }
1352                        else
1353                        {
1354                            t = gt/extsignbdsqr(d, ft)+m/t;
1355                        }
1356                    }
1357                    else
1358                    {
1359                        t = (m/(s+t)+m/(r+l))*(1+a);
1360                    }
1361                    l = Math.Sqrt(t*t+4);
1362                    crt = 2/l;
1363                    srt = t/l;
1364                    clt = (crt+srt*m)/a;
1365                    v = ht/ft;
1366                    slt = v*srt/a;
1367                }
1368            }
1369            if( swp )
1370            {
1371                csl = srt;
1372                snl = crt;
1373                csr = slt;
1374                snr = clt;
1375            }
1376            else
1377            {
1378                csl = clt;
1379                snl = slt;
1380                csr = crt;
1381                snr = srt;
1382            }
1383           
1384            //
1385            // Correct signs of SSMAX and SSMIN
1386            //
1387            if( pmax==1 )
1388            {
1389                tsign = extsignbdsqr(1, csr)*extsignbdsqr(1, csl)*extsignbdsqr(1, f);
1390            }
1391            if( pmax==2 )
1392            {
1393                tsign = extsignbdsqr(1, snr)*extsignbdsqr(1, csl)*extsignbdsqr(1, g);
1394            }
1395            if( pmax==3 )
1396            {
1397                tsign = extsignbdsqr(1, snr)*extsignbdsqr(1, snl)*extsignbdsqr(1, h);
1398            }
1399            ssmax = extsignbdsqr(ssmax, tsign);
1400            ssmin = extsignbdsqr(ssmin, tsign*extsignbdsqr(1, f)*extsignbdsqr(1, h));
1401        }
1402    }
1403}
Note: See TracBrowser for help on using the repository browser.