matrix.c (6185B)
1 /* $Id$ */ 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 <gc.h> 38 39 /* 40 * Macros from "fm.h". 41 */ 42 43 #define New(type) ((type*)GC_MALLOC(sizeof(type))) 44 #define NewAtom(type) ((type*)GC_MALLOC_ATOMIC(sizeof(type))) 45 #define New_N(type,n) ((type*)GC_MALLOC((n)*sizeof(type))) 46 #define NewAtom_N(type,n) ((type*)GC_MALLOC_ATOMIC((n)*sizeof(type))) 47 #define Renew_N(type,ptr,n) ((type*)GC_REALLOC((ptr),(n)*sizeof(type))) 48 49 #define SWAPD(a,b) { double tmp = a; a = b; b = tmp; } 50 #define SWAPI(a,b) { int tmp = a; a = b; b = tmp; } 51 52 #ifdef HAVE_FLOAT_H 53 #include <float.h> 54 #endif /* not HAVE_FLOAT_H */ 55 #if defined(DBL_MAX) 56 static double Tiny = 10.0 / DBL_MAX; 57 #elif defined(FLT_MAX) 58 static double Tiny = 10.0 / FLT_MAX; 59 #else /* not defined(FLT_MAX) */ 60 static double Tiny = 1.0e-30; 61 #endif /* not defined(FLT_MAX */ 62 63 /* 64 * LUfactor -- gaussian elimination with scaled partial pivoting 65 * -- Note: returns LU matrix which is A. 66 */ 67 68 int 69 LUfactor(Matrix A, int *indexarray) 70 { 71 int dim = A->dim, i, j, k, i_max, k_max; 72 Vector scale; 73 double mx, tmp; 74 75 scale = new_vector(dim); 76 77 for (i = 0; i < dim; i++) 78 indexarray[i] = i; 79 80 for (i = 0; i < dim; i++) { 81 mx = 0.; 82 for (j = 0; j < dim; j++) { 83 tmp = fabs(M_VAL(A, i, j)); 84 if (mx < tmp) 85 mx = tmp; 86 } 87 scale->ve[i] = mx; 88 } 89 90 k_max = dim - 1; 91 for (k = 0; k < k_max; k++) { 92 mx = 0.; 93 i_max = -1; 94 for (i = k; i < dim; i++) { 95 if (fabs(scale->ve[i]) >= Tiny * fabs(M_VAL(A, i, k))) { 96 tmp = fabs(M_VAL(A, i, k)) / scale->ve[i]; 97 if (mx < tmp) { 98 mx = tmp; 99 i_max = i; 100 } 101 } 102 } 103 if (i_max == -1) { 104 M_VAL(A, k, k) = 0.; 105 continue; 106 } 107 108 if (i_max != k) { 109 SWAPI(indexarray[i_max], indexarray[k]); 110 for (j = 0; j < dim; j++) 111 SWAPD(M_VAL(A, i_max, j), M_VAL(A, k, j)); 112 } 113 114 for (i = k + 1; i < dim; i++) { 115 tmp = M_VAL(A, i, k) = M_VAL(A, i, k) / M_VAL(A, k, k); 116 for (j = k + 1; j < dim; j++) 117 M_VAL(A, i, j) -= tmp * M_VAL(A, k, j); 118 } 119 } 120 return 0; 121 } 122 123 /* 124 * LUsolve -- given an LU factorisation in A, solve Ax=b. 125 */ 126 127 int 128 LUsolve(Matrix A, int *indexarray, Vector b, Vector x) 129 { 130 int i, dim = A->dim; 131 132 for (i = 0; i < dim; i++) 133 x->ve[i] = b->ve[indexarray[i]]; 134 135 if (Lsolve(A, x, x, 1.) == -1 || Usolve(A, x, x, 0.) == -1) 136 return -1; 137 return 0; 138 } 139 140 /* m_inverse -- returns inverse of A, provided A is not too rank deficient 141 * -- uses LU factorisation */ 142 #if 0 143 Matrix 144 m_inverse(Matrix A, Matrix out) 145 { 146 int *indexarray = NewAtom_N(int, A->dim); 147 Matrix A1 = new_matrix(A->dim); 148 m_copy(A, A1); 149 LUfactor(A1, indexarray); 150 return LUinverse(A1, indexarray, out); 151 } 152 #endif /* 0 */ 153 154 Matrix 155 LUinverse(Matrix A, int *indexarray, Matrix out) 156 { 157 int i, j, dim = A->dim; 158 Vector tmp, tmp2; 159 160 if (!out) 161 out = new_matrix(dim); 162 tmp = new_vector(dim); 163 tmp2 = new_vector(dim); 164 for (i = 0; i < dim; i++) { 165 for (j = 0; j < dim; j++) 166 tmp->ve[j] = 0.; 167 tmp->ve[i] = 1.; 168 if (LUsolve(A, indexarray, tmp, tmp2) == -1) 169 return NULL; 170 for (j = 0; j < dim; j++) 171 M_VAL(out, j, i) = tmp2->ve[j]; 172 } 173 return out; 174 } 175 176 /* 177 * Usolve -- back substitution with optional over-riding diagonal 178 * -- can be in-situ but doesn't need to be. 179 */ 180 181 int 182 Usolve(Matrix mat, Vector b, Vector out, double diag) 183 { 184 int i, j, i_lim, dim = mat->dim; 185 double sum; 186 187 for (i = dim - 1; i >= 0; i--) { 188 if (b->ve[i] != 0.) 189 break; 190 else 191 out->ve[i] = 0.; 192 } 193 i_lim = i; 194 195 for (; i >= 0; i--) { 196 sum = b->ve[i]; 197 for (j = i + 1; j <= i_lim; j++) 198 sum -= M_VAL(mat, i, j) * out->ve[j]; 199 if (diag == 0.) { 200 if (fabs(M_VAL(mat, i, i)) <= Tiny * fabs(sum)) 201 return -1; 202 else 203 out->ve[i] = sum / M_VAL(mat, i, i); 204 } 205 else 206 out->ve[i] = sum / diag; 207 } 208 209 return 0; 210 } 211 212 /* 213 * Lsolve -- forward elimination with (optional) default diagonal value. 214 */ 215 216 int 217 Lsolve(Matrix mat, Vector b, Vector out, double diag) 218 { 219 int i, j, i_lim, dim = mat->dim; 220 double sum; 221 222 for (i = 0; i < dim; i++) { 223 if (b->ve[i] != 0.) 224 break; 225 else 226 out->ve[i] = 0.; 227 } 228 i_lim = i; 229 230 for (; i < dim; i++) { 231 sum = b->ve[i]; 232 for (j = i_lim; j < i; j++) 233 sum -= M_VAL(mat, i, j) * out->ve[j]; 234 if (diag == 0.) { 235 if (fabs(M_VAL(mat, i, i)) <= Tiny * fabs(sum)) 236 return -1; 237 else 238 out->ve[i] = sum / M_VAL(mat, i, i); 239 } 240 else 241 out->ve[i] = sum / diag; 242 } 243 244 return 0; 245 } 246 247 /* 248 * new_matrix -- generate a nxn matrix. 249 */ 250 251 Matrix 252 new_matrix(int n) 253 { 254 Matrix mat; 255 256 mat = New(struct matrix); 257 mat->dim = n; 258 mat->me = NewAtom_N(double, n * n); 259 return mat; 260 } 261 262 /* 263 * new_matrix -- generate a n-dimension vector. 264 */ 265 266 Vector 267 new_vector(int n) 268 { 269 Vector vec; 270 271 vec = New(struct vector); 272 vec->dim = n; 273 vec->ve = NewAtom_N(double, n); 274 return vec; 275 }