Free cookie consent management tool by TermsFeed Policy Generator

source: trunk/sources/ALGLIB/ldlt.cs @ 2575

Last change on this file since 2575 was 2563, checked in by gkronber, 15 years ago

Updated ALGLIB to latest version. #751 (Plugin for for data-modeling with ANN (integrated into CEDMA))

File size: 50.9 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 ldlt
32    {
33        /*************************************************************************
34        LDLTDecomposition of a symmetric matrix
35
36        The algorithm represents a symmetric matrix (which is not necessarily
37        positive definite) as A=L*D*L' or A = U*D*U', where D is a block-diagonal
38        matrix with blocks 1x1 or 2x2, matrix L (matrix U) is a product of lower
39        (upper) triangular matrices with unit diagonal and permutation matrices.
40
41        Input parameters:
42            A       -   factorized matrix, array with elements [0..N-1, 0..N-1].
43                        If IsUpper – True, then the upper triangle contains
44                        elements of symmetric matrix A, and the lower triangle is
45                        not used.
46                        The same applies if IsUpper = False.
47            N       -   size of factorized matrix.
48            IsUpper -   parameter which shows a method of matrix definition (lower
49                        or upper triangle).
50
51        Output parameters:
52            A       -   matrices D and U, if IsUpper = True, or L, if IsUpper = False,
53                        in compact form, replacing the upper (lower) triangle of
54                        matrix A. In that case, the elements under (over) the main
55                        diagonal are not used nor modified.
56            Pivots  -   tables of performed permutations (see below).
57
58        If IsUpper = True, then A = U*D*U', U = P(n)*U(n)*...*P(k)*U(k), where
59        P(k) is the permutation matrix, U(k) - upper triangular matrix with its
60        unit main diagonal and k decreases from n with step s which is equal to
61        1 or 2 (according to the size of the blocks of matrix D).
62
63                (   I    v    0   )   k-s+1
64        U(k) =  (   0    I    0   )   s
65                (   0    0    I   )   n-k-1
66                   k-s+1 s   n-k-1
67
68        If Pivots[k]>=0, then s=1, P(k) - permutation of rows k and Pivots[k], the
69        vectorv forming matrix U(k) is stored in elements A(0:k-1,k), D(k) replaces
70        A(k,k). If Pivots[k]=Pivots[k-1]<0 then s=2, P(k) - permutation of rows k-1
71        and N+Pivots[k-1], the vector v forming matrix U(k) is stored in elements
72        A(0:k-1,k:k+1), the upper triangle of block D(k) is stored in A(k,k),
73        A(k,k+1) and A(k+1,k+1).
74
75        If IsUpper = False, then A = L*D*L', L=P(0)*L(0)*...*P(k)*L(k), where P(k)
76        is the permutation matrix, L(k) – lower triangular matrix with unit main
77        diagonal and k decreases from 1 with step s which is equal to 1 or 2
78        (according to the size of the blocks of matrix D).
79
80                (   I    0     0   )  k-1
81        L(k) =  (   0    I     0   )  s
82                (   0    v     I   )  n-k-s+1
83                   k-1   s  n-k-s+1
84
85        If Pivots[k]>=0 then s=1, P(k) – permutation of rows k and Pivots[k], the
86        vector v forming matrix L(k) is stored in elements A(k+1:n-1,k), D(k)
87        replaces A(k,k). If Pivots[k]=Pivots[k+1]<0 then s=2, P(k) - permutation
88        of rows k+1 and N+Pivots[k+1], the vector v forming matrix L(k) is stored
89        in elements A(k+2:n-1,k:k+1), the lower triangle of block D(k) is stored in
90        A(k,k), A(k+1,k) and A(k+1,k+1).
91
92          -- LAPACK routine (version 3.0) --
93             Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
94             Courant Institute, Argonne National Lab, and Rice University
95             June 30, 1999
96        *************************************************************************/
97        public static void smatrixldlt(ref double[,] a,
98            int n,
99            bool isupper,
100            ref int[] pivots)
101        {
102            int i = 0;
103            int imax = 0;
104            int j = 0;
105            int jmax = 0;
106            int k = 0;
107            int kk = 0;
108            int kp = 0;
109            int kstep = 0;
110            double absakk = 0;
111            double alpha = 0;
112            double colmax = 0;
113            double d11 = 0;
114            double d12 = 0;
115            double d21 = 0;
116            double d22 = 0;
117            double r1 = 0;
118            double rowmax = 0;
119            double t = 0;
120            double wk = 0;
121            double wkm1 = 0;
122            double wkp1 = 0;
123            int ii = 0;
124            int i1 = 0;
125            int i2 = 0;
126            double vv = 0;
127            double[] temp = new double[0];
128            int i_ = 0;
129
130            pivots = new int[n-1+1];
131            temp = new double[n-1+1];
132           
133            //
134            // Initialize ALPHA for use in choosing pivot block size.
135            //
136            alpha = (1+Math.Sqrt(17))/8;
137            if( isupper )
138            {
139               
140                //
141                // Factorize A as U*D*U' using the upper triangle of A
142                //
143                //
144                // K is the main loop index, decreasing from N to 1 in steps of
145                // 1 or 2
146                //
147                k = n-1;
148                while( k>=0 )
149                {
150                    kstep = 1;
151                   
152                    //
153                    // Determine rows and columns to be interchanged and whether
154                    // a 1-by-1 or 2-by-2 pivot block will be used
155                    //
156                    absakk = Math.Abs(a[k,k]);
157                   
158                    //
159                    // IMAX is the row-index of the largest off-diagonal element in
160                    // column K+1, and COLMAX is its absolute value
161                    //
162                    if( k>0 )
163                    {
164                        imax = 1;
165                        for(ii=2; ii<=k; ii++)
166                        {
167                            if( (double)(Math.Abs(a[ii-1,k]))>(double)(Math.Abs(a[imax-1,k])) )
168                            {
169                                imax = ii;
170                            }
171                        }
172                        colmax = Math.Abs(a[imax-1,k]);
173                    }
174                    else
175                    {
176                        colmax = 0;
177                    }
178                    if( (double)(Math.Max(absakk, colmax))==(double)(0) )
179                    {
180                       
181                        //
182                        // Column K is zero
183                        //
184                        kp = k;
185                    }
186                    else
187                    {
188                        if( (double)(absakk)>=(double)(alpha*colmax) )
189                        {
190                           
191                            //
192                            // no interchange, use 1-by-1 pivot block
193                            //
194                            kp = k;
195                        }
196                        else
197                        {
198                           
199                            //
200                            // JMAX is the column-index of the largest off-diagonal
201                            // element in row IMAX, and ROWMAX is its absolute value
202                            //
203                            jmax = imax+1;
204                            for(ii=imax+2; ii<=k+1; ii++)
205                            {
206                                if( (double)(Math.Abs(a[imax-1,ii-1]))>(double)(Math.Abs(a[imax-1,jmax-1])) )
207                                {
208                                    jmax = ii;
209                                }
210                            }
211                            rowmax = Math.Abs(a[imax-1,jmax-1]);
212                            if( imax>1 )
213                            {
214                                jmax = 1;
215                                for(ii=2; ii<=imax-1; ii++)
216                                {
217                                    if( (double)(Math.Abs(a[ii-1,imax-1]))>(double)(Math.Abs(a[jmax-1,imax-1])) )
218                                    {
219                                        jmax = ii;
220                                    }
221                                }
222                                rowmax = Math.Max(rowmax, Math.Abs(a[jmax-1,imax-1]));
223                            }
224                            vv = colmax/rowmax;
225                            if( (double)(absakk)>=(double)(alpha*colmax*vv) )
226                            {
227                               
228                                //
229                                // no interchange, use 1-by-1 pivot block
230                                //
231                                kp = k;
232                            }
233                            else
234                            {
235                                if( (double)(Math.Abs(a[imax-1,imax-1]))>=(double)(alpha*rowmax) )
236                                {
237                                   
238                                    //
239                                    // interchange rows and columns K and IMAX, use 1-by-1
240                                    // pivot block
241                                    //
242                                    kp = imax-1;
243                                }
244                                else
245                                {
246                                   
247                                    //
248                                    // interchange rows and columns K-1 and IMAX, use 2-by-2
249                                    // pivot block
250                                    //
251                                    kp = imax-1;
252                                    kstep = 2;
253                                }
254                            }
255                        }
256                        kk = k+1-kstep;
257                        if( kp+1!=kk+1 )
258                        {
259                           
260                            //
261                            // Interchange rows and columns KK and KP+1 in the leading
262                            // submatrix A(0:K,0:K)
263                            //
264                            for(i_=0; i_<=kp-1;i_++)
265                            {
266                                temp[i_] = a[i_,kk];
267                            }
268                            for(i_=0; i_<=kp-1;i_++)
269                            {
270                                a[i_,kk] = a[i_,kp];
271                            }
272                            for(i_=0; i_<=kp-1;i_++)
273                            {
274                                a[i_,kp] = temp[i_];
275                            }
276                            for(i_=kp+1; i_<=kk-1;i_++)
277                            {
278                                temp[i_] = a[i_,kk];
279                            }
280                            for(i_=kp+1; i_<=kk-1;i_++)
281                            {
282                                a[i_,kk] = a[kp,i_];
283                            }
284                            for(i_=kp+1; i_<=kk-1;i_++)
285                            {
286                                a[kp,i_] = temp[i_];
287                            }
288                            t = a[kk,kk];
289                            a[kk,kk] = a[kp,kp];
290                            a[kp,kp] = t;
291                            if( kstep==2 )
292                            {
293                                t = a[k-1,k];
294                                a[k-1,k] = a[kp,k];
295                                a[kp,k] = t;
296                            }
297                        }
298                       
299                        //
300                        // Update the leading submatrix
301                        //
302                        if( kstep==1 )
303                        {
304                           
305                            //
306                            // 1-by-1 pivot block D(k): column k now holds
307                            //
308                            // W(k) = U(k)*D(k)
309                            //
310                            // where U(k) is the k-th column of U
311                            //
312                            // Perform a rank-1 update of A(1:k-1,1:k-1) as
313                            //
314                            // A := A - U(k)*D(k)*U(k)' = A - W(k)*1/D(k)*W(k)'
315                            //
316                            r1 = 1/a[k,k];
317                            for(i=0; i<=k-1; i++)
318                            {
319                                vv = -(r1*a[i,k]);
320                                for(i_=i; i_<=k-1;i_++)
321                                {
322                                    a[i,i_] = a[i,i_] + vv*a[i_,k];
323                                }
324                            }
325                           
326                            //
327                            // Store U(K+1) in column K+1
328                            //
329                            for(i_=0; i_<=k-1;i_++)
330                            {
331                                a[i_,k] = r1*a[i_,k];
332                            }
333                        }
334                        else
335                        {
336                           
337                            //
338                            // 2-by-2 pivot block D(k): columns k and k-1 now hold
339                            //
340                            // ( W(k-1) W(k) ) = ( U(k-1) U(k) )*D(k)
341                            //
342                            // where U(k) and U(k-1) are the k-th and (k-1)-th columns
343                            // of U
344                            //
345                            // Perform a rank-2 update of A(1:k-2,1:k-2) as
346                            //
347                            // A := A - ( U(k-1) U(k) )*D(k)*( U(k-1) U(k) )'
348                            //    = A - ( W(k-1) W(k) )*inv(D(k))*( W(k-1) W(k) )'
349                            //
350                            if( k>1 )
351                            {
352                                d12 = a[k-1,k];
353                                d22 = a[k-1,k-1]/d12;
354                                d11 = a[k,k]/d12;
355                                t = 1/(d11*d22-1);
356                                d12 = t/d12;
357                                for(j=k-2; j>=0; j--)
358                                {
359                                    wkm1 = d12*(d11*a[j,k-1]-a[j,k]);
360                                    wk = d12*(d22*a[j,k]-a[j,k-1]);
361                                    for(i_=0; i_<=j;i_++)
362                                    {
363                                        a[i_,j] = a[i_,j] - wk*a[i_,k];
364                                    }
365                                    for(i_=0; i_<=j;i_++)
366                                    {
367                                        a[i_,j] = a[i_,j] - wkm1*a[i_,k-1];
368                                    }
369                                    a[j,k] = wk;
370                                    a[j,k-1] = wkm1;
371                                }
372                            }
373                        }
374                    }
375                   
376                    //
377                    // Store details of the interchanges in IPIV
378                    //
379                    if( kstep==1 )
380                    {
381                        pivots[k] = kp;
382                    }
383                    else
384                    {
385                        pivots[k] = kp-n;
386                        pivots[k-1] = kp-n;
387                    }
388                   
389                    //
390                    // Decrease K+1 and return to the start of the main loop
391                    //
392                    k = k-kstep;
393                }
394            }
395            else
396            {
397               
398                //
399                // Factorize A as L*D*L' using the lower triangle of A
400                //
401                // K+1 is the main loop index, increasing from 1 to N in steps of
402                // 1 or 2
403                //
404                k = 0;
405                while( k<=n-1 )
406                {
407                    kstep = 1;
408                   
409                    //
410                    // Determine rows and columns to be interchanged and whether
411                    // a 1-by-1 or 2-by-2 pivot block will be used
412                    //
413                    absakk = Math.Abs(a[k,k]);
414                   
415                    //
416                    // IMAX is the row-index of the largest off-diagonal element in
417                    // column K+1, and COLMAX is its absolute value
418                    //
419                    if( k<n-1 )
420                    {
421                        imax = k+1+1;
422                        for(ii=k+1+2; ii<=n; ii++)
423                        {
424                            if( (double)(Math.Abs(a[ii-1,k]))>(double)(Math.Abs(a[imax-1,k])) )
425                            {
426                                imax = ii;
427                            }
428                        }
429                        colmax = Math.Abs(a[imax-1,k]);
430                    }
431                    else
432                    {
433                        colmax = 0;
434                    }
435                    if( (double)(Math.Max(absakk, colmax))==(double)(0) )
436                    {
437                       
438                        //
439                        // Column K+1 is zero
440                        //
441                        kp = k;
442                    }
443                    else
444                    {
445                        if( (double)(absakk)>=(double)(alpha*colmax) )
446                        {
447                           
448                            //
449                            // no interchange, use 1-by-1 pivot block
450                            //
451                            kp = k;
452                        }
453                        else
454                        {
455                           
456                            //
457                            // JMAX is the column-index of the largest off-diagonal
458                            // element in row IMAX, and ROWMAX is its absolute value
459                            //
460                            jmax = k+1;
461                            for(ii=k+1+1; ii<=imax-1; ii++)
462                            {
463                                if( (double)(Math.Abs(a[imax-1,ii-1]))>(double)(Math.Abs(a[imax-1,jmax-1])) )
464                                {
465                                    jmax = ii;
466                                }
467                            }
468                            rowmax = Math.Abs(a[imax-1,jmax-1]);
469                            if( imax<n )
470                            {
471                                jmax = imax+1;
472                                for(ii=imax+2; ii<=n; ii++)
473                                {
474                                    if( (double)(Math.Abs(a[ii-1,imax-1]))>(double)(Math.Abs(a[jmax-1,imax-1])) )
475                                    {
476                                        jmax = ii;
477                                    }
478                                }
479                                rowmax = Math.Max(rowmax, Math.Abs(a[jmax-1,imax-1]));
480                            }
481                            vv = colmax/rowmax;
482                            if( (double)(absakk)>=(double)(alpha*colmax*vv) )
483                            {
484                               
485                                //
486                                // no interchange, use 1-by-1 pivot block
487                                //
488                                kp = k;
489                            }
490                            else
491                            {
492                                if( (double)(Math.Abs(a[imax-1,imax-1]))>=(double)(alpha*rowmax) )
493                                {
494                                   
495                                    //
496                                    // interchange rows and columns K+1 and IMAX, use 1-by-1
497                                    // pivot block
498                                    //
499                                    kp = imax-1;
500                                }
501                                else
502                                {
503                                   
504                                    //
505                                    // interchange rows and columns K+1+1 and IMAX, use 2-by-2
506                                    // pivot block
507                                    //
508                                    kp = imax-1;
509                                    kstep = 2;
510                                }
511                            }
512                        }
513                        kk = k+kstep-1;
514                        if( kp!=kk )
515                        {
516                           
517                            //
518                            //              Interchange rows and columns KK+1 and KP+1 in the trailing
519                            //              submatrix A(K+1:n,K+1:n)
520                            //
521                            if( kp+1<n )
522                            {
523                                for(i_=kp+1; i_<=n-1;i_++)
524                                {
525                                    temp[i_] = a[i_,kk];
526                                }
527                                for(i_=kp+1; i_<=n-1;i_++)
528                                {
529                                    a[i_,kk] = a[i_,kp];
530                                }
531                                for(i_=kp+1; i_<=n-1;i_++)
532                                {
533                                    a[i_,kp] = temp[i_];
534                                }
535                            }
536                            for(i_=kk+1; i_<=kp-1;i_++)
537                            {
538                                temp[i_] = a[i_,kk];
539                            }
540                            for(i_=kk+1; i_<=kp-1;i_++)
541                            {
542                                a[i_,kk] = a[kp,i_];
543                            }
544                            for(i_=kk+1; i_<=kp-1;i_++)
545                            {
546                                a[kp,i_] = temp[i_];
547                            }
548                            t = a[kk,kk];
549                            a[kk,kk] = a[kp,kp];
550                            a[kp,kp] = t;
551                            if( kstep==2 )
552                            {
553                                t = a[k+1,k];
554                                a[k+1,k] = a[kp,k];
555                                a[kp,k] = t;
556                            }
557                        }
558                       
559                        //
560                        // Update the trailing submatrix
561                        //
562                        if( kstep==1 )
563                        {
564                           
565                            //
566                            // 1-by-1 pivot block D(K+1): column K+1 now holds
567                            //
568                            // W(K+1) = L(K+1)*D(K+1)
569                            //
570                            // where L(K+1) is the K+1-th column of L
571                            //
572                            if( k+1<n )
573                            {
574                               
575                                //
576                                // Perform a rank-1 update of A(K+1+1:n,K+1+1:n) as
577                                //
578                                // A := A - L(K+1)*D(K+1)*L(K+1)' = A - W(K+1)*(1/D(K+1))*W(K+1)'
579                                //
580                                d11 = 1/a[k+1-1,k+1-1];
581                                for(ii=k+1; ii<=n-1; ii++)
582                                {
583                                    vv = -(d11*a[ii,k]);
584                                    for(i_=k+1; i_<=ii;i_++)
585                                    {
586                                        a[ii,i_] = a[ii,i_] + vv*a[i_,k];
587                                    }
588                                }
589                               
590                                //
591                                // Store L(K+1) in column K+1
592                                //
593                                for(i_=k+1; i_<=n-1;i_++)
594                                {
595                                    a[i_,k] = d11*a[i_,k];
596                                }
597                            }
598                        }
599                        else
600                        {
601                           
602                            //
603                            // 2-by-2 pivot block D(K+1)
604                            //
605                            if( k<n-2 )
606                            {
607                               
608                                //
609                                // Perform a rank-2 update of A(K+1+2:n,K+1+2:n) as
610                                //
611                                // A := A - ( (A(K+1) A(K+1+1))*D(K+1)**(-1) ) * (A(K+1) A(K+1+1))'
612                                //
613                                // where L(K+1) and L(K+1+1) are the K+1-th and (K+1+1)-th
614                                // columns of L
615                                //
616                                d21 = a[k+1,k];
617                                d11 = a[k+1,k+1]/d21;
618                                d22 = a[k,k]/d21;
619                                t = 1/(d11*d22-1);
620                                d21 = t/d21;
621                                for(j=k+2; j<=n-1; j++)
622                                {
623                                    wk = d21*(d11*a[j,k]-a[j,k+1]);
624                                    wkp1 = d21*(d22*a[j,k+1]-a[j,k]);
625                                    for(i_=j; i_<=n-1;i_++)
626                                    {
627                                        a[i_,j] = a[i_,j] - wk*a[i_,k];
628                                    }
629                                    for(i_=j; i_<=n-1;i_++)
630                                    {
631                                        a[i_,j] = a[i_,j] - wkp1*a[i_,k+1];
632                                    }
633                                    a[j,k] = wk;
634                                    a[j,k+1] = wkp1;
635                                }
636                            }
637                        }
638                    }
639                   
640                    //
641                    // Store details of the interchanges in IPIV
642                    //
643                    if( kstep==1 )
644                    {
645                        pivots[k+1-1] = kp+1-1;
646                    }
647                    else
648                    {
649                        pivots[k+1-1] = kp+1-1-n;
650                        pivots[k+1+1-1] = kp+1-1-n;
651                    }
652                   
653                    //
654                    // Increase K+1 and return to the start of the main loop
655                    //
656                    k = k+kstep;
657                }
658            }
659        }
660
661
662        public static void ldltdecomposition(ref double[,] a,
663            int n,
664            bool isupper,
665            ref int[] pivots)
666        {
667            int i = 0;
668            int imax = 0;
669            int j = 0;
670            int jmax = 0;
671            int k = 0;
672            int kk = 0;
673            int kp = 0;
674            int kstep = 0;
675            double absakk = 0;
676            double alpha = 0;
677            double colmax = 0;
678            double d11 = 0;
679            double d12 = 0;
680            double d21 = 0;
681            double d22 = 0;
682            double r1 = 0;
683            double rowmax = 0;
684            double t = 0;
685            double wk = 0;
686            double wkm1 = 0;
687            double wkp1 = 0;
688            int ii = 0;
689            int i1 = 0;
690            int i2 = 0;
691            double vv = 0;
692            double[] temp = new double[0];
693            int i_ = 0;
694
695            pivots = new int[n+1];
696            temp = new double[n+1];
697           
698            //
699            // Initialize ALPHA for use in choosing pivot block size.
700            //
701            alpha = (1+Math.Sqrt(17))/8;
702            if( isupper )
703            {
704               
705                //
706                // Factorize A as U*D*U' using the upper triangle of A
707                //
708                //
709                // K is the main loop index, decreasing from N to 1 in steps of
710                // 1 or 2
711                //
712                k = n;
713                while( k>=1 )
714                {
715                    kstep = 1;
716                   
717                    //
718                    // Determine rows and columns to be interchanged and whether
719                    // a 1-by-1 or 2-by-2 pivot block will be used
720                    //
721                    absakk = Math.Abs(a[k,k]);
722                   
723                    //
724                    // IMAX is the row-index of the largest off-diagonal element in
725                    // column K, and COLMAX is its absolute value
726                    //
727                    if( k>1 )
728                    {
729                        imax = 1;
730                        for(ii=2; ii<=k-1; ii++)
731                        {
732                            if( (double)(Math.Abs(a[ii,k]))>(double)(Math.Abs(a[imax,k])) )
733                            {
734                                imax = ii;
735                            }
736                        }
737                        colmax = Math.Abs(a[imax,k]);
738                    }
739                    else
740                    {
741                        colmax = 0;
742                    }
743                    if( (double)(Math.Max(absakk, colmax))==(double)(0) )
744                    {
745                       
746                        //
747                        // Column K is zero
748                        //
749                        kp = k;
750                    }
751                    else
752                    {
753                        if( (double)(absakk)>=(double)(alpha*colmax) )
754                        {
755                           
756                            //
757                            // no interchange, use 1-by-1 pivot block
758                            //
759                            kp = k;
760                        }
761                        else
762                        {
763                           
764                            //
765                            // JMAX is the column-index of the largest off-diagonal
766                            // element in row IMAX, and ROWMAX is its absolute value
767                            //
768                            jmax = imax+1;
769                            for(ii=imax+2; ii<=k; ii++)
770                            {
771                                if( (double)(Math.Abs(a[imax,ii]))>(double)(Math.Abs(a[imax,jmax])) )
772                                {
773                                    jmax = ii;
774                                }
775                            }
776                            rowmax = Math.Abs(a[imax,jmax]);
777                            if( imax>1 )
778                            {
779                                jmax = 1;
780                                for(ii=2; ii<=imax-1; ii++)
781                                {
782                                    if( (double)(Math.Abs(a[ii,imax]))>(double)(Math.Abs(a[jmax,imax])) )
783                                    {
784                                        jmax = ii;
785                                    }
786                                }
787                                rowmax = Math.Max(rowmax, Math.Abs(a[jmax,imax]));
788                            }
789                            vv = colmax/rowmax;
790                            if( (double)(absakk)>=(double)(alpha*colmax*vv) )
791                            {
792                               
793                                //
794                                // no interchange, use 1-by-1 pivot block
795                                //
796                                kp = k;
797                            }
798                            else
799                            {
800                                if( (double)(Math.Abs(a[imax,imax]))>=(double)(alpha*rowmax) )
801                                {
802                                   
803                                    //
804                                    // interchange rows and columns K and IMAX, use 1-by-1
805                                    // pivot block
806                                    //
807                                    kp = imax;
808                                }
809                                else
810                                {
811                                   
812                                    //
813                                    // interchange rows and columns K-1 and IMAX, use 2-by-2
814                                    // pivot block
815                                    //
816                                    kp = imax;
817                                    kstep = 2;
818                                }
819                            }
820                        }
821                        kk = k-kstep+1;
822                        if( kp!=kk )
823                        {
824                           
825                            //
826                            // Interchange rows and columns KK and KP in the leading
827                            // submatrix A(1:k,1:k)
828                            //
829                            i1 = kp-1;
830                            for(i_=1; i_<=i1;i_++)
831                            {
832                                temp[i_] = a[i_,kk];
833                            }
834                            for(i_=1; i_<=i1;i_++)
835                            {
836                                a[i_,kk] = a[i_,kp];
837                            }
838                            for(i_=1; i_<=i1;i_++)
839                            {
840                                a[i_,kp] = temp[i_];
841                            }
842                            i1 = kp+1;
843                            i2 = kk-1;
844                            for(i_=i1; i_<=i2;i_++)
845                            {
846                                temp[i_] = a[i_,kk];
847                            }
848                            for(i_=i1; i_<=i2;i_++)
849                            {
850                                a[i_,kk] = a[kp,i_];
851                            }
852                            for(i_=i1; i_<=i2;i_++)
853                            {
854                                a[kp,i_] = temp[i_];
855                            }
856                            t = a[kk,kk];
857                            a[kk,kk] = a[kp,kp];
858                            a[kp,kp] = t;
859                            if( kstep==2 )
860                            {
861                                t = a[k-1,k];
862                                a[k-1,k] = a[kp,k];
863                                a[kp,k] = t;
864                            }
865                        }
866                       
867                        //
868                        // Update the leading submatrix
869                        //
870                        if( kstep==1 )
871                        {
872                           
873                            //
874                            // 1-by-1 pivot block D(k): column k now holds
875                            //
876                            // W(k) = U(k)*D(k)
877                            //
878                            // where U(k) is the k-th column of U
879                            //
880                            // Perform a rank-1 update of A(1:k-1,1:k-1) as
881                            //
882                            // A := A - U(k)*D(k)*U(k)' = A - W(k)*1/D(k)*W(k)'
883                            //
884                            r1 = 1/a[k,k];
885                            for(i=1; i<=k-1; i++)
886                            {
887                                i2 = k-1;
888                                vv = -(r1*a[i,k]);
889                                for(i_=i; i_<=i2;i_++)
890                                {
891                                    a[i,i_] = a[i,i_] + vv*a[i_,k];
892                                }
893                            }
894                           
895                            //
896                            // Store U(k) in column k
897                            //
898                            i2 = k-1;
899                            for(i_=1; i_<=i2;i_++)
900                            {
901                                a[i_,k] = r1*a[i_,k];
902                            }
903                        }
904                        else
905                        {
906                           
907                            //
908                            // 2-by-2 pivot block D(k): columns k and k-1 now hold
909                            //
910                            // ( W(k-1) W(k) ) = ( U(k-1) U(k) )*D(k)
911                            //
912                            // where U(k) and U(k-1) are the k-th and (k-1)-th columns
913                            // of U
914                            //
915                            // Perform a rank-2 update of A(1:k-2,1:k-2) as
916                            //
917                            // A := A - ( U(k-1) U(k) )*D(k)*( U(k-1) U(k) )'
918                            //    = A - ( W(k-1) W(k) )*inv(D(k))*( W(k-1) W(k) )'
919                            //
920                            if( k>2 )
921                            {
922                                d12 = a[k-1,k];
923                                d22 = a[k-1,k-1]/d12;
924                                d11 = a[k,k]/d12;
925                                t = 1/(d11*d22-1);
926                                d12 = t/d12;
927                                for(j=k-2; j>=1; j--)
928                                {
929                                    wkm1 = d12*(d11*a[j,k-1]-a[j,k]);
930                                    wk = d12*(d22*a[j,k]-a[j,k-1]);
931                                    for(i_=1; i_<=j;i_++)
932                                    {
933                                        a[i_,j] = a[i_,j] - wk*a[i_,k];
934                                    }
935                                    i1 = k-1;
936                                    for(i_=1; i_<=j;i_++)
937                                    {
938                                        a[i_,j] = a[i_,j] - wkm1*a[i_,i1];
939                                    }
940                                    a[j,k] = wk;
941                                    a[j,k-1] = wkm1;
942                                }
943                            }
944                        }
945                    }
946                   
947                    //
948                    // Store details of the interchanges in IPIV
949                    //
950                    if( kstep==1 )
951                    {
952                        pivots[k] = kp;
953                    }
954                    else
955                    {
956                        pivots[k] = -kp;
957                        pivots[k-1] = -kp;
958                    }
959                   
960                    //
961                    // Decrease K and return to the start of the main loop
962                    //
963                    k = k-kstep;
964                }
965            }
966            else
967            {
968               
969                //
970                // Factorize A as L*D*L' using the lower triangle of A
971                //
972                // K is the main loop index, increasing from 1 to N in steps of
973                // 1 or 2
974                //
975                k = 1;
976                while( k<=n )
977                {
978                    kstep = 1;
979                   
980                    //
981                    // Determine rows and columns to be interchanged and whether
982                    // a 1-by-1 or 2-by-2 pivot block will be used
983                    //
984                    absakk = Math.Abs(a[k,k]);
985                   
986                    //
987                    // IMAX is the row-index of the largest off-diagonal element in
988                    // column K, and COLMAX is its absolute value
989                    //
990                    if( k<n )
991                    {
992                        imax = k+1;
993                        for(ii=k+2; ii<=n; ii++)
994                        {
995                            if( (double)(Math.Abs(a[ii,k]))>(double)(Math.Abs(a[imax,k])) )
996                            {
997                                imax = ii;
998                            }
999                        }
1000                        colmax = Math.Abs(a[imax,k]);
1001                    }
1002                    else
1003                    {
1004                        colmax = 0;
1005                    }
1006                    if( (double)(Math.Max(absakk, colmax))==(double)(0) )
1007                    {
1008                       
1009                        //
1010                        // Column K is zero
1011                        //
1012                        kp = k;
1013                    }
1014                    else
1015                    {
1016                        if( (double)(absakk)>=(double)(alpha*colmax) )
1017                        {
1018                           
1019                            //
1020                            // no interchange, use 1-by-1 pivot block
1021                            //
1022                            kp = k;
1023                        }
1024                        else
1025                        {
1026                           
1027                            //
1028                            // JMAX is the column-index of the largest off-diagonal
1029                            // element in row IMAX, and ROWMAX is its absolute value
1030                            //
1031                            jmax = k;
1032                            for(ii=k+1; ii<=imax-1; ii++)
1033                            {
1034                                if( (double)(Math.Abs(a[imax,ii]))>(double)(Math.Abs(a[imax,jmax])) )
1035                                {
1036                                    jmax = ii;
1037                                }
1038                            }
1039                            rowmax = Math.Abs(a[imax,jmax]);
1040                            if( imax<n )
1041                            {
1042                                jmax = imax+1;
1043                                for(ii=imax+2; ii<=n; ii++)
1044                                {
1045                                    if( (double)(Math.Abs(a[ii,imax]))>(double)(Math.Abs(a[jmax,imax])) )
1046                                    {
1047                                        jmax = ii;
1048                                    }
1049                                }
1050                                rowmax = Math.Max(rowmax, Math.Abs(a[jmax,imax]));
1051                            }
1052                            vv = colmax/rowmax;
1053                            if( (double)(absakk)>=(double)(alpha*colmax*vv) )
1054                            {
1055                               
1056                                //
1057                                // no interchange, use 1-by-1 pivot block
1058                                //
1059                                kp = k;
1060                            }
1061                            else
1062                            {
1063                                if( (double)(Math.Abs(a[imax,imax]))>=(double)(alpha*rowmax) )
1064                                {
1065                                   
1066                                    //
1067                                    // interchange rows and columns K and IMAX, use 1-by-1
1068                                    // pivot block
1069                                    //
1070                                    kp = imax;
1071                                }
1072                                else
1073                                {
1074                                   
1075                                    //
1076                                    // interchange rows and columns K+1 and IMAX, use 2-by-2
1077                                    // pivot block
1078                                    //
1079                                    kp = imax;
1080                                    kstep = 2;
1081                                }
1082                            }
1083                        }
1084                        kk = k+kstep-1;
1085                        if( kp!=kk )
1086                        {
1087                           
1088                            //
1089                            //              Interchange rows and columns KK and KP in the trailing
1090                            //              submatrix A(k:n,k:n)
1091                            //
1092                            if( kp<n )
1093                            {
1094                                i1 = kp+1;
1095                                for(i_=i1; i_<=n;i_++)
1096                                {
1097                                    temp[i_] = a[i_,kk];
1098                                }
1099                                for(i_=i1; i_<=n;i_++)
1100                                {
1101                                    a[i_,kk] = a[i_,kp];
1102                                }
1103                                for(i_=i1; i_<=n;i_++)
1104                                {
1105                                    a[i_,kp] = temp[i_];
1106                                }
1107                            }
1108                            i1 = kk+1;
1109                            i2 = kp-1;
1110                            for(i_=i1; i_<=i2;i_++)
1111                            {
1112                                temp[i_] = a[i_,kk];
1113                            }
1114                            for(i_=i1; i_<=i2;i_++)
1115                            {
1116                                a[i_,kk] = a[kp,i_];
1117                            }
1118                            for(i_=i1; i_<=i2;i_++)
1119                            {
1120                                a[kp,i_] = temp[i_];
1121                            }
1122                            t = a[kk,kk];
1123                            a[kk,kk] = a[kp,kp];
1124                            a[kp,kp] = t;
1125                            if( kstep==2 )
1126                            {
1127                                t = a[k+1,k];
1128                                a[k+1,k] = a[kp,k];
1129                                a[kp,k] = t;
1130                            }
1131                        }
1132                       
1133                        //
1134                        // Update the trailing submatrix
1135                        //
1136                        if( kstep==1 )
1137                        {
1138                           
1139                            //
1140                            // 1-by-1 pivot block D(k): column k now holds
1141                            //
1142                            // W(k) = L(k)*D(k)
1143                            //
1144                            // where L(k) is the k-th column of L
1145                            //
1146                            if( k<n )
1147                            {
1148                               
1149                                //
1150                                // Perform a rank-1 update of A(k+1:n,k+1:n) as
1151                                //
1152                                // A := A - L(k)*D(k)*L(k)' = A - W(k)*(1/D(k))*W(k)'
1153                                //
1154                                d11 = 1/a[k,k];
1155                                for(ii=k+1; ii<=n; ii++)
1156                                {
1157                                    i1 = k+1;
1158                                    i2 = ii;
1159                                    vv = -(d11*a[ii,k]);
1160                                    for(i_=i1; i_<=i2;i_++)
1161                                    {
1162                                        a[ii,i_] = a[ii,i_] + vv*a[i_,k];
1163                                    }
1164                                }
1165                               
1166                                //
1167                                // Store L(k) in column K
1168                                //
1169                                i1 = k+1;
1170                                for(i_=i1; i_<=n;i_++)
1171                                {
1172                                    a[i_,k] = d11*a[i_,k];
1173                                }
1174                            }
1175                        }
1176                        else
1177                        {
1178                           
1179                            //
1180                            // 2-by-2 pivot block D(k)
1181                            //
1182                            if( k<n-1 )
1183                            {
1184                               
1185                                //
1186                                // Perform a rank-2 update of A(k+2:n,k+2:n) as
1187                                //
1188                                // A := A - ( (A(k) A(k+1))*D(k)**(-1) ) * (A(k) A(k+1))'
1189                                //
1190                                // where L(k) and L(k+1) are the k-th and (k+1)-th
1191                                // columns of L
1192                                //
1193                                d21 = a[k+1,k];
1194                                d11 = a[k+1,k+1]/d21;
1195                                d22 = a[k,k]/d21;
1196                                t = 1/(d11*d22-1);
1197                                d21 = t/d21;
1198                                for(j=k+2; j<=n; j++)
1199                                {
1200                                    wk = d21*(d11*a[j,k]-a[j,k+1]);
1201                                    wkp1 = d21*(d22*a[j,k+1]-a[j,k]);
1202                                    ii = k+1;
1203                                    for(i_=j; i_<=n;i_++)
1204                                    {
1205                                        a[i_,j] = a[i_,j] - wk*a[i_,k];
1206                                    }
1207                                    for(i_=j; i_<=n;i_++)
1208                                    {
1209                                        a[i_,j] = a[i_,j] - wkp1*a[i_,ii];
1210                                    }
1211                                    a[j,k] = wk;
1212                                    a[j,k+1] = wkp1;
1213                                }
1214                            }
1215                        }
1216                    }
1217                   
1218                    //
1219                    // Store details of the interchanges in IPIV
1220                    //
1221                    if( kstep==1 )
1222                    {
1223                        pivots[k] = kp;
1224                    }
1225                    else
1226                    {
1227                        pivots[k] = -kp;
1228                        pivots[k+1] = -kp;
1229                    }
1230                   
1231                    //
1232                    // Increase K and return to the start of the main loop
1233                    //
1234                    k = k+kstep;
1235                }
1236            }
1237        }
1238    }
1239}
Note: See TracBrowser for help on using the repository browser.