gaussjordan.c 3.88 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
/**
 * taken from: http://coastal.er.usgs.gov/rvm/toolkit/rvmlibv2.c
 * unknown license
 */

/*------------------------------------------------------*/
/* gauss.c                                              */
/*                                                      */
/* 20 jan 89                                            */
/*                                                      */
/* Now does full pivoting--row and column swapping.     */
/* Requires keeping track of which variable corresponds */
/* to each vector position.                             */
/*                                                      */
/* 18 jan 89                                            */
/* paul o'neill                                         */
/*                                                      */
/* from rob holman's pascal program--geo.pas            */
/*                                                      */
/* Gauss-Jordan procedure to solve even-determined      */
/* square matrix equation Ax = vec,where A is N x N     */
/* and vec is N x 1.  This is taken roughly from        */
/* Menke's book (Geophysical Data Analysis: Discrete    */
/* Inverse Theory--1984 p210 ), but performs actual     */
/* row switching to simplify the programming.           */
/* Partial pivoting is used.                            */
/*                                                      */
/* A[][] is a square matrix, N x N                      */
/* vec[] is N x 1 of the matrix                         */
/* nsize is the size of the equation system             */
/*                                                      */
/* returns 0 if successful                              */
/* returns -1 if ill-conditioned matrix                 */
/*------------------------------------------------------*/
Michael Beck's avatar
Michael Beck committed
35
36
#include "config.h"

37
38
#include <math.h>
#include <stdlib.h>
Michael Beck's avatar
Michael Beck committed
39
#include "xmalloc.h"
40

41
42
#include "gaussjordan.h"

43
44
45
46
#define SMALL 0.00001

int firm_gaussjordansolve(double *A, double *vec, int nsize)
{
Michael Beck's avatar
Michael Beck committed
47
48
	int i, j, row, col, col2, biggest_r = 0, biggest_c = 0, t;
	double big, temp, sum;
49
50
	double *scramvec = XMALLOCN(double, nsize);
	int    *x        = XMALLOCN(int,    nsize);
Michael Beck's avatar
Michael Beck committed
51
	int res = 0;
52
53

#define _A(row,col) A[(row)*nsize + (col)]
Michael Beck's avatar
Michael Beck committed
54
55
56
	/* init x[] */
	for (i = 0; i < nsize; ++i)
		x[i] = i;
57

Michael Beck's avatar
Michael Beck committed
58
	/* triangularize A */
59
	/* ie A has zeros below its diagonal */
Michael Beck's avatar
Michael Beck committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
	for (col = 0; col < nsize - 1; ++col) {
		big = 0;
		/* find the largest left in LRH box */
		for (row = col; row < nsize; ++row) {
			for (col2 = col; col2 < nsize; ++col2) {
				temp = fabs(_A(row,col2));
				if (temp > big) {
					biggest_r = row;
					biggest_c = col2;
					big = temp; /* largest element left */
				}
			}
		}
		if (big < SMALL) {
			res = -1;
75
76
			goto end;
		}
77

78
		/* swap rows */
79
		for (i=0;i<nsize;i++) {
80
81
82
83
84
85
86
87
			temp = _A(col,i);
			_A(col,i) = _A(biggest_r,i);
			_A(biggest_r,i) = temp;
		}
		/* swap vec elements */
		temp = vec[col];
		vec[col] = vec[biggest_r];
		vec[biggest_r] = temp;
88

89
		/* swap columns */
90
		for (i=0;i<nsize;i++) {
91
92
93
94
95
96
97
98
			temp = _A(i,col);
			_A(i,col) = _A(i,biggest_c);
			_A(i,biggest_c) = temp;
		}
		/* swap unknowns */
		t = x[col];
		x[col] = x[biggest_c];
		x[biggest_c] = t;
99

100
101
		/* partially annihilate this col */
		/* zero columns below diag */
102
		for (row=col+1;row<nsize;row++) {
103

104
105
106
107
			/* changes during calc */
			temp = _A(row,col) / _A(col,col);

			/* annihilates A[][] */
108
			for (i=col;i<nsize;i++)
109
110
111
112
				_A(row,i) = _A(row,i) - temp * _A(col,i);

			/* same op on vec */
			vec[row] = vec[row] - temp * vec[col];
Michael Beck's avatar
Michael Beck committed
113
114
		}
	}
115
116
117
118
119

	/* back-solve, store solution in scramvec */
	scramvec[nsize - 1] = vec[nsize - 1] / _A(nsize - 1,nsize - 1);

	/* answer needs sorting */
120
	for (i=nsize-2;i>=0;i--) {
121
		sum = 0;
122
		for (j=i+1;j<nsize;j++)
123
124
125
126
			sum = sum + _A(i,j) * scramvec[j];
		scramvec[i] = (vec[i] - sum) / _A(i,i);
	}
	/* reorder unknowns--return in vec */
127
	for (i=0;i<nsize;i++) {
128
129
130
131
132
		j = x[i];
		vec[j] = scramvec[i];
	}

end:
Michael Beck's avatar
Michael Beck committed
133
134
	free(x);
	free(scramvec);
135

Michael Beck's avatar
Michael Beck committed
136
	return res;
137
138
139
}

#undef _A