matrix.c

   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 <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 }