-
Notifications
You must be signed in to change notification settings - Fork 0
/
lib_gauss.cpp
114 lines (92 loc) · 2.79 KB
/
lib_gauss.cpp
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
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
/*
* lib_gauss.cpp
*
* Created on: May 7, 2022
* Author: d-w-h
*/
#include <math.h>
#include <stdio.h>
#include <time.h>
#include "lib_mat.hpp"
#include "lib_mem.hpp"
#include "user_types.hpp"
void init_mat_inv(double ** mat_inv, int n) {
for(int row = 0; row < n; ++row) {
for(int c = 0; c < n; ++c) {
if(c == row) {
mat_inv[row][c] = 1.0;
}
else {
mat_inv[row][c] = 0.0;
}
}
}
}
void swap(double ** mat, int r1, int r2, int n) {
double * row = new double[n];
for(int j = 0; j < n; j++) {
row[j] = mat[r1][j];
mat[r1][j] = mat[r2][j];
}
for(int j = 0; j < n; j++)
mat[r2][j] = row[j];
}
int find(double ** mat, int c, int n) {
for(int row = c + 1; row < n; row++) {
if(fabs(mat[row][c]) > SMALL_NUM)
return row;
}
return -1;
}
void gauss_jordan(double ** mat, int n, double ** mat_inv) {
double * order_arr = new double[n];
// Initialize matrix inverse
init_mat_inv(mat_inv, n);
// Convert to row echelon form
for(int c = 0; c < n; ++c) {
// Swap if under threshold
if(fabs(mat[c][c]) <= SMALL_NUM) {
int row = find(mat, c, n);
if(row == -1) {
print_mat_singular();
return;
}
swap(mat, row, c, n);
swap(mat_inv, row, c, n);
}
// Normalize matrix row
for(int col = c + 1; col < n; ++col) {
mat[c][col] = fabs(mat[c][c]) <= SMALL_NUM ? 0.0 : mat[c][col] / mat[c][c];
}
// Update row matrix inverse
for(int col = 0; col < n; ++col) {
mat_inv[c][col] = fabs(mat[c][c]) <= SMALL_NUM ? 0.0 : mat_inv[c][col] / mat[c][c];
}
mat[c][c] = 1.0;
// Delete elements in rows below
for(int row = c + 1; row < n; ++row) {
if(mat[row][c] != 0) {
for(int col = c + 1; col < n; ++col) {
mat[row][col] = -1.0 * mat[row][c] * mat[c][col] + mat[row][col];
}
for(int col = 0; col < n; ++col) {
mat_inv[row][col] = -1.0 * mat[row][c] * mat_inv[c][col] + mat_inv[row][col];
}
mat[row][c] = 0;
}
}
}
// Backtrack to convert to reduced row echelon form
for(int c = n - 1; c > 0; --c) {
for(int row = c - 1; row > -1; --row) {
if(mat[row][c] != 0) {
for(int col = 0; col < n; ++col) {
mat_inv[row][col] = -1.0 * mat[row][c] * mat_inv[c][col] + mat_inv[row][col];
}
mat[row][c] = 0;
}
}
}
// Free allocated space
delete [] order_arr;
}