gaussjordan.c 3.66 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
/**
 * 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.                            */
/*------------------------------------------------------*/
Matthias Braun's avatar
Matthias Braun committed
28
29
#include "gaussjordan.h"

30
31
#include <math.h>
#include <stdlib.h>
Michael Beck's avatar
Michael Beck committed
32
#include "xmalloc.h"
33
34
35

#define SMALL 0.00001

Matthias Braun's avatar
Matthias Braun committed
36
int firm_gaussjordansolve(double *A, double *vec, unsigned nsize)
37
{
Matthias Braun's avatar
Matthias Braun committed
38
39
40
	double   *scramvec  = XMALLOCN(double,   nsize);
	unsigned *x         = XMALLOCN(unsigned, nsize);
	int       res       = 0;
41
42

#define _A(row,col) A[(row)*nsize + (col)]
Michael Beck's avatar
Michael Beck committed
43
	/* init x[] */
Matthias Braun's avatar
Matthias Braun committed
44
	for (unsigned i = 0; i < nsize; ++i) {
Michael Beck's avatar
Michael Beck committed
45
		x[i] = i;
Matthias Braun's avatar
Matthias Braun committed
46
	}
47

Michael Beck's avatar
Michael Beck committed
48
	/* triangularize A */
49
	/* ie A has zeros below its diagonal */
Matthias Braun's avatar
Matthias Braun committed
50
51
52
53
	unsigned biggest_r = 0;
	unsigned biggest_c = 0;
	for (unsigned col = 0; col < nsize - 1; ++col) {
		double big = 0;
Michael Beck's avatar
Michael Beck committed
54
		/* find the largest left in LRH box */
Matthias Braun's avatar
Matthias Braun committed
55
56
57
		for (unsigned row = col; row < nsize; ++row) {
			for (unsigned col2 = col; col2 < nsize; ++col2) {
				double temp = fabs(_A(row,col2));
Michael Beck's avatar
Michael Beck committed
58
59
60
				if (temp > big) {
					biggest_r = row;
					biggest_c = col2;
Matthias Braun's avatar
Matthias Braun committed
61
					big       = temp; /* largest element left */
Michael Beck's avatar
Michael Beck committed
62
63
64
65
66
				}
			}
		}
		if (big < SMALL) {
			res = -1;
67
68
			goto end;
		}
69

70
		/* swap rows */
Matthias Braun's avatar
Matthias Braun committed
71
72
		for (unsigned i = 0; i < nsize; ++i) {
			double temp = _A(col,i);
73
74
75
76
			_A(col,i) = _A(biggest_r,i);
			_A(biggest_r,i) = temp;
		}
		/* swap vec elements */
Matthias Braun's avatar
Matthias Braun committed
77
		double temp = vec[col];
78
79
		vec[col] = vec[biggest_r];
		vec[biggest_r] = temp;
80

81
		/* swap columns */
Matthias Braun's avatar
Matthias Braun committed
82
83
		for (unsigned i = 0; i < nsize; ++i) {
			double temp = _A(i,col);
84
85
86
87
			_A(i,col) = _A(i,biggest_c);
			_A(i,biggest_c) = temp;
		}
		/* swap unknowns */
Matthias Braun's avatar
Matthias Braun committed
88
		unsigned t = x[col];
89
90
		x[col] = x[biggest_c];
		x[biggest_c] = t;
91

92
93
		/* partially annihilate this col */
		/* zero columns below diag */
Matthias Braun's avatar
Matthias Braun committed
94
		for (unsigned row = col+1; row < nsize; ++row) {
95
			/* changes during calc */
Matthias Braun's avatar
Matthias Braun committed
96
			double temp = _A(row,col) / _A(col,col);
97
98

			/* annihilates A[][] */
Matthias Braun's avatar
Matthias Braun committed
99
			for (unsigned i = col; i < nsize; ++i)
100
101
102
103
				_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
104
105
		}
	}
106
107
108
109
110

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

	/* answer needs sorting */
Matthias Braun's avatar
Matthias Braun committed
111
112
113
114
	for (unsigned i = nsize - 1; i-- > 0;) {
		int sum = 0;
		for (unsigned j = i+1; j < nsize; ++j)
			sum += _A(i,j) * scramvec[j];
115
116
117
		scramvec[i] = (vec[i] - sum) / _A(i,i);
	}
	/* reorder unknowns--return in vec */
Matthias Braun's avatar
Matthias Braun committed
118
119
	for (unsigned i = 0; i < nsize; ++i) {
		unsigned j = x[i];
120
121
122
123
		vec[j] = scramvec[i];
	}

end:
Michael Beck's avatar
Michael Beck committed
124
125
	free(x);
	free(scramvec);
126

Michael Beck's avatar
Michael Beck committed
127
	return res;
128
129
130
}

#undef _A