高斯约旦消元法求n阶矩阵的逆

高斯约旦消元法求n阶矩阵的逆

本代码以四阶矩阵为例，需要求更高次的，只需要将代码中的4修改下即可
// gaussJordan.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;
using namespace cv;


void step0(double m, double newMat[4][4]);
void step1(double m, double swap[4], double l[4][4], double mat11[4][4]);
void step2(double m, double mat11[4][4], double l1[4][4]);
void step3(double m, double mat11[4][4], double l2[4]);
void gaussJordan(int m, double mat11[4][4]);

void step0(double m, double newMat[4][4]) {
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			if (i == j) {
				newMat[i][j] = 1;
			}
			else {
				newMat[i][j] = 0;
			}
		}
	}
}

void step1(double m, double swap[4], double l[4][4], double mat11[4][4]) {
	for (int i = 0; i < 4; i++) {
		swap[i] = i;
		for (int j = 0; j < 4; j++) {
			l[i][j] = 0;
		}
	}
	for (int i = 0; i < 4; i++) {
		double max_row = mat11[i][i];
		int row = i;
		for (int j = i; j < 4; j++) {
			if (mat11[j][i] >= max_row) {
				max_row = mat11[j][i];
				row = j;
			}
		}
		swap[i] = row;
		if (row != i) {
			for (int j = 0; j < 4; j++) {
				double swapk = mat11[i][j];
				mat11[i][j] = mat11[row][j];
				mat11[row][j] = swapk;
			}
		}
		for (int j = i+1; j < 4; j++) {
			if (mat11[j][i] != 0) {
				l[j][i] = mat11[j][i] / mat11[i][i];
				for (int k = 0; k < 4; k++) {
					mat11[j][k] = mat11[j][k] - (l[j][i] * mat11[i][k]);
				}
			}
		}
	}
}

void step2(double m, double mat11[4][4], double l1[4][4]) {
	int longM = m - 1;
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			l1[i][j] = 0;
		}
	}
	for (int i = 0; i < 4 - 1; i++) {
		for (int j = 0; j < longM - i; j++) {
			if ((mat11[longM - i - j - 1][longM - i] != 0) && (mat11[longM - i][longM - i] != 0)) {
				l1[longM - i - j - 1][longM - i] = mat11[longM - i - j - 1][longM - i] / mat11[longM - i][longM - i];
				for (int k = 0; k < 4; k++) {
					mat11[longM - i - j - 1][k] = mat11[longM - i - j - 1][k] - l1[longM - i - j - 1][longM - i] * mat11[longM - i][k];
				}
			}
		}
	}
}

void step3(double m, double mat11[4][4], double l2[4]) {
	for (int i = 0; i < 4; i++) {
		l2[i] = mat11[i][i];
	}
}

void gaussJordan(int m, double mat11[4][4]) {
	double newMat[4][4], swap[4], l[4][4], l1[4][4], l2[4];
	step0(4, newMat);
	step1(4, swap, l, mat11);
	step2(4, mat11, l1);
	step3(4, mat11, l2);

	for (int i = 0; i < 4; i++) {
		if (swap[i] != i) {
			for (int j = 0; j < 4; j++) {
				double swapk1 = newMat[i][j];
				int k1 = swap[i];
				newMat[i][j] = newMat[k1][j];
				newMat[k1][j] = swapk1;
			}
		}
		for (int j = i + 1; j < 4; j++) {
			for (int k = 0; k < 4; k++) {
				if (l[j][i] != 0) {
					newMat[j][k] = newMat[j][k] - l[j][i] * newMat[i][k];
				}
			}
		}
	}
	for (int i = 0; i < 4 - 1; i++) {
		for (int j = 0; j < 4 - i - 1; j++) {
			if (l1[4 - 1 - i - j - 1][4 - 1 - i] != 0) {
				for (int k = 0; k < 4; k++) {
					newMat[4 - 1 - i - j - 1][k] = newMat[4 - 1 - i - j - 1][k] - l1[4 - 1 - i - j - 1][4 - i - 1] * newMat[4 - i - 1][k];
				}
			}
		}
	}
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			newMat[i][j] = newMat[i][j] / l2[i];
		}
	}
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			cout << newMat[i][j] << endl;
		}
	}
}

int main()
{
	double mat11[4][4] = { {3.54677704e+02, 1.03740405e+03, 3.67944094e+05, 1.00000000e+00},
	{1.45565262e+02, 6.24826965e+02, 9.09531016e+04, 1.00000000e+00},
	{7.50557922e+02, 7.80734863e+02, 5.85986750e+05, 1.00000000e+00},
	{4.80305298e+02, 4.25448975e+02, 2.04345391e+05, 1.00000000e+00} };
	gaussJordan(4, mat11);
}

有问题可以加入我的群，QQ群号109530447