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