1 /*****************************************************************************
2  *
3  *  Elmer, A Finite Element Software for Multiphysical Problems
4  *
5  *  Copyright 1st April 1995 - , CSC - IT Center for Science Ltd., Finland
6  *
7  *  This program is free software; you can redistribute it and/or
8  *  modify it under the terms of the GNU General Public License
9  *  as published by the Free Software Foundation; either version 2
10  *  of the License, or (at your option) any later version.
11  *
12  *  This program is distributed in the hope that it will be useful,
13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15  *  GNU General Public License for more details.
16  *
17  *  You should have received a copy of the GNU General Public License
18  *  along with this program (in file fem/GPL-2); if not, write to the
19  *  Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
20  *  Boston, MA 02110-1301, USA.
21  *
22  *****************************************************************************/
23 
24 /******************************************************************************
25  *
26  *
27  *
28  ******************************************************************************
29  *
30  *                     Author:       Juha Ruokolainen
31  *
32  *                    Address: CSC - IT Center for Science Ltd.
33  *                                Keilaranta 14, P.O. BOX 405
34  *                                  02
35  *                                  Tel. +358 0 457 2723
36  *                                Telefax: +358 0 457 2302
37  *                              EMail: Juha.Ruokolainen@csc.fi
38  *
39  *                       Date: 02 Jun 1997
40  *
41  *                Modified by:
42  *
43  *       Date of modification:
44  *
45  *****************************************************************************/
46 
47 #include <stdio.h>
48 #include <math.h>
49 #include <stdlib.h>
50 
51 #define MAX(x,y) ( (x) > (y) ? (x) : (y) )
52 #define MIN(x,y) ( (x) > (y) ? (y) : (x) )
53 #define ABS(x) ( (x) > 0 ? (x) : (-(x ) ) )
54 
55 #define AEPS 1.0e-15
56 
57 #define ALLOCMEM( s ) calloc( s, 1 )
58 #define FREEMEM( p ) free( p )
59 
60 #define A( i, j ) a[n * ( i ) + ( j )]
61 
62 void ludecomp( double *, int , int * );
63 
mtrinv(double * a,int n)64 void mtrinv( double *a, int n )
65 {
66     int i,j,k;
67     int *pivot;
68 
69     double s;
70 
71     pivot = (int *)ALLOCMEM(n*sizeof(int));
72 
73     /*
74      *  AP = LU
75      */
76     ludecomp( a,n,pivot );
77 
78     for( i=0; i<n; i++ )
79     {
80         if ( ABS(A(i,i))<AEPS )
81         {
82             fprintf( stderr, "Inv: Matrix is singular.\n" );
83             return;
84         }
85         A(i,i) = 1.0/A(i,i);
86     }
87 
88     /*
89      *  INV(U)
90      */
91     for( i=n-2; i>=0; i-- )
92         for( j=n-1; j>i; j-- )
93         {
94             s = -A(i,j);
95             for( k=i+1; k<j; k++ ) s -= A(i,k)*A(k,j);
96             A(i,j) = s;
97         }
98 
99     /*
100      * INV(L)
101      */
102     for( i=n-2; i>=0; i-- )
103         for( j=n-1; j>i; j-- )
104         {
105             s = 0.0;
106             for( k=i+1; k<=j; k++ ) s -= A(j,k)*A(k,i);
107             A(j,i) = A(i,i)*s;
108         }
109 
110     /*
111      * A  = INV(AP)
112      */
113     for( i=0; i<n; i++ )
114         for( j=0; j<n; j++ )
115         {
116             s = 0.0;
117             for( k=MAX(i,j); k<n; k++ )
118             {
119                 if ( k!=i )
120                     s += A(i,k)*A(k,j);
121                 else
122                     s += A(k,j);
123 
124                 A(i,j) = s;
125             }
126         }
127 
128     /*
129      * A = INV(A) (at last)
130      */
131     for( i=n-1; i>=0; i-- )
132         if ( pivot[i]!=i )
133             for( j=0; j<n; j++ )
134             {
135                 s = A(i,j);
136                 A(i,j) = A(pivot[i],j);
137                 A(pivot[i],j) = s;
138             }
139 
140     FREEMEM((char *)pivot);
141 }
142 
143 /*
144  * LU- decomposition by gaussian elimination. Row pivoting is used.
145  *
146  * result : AP = L'U ; L' = LD; pivot[i] is the swapped column number
147  * for column i.
148  *
149  * Result is stored in place of original matrix.
150  *
151  */
ludecomp(double * a,int n,int * pivot)152 void ludecomp( double *a, int n, int *pivot )
153 {
154     double swap;
155     int i,j,k,l;
156 
157     for( i=0; i<n-1; i++ )
158     {
159         j = i;
160         for( k=i+1; k<n; k++ ) if ( ABS(A(i,k)) > ABS(A(i,j)) ) j = k;
161 
162         if ( ABS(A(i,j)) < AEPS )
163         {
164             fprintf( stderr, "LUDecomp: Matrix is (at least almost) singular, %d %d.\n", i, j );
165             return;
166         }
167 
168         pivot[i] = j;
169 
170         if ( j != i )
171             for( k=0; k<=i; k++ )
172             {
173                 swap = A(k,j);
174                 A(k,j) = A(k,i);
175                 A(k,i) = swap;
176             }
177 
178         for( k=i+1; k<n; k++ ) A(i,k) /= A(i,i);
179 
180         for( k=i+1; k<n; k++ )
181         {
182             if ( j != i )
183             {
184                 swap = A(k,i);
185                 A(k,i) = A(k,j);
186                 A(k,j) = swap;
187             }
188 
189             for( l=i+1; l<n; l++ ) A(k,l) -= A(k,i) * A(i,l);
190         }
191     }
192 
193     pivot[n-1] = n-1;
194     if ( ABS(A(n-1,n-1))<AEPS )
195     {
196         fprintf( stderr, "LUDecomp: Matrix is (at least almost) singular.\n" );
197     }
198 }
199