cdg/LinAlg.cpp at master · davelee2804/cdg · GitHub

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
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include <cmath>
#include <iostream>

#include "LinAlg.h"

using namespace std;

#define I(p,q,N) \
        p*N + q

#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
void MatInv( double* A, double* Ainv, int n ) {
	int *indxc, *indxr, *ipiv;
	int i, j, k, l, irow = 0, icol = 0, ll;
	double big, dum, pivinv, temp;

	indxc = new int[n]; indxr = new int[n]; ipiv  = new int[n];

	for( i = 0; i < n*n; i++ ) { Ainv[i] = A[i]; }
	for( j = 0; j< n; j++ ) { ipiv[j] = 0; }
	for( i = 0; i < n; i++ ) {
		big = 0.0;
		for( j = 0; j < n; j++ ) {
			if( ipiv[j] != 1 ) {
				for( k = 0; k < n; k++ ) {
					if( ipiv[k] == 0 ) {
						if( fabs(Ainv[I(j,k,n)]) >= big ) {
							big = fabs(Ainv[I(j,k,n)]);
							irow = j;
							icol = k;
						}
					}
					else if( ipiv[k] > 1 ) {
						cerr << "Matrix inverse error! - singular matrix (1)\n";
					}
				}
			}
		}
		++(ipiv[icol]);
		if( irow != icol ) {
			for( l = 0; l < n; l++ ) {
        			SWAP( Ainv[I(irow,l,n)], Ainv[I(icol,l,n)] );
			}
		}
		indxr[i] = irow;
		indxc[i] = icol;
		if( fabs(Ainv[I(icol,icol,n)]) < 1.0e-12 ) {
			cerr << "Matrix inverse error! - singular matrix (2)\n";
		}
		pivinv = 1.0/Ainv[I(icol,icol,n)];
		Ainv[I(icol,icol,n)] = 1.0;
		for( l = 0; l < n; l++ ) { Ainv[I(icol,l,n)] *= pivinv; }
		for( ll = 0; ll < n; ll++ ) {
			if( ll != icol ) {
				dum = Ainv[I(ll,icol,n)];
				Ainv[I(ll,icol,n)] = 0.0;
				for( l = 0; l < n; l++ ) { Ainv[I(ll,l,n)] -= Ainv[I(icol,l,n)]*dum; }
			}
		}
	}
	for( l = n-1; l >= 0; l-- ) {
		if( indxr[l] != indxc[l] ) {
			for( k = 0; k < n; k++ ) {
				SWAP( Ainv[I(k,indxr[l],n)], Ainv[I(k,indxc[l],n)] );
			}
		}
	}
	delete[] indxc; delete[] indxr; delete[] ipiv;
}

/* b_i = A_{ij}x_j */
void AXEB( double* A, double* x, double* b, int n ) {
	int i, j;

	for( i = 0; i < n; i++ ) {
		b[i] = 0.0;
		for( j = 0; j < n; j++ ) {
			b[i] += A[I(i,j,n)]*x[j];
		}
	}
}

/* C_{nm} = A_{nl}*B_{lm} */
void Mult( double* A, double* B, double* C, int n ) {
	int i, j, k;

	for( i = 0; i < n*n; i++ ) {
		C[i] = 0.0;
	}

	for( i = 0; i < n; i++ ) {
		for( j = 0; j < n; j++ ) {
			for( k = 0; k < n; k++ ) {
				C[I(i,j,n)] += A[I(i,k,n)]*B[I(k,j,n)];
			}
		}
	}
}