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 /*------------------------------------------------------*/
43 int firm_gaussjordansolve(double *A, double *vec, int nsize)
45 int i, j, row, col, col2, biggest_r = 0, biggest_c = 0, t;
46 double big, temp, sum;
47 double *scramvec = XMALLOCN(double, nsize);
48 int *x = XMALLOCN(int, nsize);
51 #define _A(row,col) A[(row)*nsize + (col)]
53 for (i = 0; i < nsize; ++i)
57 /* ie A has zeros below it's diagonal */
58 for (col = 0; col < nsize - 1; ++col) {
60 /* find the largest left in LRH box */
61 for (row = col; row < nsize; ++row) {
62 for (col2 = col; col2 < nsize; ++col2) {
63 temp = fabs(_A(row,col2));
67 big = temp; /* largest element left */
77 for(i=0;i<nsize;i++) {
79 _A(col,i) = _A(biggest_r,i);
80 _A(biggest_r,i) = temp;
82 /* swap vec elements */
84 vec[col] = vec[biggest_r];
85 vec[biggest_r] = temp;
88 for(i=0;i<nsize;i++) {
90 _A(i,col) = _A(i,biggest_c);
91 _A(i,biggest_c) = temp;
95 x[col] = x[biggest_c];
98 /* partially annihilate this col */
99 /* zero columns below diag */
100 for(row=col+1;row<nsize;row++) {
102 /* changes during calc */
103 temp = _A(row,col) / _A(col,col);
105 /* annihilates A[][] */
106 for(i=col;i<nsize;i++)
107 _A(row,i) = _A(row,i) - temp * _A(col,i);
110 vec[row] = vec[row] - temp * vec[col];
114 /* back-solve, store solution in scramvec */
115 scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);
117 /* answer needs sorting */
118 for(i=nsize-2;i>=0;i--) {
120 for(j=i+1;j<nsize;j++)
121 sum = sum + _A(i,j) * scramvec[j];
122 scramvec[i] = (vec[i] - sum) / _A(i,i);
124 /* reorder unknowns--return in vec */
125 for(i=0;i<nsize;i++) {
127 vec[j] = scramvec[i];