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 #ifdef HAVE_CONFIG_H
  37 #include "config.h"
  38 #endif
  39
  40 #include <math.h>
  41 #include <stdlib.h>
  42 #include "xmalloc.h"
  43
  44 #define SMALL 0.00001
  45
  46 int firm_gaussjordansolve(double *A, double *vec, int nsize)
  47 {
  48   int i, j, row, col, col2, biggest_r = 0, biggest_c = 0, t;
  49   double big, temp, sum;
  50   double *scramvec = xmalloc(nsize * sizeof(*scramvec));
  51   int *x = xmalloc(nsize * sizeof(*x));
  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 it's 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       return (-1);
  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   /* back-solve, store solution in scramvec */
 114   scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);
 115
 116   /* answer needs sorting */
 117   for(i=nsize-2;i>=0;i--) {
 118     sum = 0;
 119     for(j=i+1;j<nsize;j++)
 120       sum = sum + _A(i,j) * scramvec[j];
 121     scramvec[i] = (vec[i] - sum) / _A(i,i);
 122   }
 123   /* reorder unknowns--return in vec */
 124   for(i=0;i<nsize;i++) {
 125     j = x[i];
 126     vec[j] = scramvec[i];
 127   }
 128
 129   free(x);
 130   free(scramvec);
 131
 132   return (0);
 133 }
 134
 135 #undef _A