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