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 #include "gaussjordan.h"
  42
  43 #define SMALL 0.00001
  44
  45 int firm_gaussjordansolve(double *A, double *vec, int nsize)
  46 {
  47         int i, j, row, col, col2, biggest_r = 0, biggest_c = 0, t;
  48         double big, temp, sum;
  49         double *scramvec = XMALLOCN(double, nsize);
  50         int    *x        = XMALLOCN(int,    nsize);
  51         int res = 0;
  52
  53 #define _A(row,col) A[(row)*nsize + (col)]
  54         /* init x[] */
  55         for (i = 0; i < nsize; ++i)
  56                 x[i] = i;
  57
  58         /* triangularize A */
  59         /* ie A has zeros below its diagonal */
  60         for (col = 0; col < nsize - 1; ++col) {
  61                 big = 0;
  62                 /* find the largest left in LRH box */
  63                 for (row = col; row < nsize; ++row) {
  64                         for (col2 = col; col2 < nsize; ++col2) {
  65                                 temp = fabs(_A(row,col2));
  66                                 if (temp > big) {
  67                                         biggest_r = row;
  68                                         biggest_c = col2;
  69                                         big = temp; /* largest element left */
  70                                 }
  71                         }
  72                 }
  73                 if (big < SMALL) {
  74                         res = -1;
  75                         goto end;
  76                 }
  77
  78                 /* swap rows */
  79                 for (i=0;i<nsize;i++) {
  80                         temp = _A(col,i);
  81                         _A(col,i) = _A(biggest_r,i);
  82                         _A(biggest_r,i) = temp;
  83                 }
  84                 /* swap vec elements */
  85                 temp = vec[col];
  86                 vec[col] = vec[biggest_r];
  87                 vec[biggest_r] = temp;
  88
  89                 /* swap columns */
  90                 for (i=0;i<nsize;i++) {
  91                         temp = _A(i,col);
  92                         _A(i,col) = _A(i,biggest_c);
  93                         _A(i,biggest_c) = temp;
  94                 }
  95                 /* swap unknowns */
  96                 t = x[col];
  97                 x[col] = x[biggest_c];
  98                 x[biggest_c] = t;
  99
 100                 /* partially annihilate this col */
 101                 /* zero columns below diag */
 102                 for (row=col+1;row<nsize;row++) {
 103
 104                         /* changes during calc */
 105                         temp = _A(row,col) / _A(col,col);
 106
 107                         /* annihilates A[][] */
 108                         for (i=col;i<nsize;i++)
 109                                 _A(row,i) = _A(row,i) - temp * _A(col,i);
 110
 111                         /* same op on vec */
 112                         vec[row] = vec[row] - temp * vec[col];
 113                 }
 114         }
 115
 116         /* back-solve, store solution in scramvec */
 117         scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);
 118
 119         /* answer needs sorting */
 120         for (i=nsize-2;i>=0;i--) {
 121                 sum = 0;
 122                 for (j=i+1;j<nsize;j++)
 123                         sum = sum + _A(i,j) * scramvec[j];
 124                 scramvec[i] = (vec[i] - sum) / _A(i,i);
 125         }
 126         /* reorder unknowns--return in vec */
 127         for (i=0;i<nsize;i++) {
 128                 j = x[i];
 129                 vec[j] = scramvec[i];
 130         }
 131
 132 end:
 133         free(x);
 134         free(scramvec);
 135
 136         return res;
 137 }
 138
 139 #undef _A