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