2 * taken from: http://coastal.er.usgs.gov/rvm/toolkit/rvmlibv2.c
6 /*------------------------------------------------------*/
11 /* Now does full pivoting--row and column swapping. */
12 /* Requires keeping track of which variable corresponds */
13 /* to each vector position. */
18 /* from rob holman's pascal program--geo.pas */
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. */
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 */
32 /* returns 0 if successful */
33 /* returns -1 if ill-conditioned matrix */
34 /*------------------------------------------------------*/
45 int firm_gaussjordansolve(double *A, double *vec, int nsize)
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));
53 #define _A(row,col) A[(row)*nsize + (col)]
55 for (i = 0; i < nsize; ++i)
59 /* ie A has zeros below it's diagonal */
60 for (col = 0; col < nsize - 1; ++col) {
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));
69 big = temp; /* largest element left */
79 for(i=0;i<nsize;i++) {
81 _A(col,i) = _A(biggest_r,i);
82 _A(biggest_r,i) = temp;
84 /* swap vec elements */
86 vec[col] = vec[biggest_r];
87 vec[biggest_r] = temp;
90 for(i=0;i<nsize;i++) {
92 _A(i,col) = _A(i,biggest_c);
93 _A(i,biggest_c) = temp;
97 x[col] = x[biggest_c];
100 /* partially annihilate this col */
101 /* zero columns below diag */
102 for(row=col+1;row<nsize;row++) {
104 /* changes during calc */
105 temp = _A(row,col) / _A(col,col);
107 /* annihilates A[][] */
108 for(i=col;i<nsize;i++)
109 _A(row,i) = _A(row,i) - temp * _A(col,i);
112 vec[row] = vec[row] - temp * vec[col];
116 /* back-solve, store solution in scramvec */
117 scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);
119 /* answer needs sorting */
120 for(i=nsize-2;i>=0;i--) {
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);
126 /* reorder unknowns--return in vec */
127 for(i=0;i<nsize;i++) {
129 vec[j] = scramvec[i];