1 /* $Id: matrix.c,v 1.8 2003/04/07 16:27:10 ukai Exp $ */
2 /*
3 * matrix.h, matrix.c: Liner equation solver using LU decomposition.
4 *
5 * by K.Okabe Aug. 1999
6 *
7 * LUfactor, LUsolve, Usolve and Lsolve, are based on the functions in
8 * Meschach Library Version 1.2b.
9 */
10
11 /**************************************************************************
12 **
13 ** Copyright (C) 1993 David E. Steward & Zbigniew Leyk, all rights reserved.
14 **
15 ** Meschach Library
16 **
17 ** This Meschach Library is provided "as is" without any express
18 ** or implied warranty of any kind with respect to this software.
19 ** In particular the authors shall not be liable for any direct,
20 ** indirect, special, incidental or consequential damages arising
21 ** in any way from use of the software.
22 **
23 ** Everyone is granted permission to copy, modify and redistribute this
24 ** Meschach Library, provided:
25 ** 1. All copies contain this copyright notice.
26 ** 2. All modified copies shall carry a notice stating who
27 ** made the last modification and the date of such modification.
28 ** 3. No charge is made for this software or works derived from it.
29 ** This clause shall not be construed as constraining other software
30 ** distributed on the same medium as this software, nor is a
31 ** distribution fee considered a charge.
32 **
33 ***************************************************************************/
34
35 #include "config.h"
36 #include "matrix.h"
37 #include "alloc.h"
38
39 /*
40 * Macros from "fm.h".
41 */
42
43 #define SWAPD(a,b) { double tmp = a; a = b; b = tmp; }
44 #define SWAPI(a,b) { int tmp = a; a = b; b = tmp; }
45
46 #ifdef HAVE_FLOAT_H
47 #include <float.h>
48 #endif /* not HAVE_FLOAT_H */
49 #if defined(DBL_MAX)
50 static double Tiny = 10.0 / DBL_MAX;
51 #elif defined(FLT_MAX)
52 static double Tiny = 10.0 / FLT_MAX;
53 #else /* not defined(FLT_MAX) */
54 static double Tiny = 1.0e-30;
55 #endif /* not defined(FLT_MAX */
56
57 /*
58 * LUfactor -- gaussian elimination with scaled partial pivoting
59 * -- Note: returns LU matrix which is A.
60 */
61
62 int
LUfactor(Matrix A,int * indexarray)63 LUfactor(Matrix A, int *indexarray)
64 {
65 int dim = A->dim, i, j, k, i_max, k_max;
66 Vector scale;
67 double mx, tmp;
68
69 scale = new_vector(dim);
70
71 for (i = 0; i < dim; i++)
72 indexarray[i] = i;
73
74 for (i = 0; i < dim; i++) {
75 mx = 0.;
76 for (j = 0; j < dim; j++) {
77 tmp = fabs(M_VAL(A, i, j));
78 if (mx < tmp)
79 mx = tmp;
80 }
81 scale->ve[i] = mx;
82 }
83
84 k_max = dim - 1;
85 for (k = 0; k < k_max; k++) {
86 mx = 0.;
87 i_max = -1;
88 for (i = k; i < dim; i++) {
89 if (fabs(scale->ve[i]) >= Tiny * fabs(M_VAL(A, i, k))) {
90 tmp = fabs(M_VAL(A, i, k)) / scale->ve[i];
91 if (mx < tmp) {
92 mx = tmp;
93 i_max = i;
94 }
95 }
96 }
97 if (i_max == -1) {
98 M_VAL(A, k, k) = 0.;
99 continue;
100 }
101
102 if (i_max != k) {
103 SWAPI(indexarray[i_max], indexarray[k]);
104 for (j = 0; j < dim; j++)
105 SWAPD(M_VAL(A, i_max, j), M_VAL(A, k, j));
106 }
107
108 for (i = k + 1; i < dim; i++) {
109 tmp = M_VAL(A, i, k) = M_VAL(A, i, k) / M_VAL(A, k, k);
110 for (j = k + 1; j < dim; j++)
111 M_VAL(A, i, j) -= tmp * M_VAL(A, k, j);
112 }
113 }
114 return 0;
115 }
116
117 /*
118 * LUsolve -- given an LU factorisation in A, solve Ax=b.
119 */
120
121 int
LUsolve(Matrix A,int * indexarray,Vector b,Vector x)122 LUsolve(Matrix A, int *indexarray, Vector b, Vector x)
123 {
124 int i, dim = A->dim;
125
126 for (i = 0; i < dim; i++)
127 x->ve[i] = b->ve[indexarray[i]];
128
129 if (Lsolve(A, x, x, 1.) == -1 || Usolve(A, x, x, 0.) == -1)
130 return -1;
131 return 0;
132 }
133
134 /* m_inverse -- returns inverse of A, provided A is not too rank deficient
135 * -- uses LU factorisation */
136 #if 0
137 Matrix
138 m_inverse(Matrix A, Matrix out)
139 {
140 int *indexarray = NewAtom_N(int, A->dim);
141 Matrix A1 = new_matrix(A->dim);
142 m_copy(A, A1);
143 LUfactor(A1, indexarray);
144 return LUinverse(A1, indexarray, out);
145 }
146 #endif /* 0 */
147
148 Matrix
LUinverse(Matrix A,int * indexarray,Matrix out)149 LUinverse(Matrix A, int *indexarray, Matrix out)
150 {
151 int i, j, dim = A->dim;
152 Vector tmp, tmp2;
153
154 if (!out)
155 out = new_matrix(dim);
156 tmp = new_vector(dim);
157 tmp2 = new_vector(dim);
158 for (i = 0; i < dim; i++) {
159 for (j = 0; j < dim; j++)
160 tmp->ve[j] = 0.;
161 tmp->ve[i] = 1.;
162 if (LUsolve(A, indexarray, tmp, tmp2) == -1)
163 return NULL;
164 for (j = 0; j < dim; j++)
165 M_VAL(out, j, i) = tmp2->ve[j];
166 }
167 return out;
168 }
169
170 /*
171 * Usolve -- back substitution with optional over-riding diagonal
172 * -- can be in-situ but doesn't need to be.
173 */
174
175 int
Usolve(Matrix mat,Vector b,Vector out,double diag)176 Usolve(Matrix mat, Vector b, Vector out, double diag)
177 {
178 int i, j, i_lim, dim = mat->dim;
179 double sum;
180
181 for (i = dim - 1; i >= 0; i--) {
182 if (b->ve[i] != 0.)
183 break;
184 else
185 out->ve[i] = 0.;
186 }
187 i_lim = i;
188
189 for (; i >= 0; i--) {
190 sum = b->ve[i];
191 for (j = i + 1; j <= i_lim; j++)
192 sum -= M_VAL(mat, i, j) * out->ve[j];
193 if (diag == 0.) {
194 if (fabs(M_VAL(mat, i, i)) <= Tiny * fabs(sum))
195 return -1;
196 else
197 out->ve[i] = sum / M_VAL(mat, i, i);
198 }
199 else
200 out->ve[i] = sum / diag;
201 }
202
203 return 0;
204 }
205
206 /*
207 * Lsolve -- forward elimination with (optional) default diagonal value.
208 */
209
210 int
Lsolve(Matrix mat,Vector b,Vector out,double diag)211 Lsolve(Matrix mat, Vector b, Vector out, double diag)
212 {
213 int i, j, i_lim, dim = mat->dim;
214 double sum;
215
216 for (i = 0; i < dim; i++) {
217 if (b->ve[i] != 0.)
218 break;
219 else
220 out->ve[i] = 0.;
221 }
222 i_lim = i;
223
224 for (; i < dim; i++) {
225 sum = b->ve[i];
226 for (j = i_lim; j < i; j++)
227 sum -= M_VAL(mat, i, j) * out->ve[j];
228 if (diag == 0.) {
229 if (fabs(M_VAL(mat, i, i)) <= Tiny * fabs(sum))
230 return -1;
231 else
232 out->ve[i] = sum / M_VAL(mat, i, i);
233 }
234 else
235 out->ve[i] = sum / diag;
236 }
237
238 return 0;
239 }
240
241 /*
242 * new_matrix -- generate a nxn matrix.
243 */
244
245 Matrix
new_matrix(int n)246 new_matrix(int n)
247 {
248 Matrix mat;
249
250 mat = New(struct matrix);
251 mat->dim = n;
252 mat->me = NewAtom_N(double, n * n);
253 return mat;
254 }
255
256 /*
257 * new_matrix -- generate a n-dimension vector.
258 */
259
260 Vector
new_vector(int n)261 new_vector(int n)
262 {
263 Vector vec;
264
265 vec = New(struct vector);
266 vec->dim = n;
267 vec->ve = NewAtom_N(double, n);
268 return vec;
269 }
270