nsz Git - libfirm/blob - ir/adt/gaussjordan.c

   1 /**
   2  * taken from: http://coastal.er.usgs.gov/rvm/toolkit/rvmlibv2.c
   3  * unknown license
   4  */
   5
   6 /*------------------------------------------------------*/
   7 /* gauss.c                                              */
   8 /*                                                      */
   9 /* 20 jan 89                                            */
  10 /*                                                      */
  11 /* Now does full pivoting--row and column swapping.     */
  12 /* Requires keeping track of which variable corresponds */
  13 /* to each vector position.                             */
  14 /*                                                      */
  15 /* 18 jan 89                                            */
  16 /* paul o'neill                                         */
  17 /*                                                      */
  18 /* from rob holman's pascal program--geo.pas            */
  19 /*                                                      */
  20 /* Gauss-Jordan procedure to solve even-determined      */
  21 /* square matrix equation Ax = vec,where A is N x N     */
  22 /* and vec is N x 1.  This is taken roughly from        */
  23 /* Menke's book (Geophysical Data Analysis: Discrete    */
  24 /* Inverse Theory--1984 p210 ), but performs actual     */
  25 /* row switching to simplify the programming.           */
  26 /* Partial pivoting is used.                            */
  27 /*                                                      */
  28 /* A[][] is a square matrix, N x N                      */
  29 /* vec[] is N x 1 of the matrix                         */
  30 /* nsize is the size of the equation system             */
  31 /*                                                      */
  32 /* returns 0 if successful                              */
  33 /* returns -1 if ill-conditioned matrix                 */
  34 /*------------------------------------------------------*/
  35 #include "config.h"
  36
  37 #include <math.h>
  38 #include <stdlib.h>
  39 #include "xmalloc.h"
  40
  41 #define SMALL 0.00001
  42
  43 int firm_gaussjordansolve(double *A, double *vec, int nsize)
  44 {
  45         int i, j, row, col, col2, biggest_r = 0, biggest_c = 0, t;
  46         double big, temp, sum;
  47         double *scramvec = XMALLOCN(double, nsize);
  48         int    *x        = XMALLOCN(int,    nsize);
  49         int res = 0;
  50
  51 #define _A(row,col) A[(row)*nsize + (col)]
  52         /* init x[] */
  53         for (i = 0; i < nsize; ++i)
  54                 x[i] = i;
  55
  56         /* triangularize A */
  57         /* ie A has zeros below it's diagonal */
  58         for (col = 0; col < nsize - 1; ++col) {
  59                 big = 0;
  60                 /* find the largest left in LRH box */
  61                 for (row = col; row < nsize; ++row) {
  62                         for (col2 = col; col2 < nsize; ++col2) {
  63                                 temp = fabs(_A(row,col2));
  64                                 if (temp > big) {
  65                                         biggest_r = row;
  66                                         biggest_c = col2;
  67                                         big = temp; /* largest element left */
  68                                 }
  69                         }
  70                 }
  71                 if (big < SMALL) {
  72                         res = -1;
  73                         goto end;
  74                 }
  75
  76                 /* swap rows */
  77                 for(i=0;i<nsize;i++) {
  78                         temp = _A(col,i);
  79                         _A(col,i) = _A(biggest_r,i);
  80                         _A(biggest_r,i) = temp;
  81                 }
  82                 /* swap vec elements */
  83                 temp = vec[col];
  84                 vec[col] = vec[biggest_r];
  85                 vec[biggest_r] = temp;
  86
  87                 /* swap columns */
  88                 for(i=0;i<nsize;i++) {
  89                         temp = _A(i,col);
  90                         _A(i,col) = _A(i,biggest_c);
  91                         _A(i,biggest_c) = temp;
  92                 }
  93                 /* swap unknowns */
  94                 t = x[col];
  95                 x[col] = x[biggest_c];
  96                 x[biggest_c] = t;
  97
  98                 /* partially annihilate this col */
  99                 /* zero columns below diag */
 100                 for(row=col+1;row<nsize;row++) {
 101
 102                         /* changes during calc */
 103                         temp = _A(row,col) / _A(col,col);
 104
 105                         /* annihilates A[][] */
 106                         for(i=col;i<nsize;i++)
 107                                 _A(row,i) = _A(row,i) - temp * _A(col,i);
 108
 109                         /* same op on vec */
 110                         vec[row] = vec[row] - temp * vec[col];
 111                 }
 112         }
 113
 114         /* back-solve, store solution in scramvec */
 115         scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);
 116
 117         /* answer needs sorting */
 118         for(i=nsize-2;i>=0;i--) {
 119                 sum = 0;
 120                 for(j=i+1;j<nsize;j++)
 121                         sum = sum + _A(i,j) * scramvec[j];
 122                 scramvec[i] = (vec[i] - sum) / _A(i,i);
 123         }
 124         /* reorder unknowns--return in vec */
 125         for(i=0;i<nsize;i++) {
 126                 j = x[i];
 127                 vec[j] = scramvec[i];
 128         }
 129
 130 end:
 131         free(x);
 132         free(scramvec);
 133
 134         return res;
 135 }
 136
 137 #undef _A