Gaussian elimination pivot all 求解线性方程组

高斯消元法的其中一种伪代码:

i := 1
j := 1
while (i ≤ m and j ≤ n) do
  Find pivot in column j, starting in row i:
  maxi := i
  for k := i+1 to m do
    if abs(A[k,j]) > abs(A[maxi,j]) then
      maxi := k
    end if
  end for
  if A[maxi,j] ≠ 0 then
    swap rows i and maxi, but do not change the value of i
    Now A[i,j] will contain the old value of A[maxi,j].
    divide each entry in row i by A[i,j]
    Now A[i,j] will have the value 1.
    for u := i+1 to m do
      subtract A[u,j] * row i from row u
      Now A[u,j] will be 0, since A[u,j] - A[i,j] * A[u,j] = A[u,j] - 1 * A[u,j] = 0.
    end for
    i := i + 1
  end if
  j := j + 1
end while

这个算法和之前谈及的有点儿不同，它由绝对值最大的部分开始做起，这样可以改善算法上的稳定性。将经过调换后的第一列作为起点，这算法由左至右地计算。每作出以下两个步骤，才跳到下一列：

1. 定出每列的最后一个非0的数，将每行的数字除以该数，使到每行的第一个数成为1；
2. 将每行的数字减去第一行的第一个数的某个倍数。

所有步骤完成后，这个矩阵会变成一个行梯阵式，再用代入法就可解决这个方程组。

 

#include <stdio.h>
#include <stdlib.h>
#include <opencv.hpp>
using namespace cv; 

int imatrix_solve_n( float* a,  float* b, float* X, int* js , int n)
{
	int l, k, i, j, is, p, q;
	float  d, t;

	is = 0;
	l = 1;

	// solve main loop
	for (k = 0; k <= n - 1; k++) 
	{ 
		d = 0.0;
		for (i = k; i <= n - 1; i++) 
		{
			for (j = k; j <= n - 1; j++)
			{ 
				t = a[i * n + j] ;
				t > 0 ? t : -t;
				
				if (t > d) 
				{ 
					d = t; 
					js[k] = j; 
					is = i; 
				}
			}
		}

		if (d + 1.0 == 1.0) 
			l = 0;
		else
		{ 
			if (js[k] != k) 
			{
				for (i = 0; i <= n - 1; i++) 
				{ 
					p = i * n + k; q = i * n + js[k];
					t = a[p]; a[p] = a[q]; a[q] = t;
				}
			}
			if (is != k) 
			{ 
				for (j = k; j <= n - 1; j++) 
				{ 
					p = k * n + j; 	q = is * n + j;
					t = a[p]; a[p] = a[q]; 	a[q] = t;
				}
				t = b[k]; b[k] = b[is]; b[is] = t;
			}
		}


		// check if no result
		if (l == 0)
		{
			return -1;
		}

		d = a[k * n + k];

		for (j = k + 1; j <= n - 1; j++) 
		{ 
			a[k * n + j]  /= d;
		}

		b[k] = b[k] / d;

		for (j = k + 1; j <= n - 1; j++) 
		{
			for (i = 0; i <= n - 1; i++) 
			{ 
				if (i != k) 
				{
					a[i * n + j]  -= a[i * n + k] * a[k * n + j];
				}
			}
		}

		for (i = 0; i <= n - 1; i++) 
		{ 
			if (i != k) 
			{
				b[i]  -= a[i * n + k] * b[k];
			}
		}

	}

	for (k = n - 1; k >= 0; k--)
	{
		if (js[k] != k) 
		{
			p = k ; q = js[k] ;
			t = b[p]; b[p] = b[q]; b[q] = t;
		}
	}

	for (i =  n - 1; i >= 0; i--) 
		X[i] = b[i];

	return 0;
}


//==============================================================================
void VectorPrint(int nDim, float* pfVect)
{
	int i;

	printf("------------------------------------------------------------------\n");
	for(i=0; i<nDim; i++)
	{
		printf("%9.2lf ", pfVect[i]);
	}
	printf("\n");
}
//==============================================================================

void MatrixPrint(int nDim, float* pfMatr)
{
	int i,j;

	printf("------------------------------------------------------------------\n");
	for(i=0; i<nDim; i++)
	{
		for(j=0; j<nDim; j++)
		{
			printf("%9.2lf ", pfMatr[nDim*i + j]);
		}
		printf("\n");
	}
}

//==============================================================================
// testing of function
//==============================================================================

#define MATRIX_DIMENSION 8

int main(int nArgs, char** pArgs)
{
	int nDim = MATRIX_DIMENSION;
	float fMatr[MATRIX_DIMENSION*MATRIX_DIMENSION] =
	{
		10.000 , 70.000 ,1.000 ,  0.000 ,   0.000 , 0.000 ,     -0.000 ,    -0.000,
		200.000 ,230.000 ,1.000 ,  0.000 ,   0.000 , 0.000 ,     -0.000 ,    -0.000,
		250.000 ,100.000 ,1.000 ,  0.000 ,   0.000 , 0.000 ,-100000.000 ,-40000.000,
		130.000 , 20.000 ,1.000 ,  0.000 ,   0.000 , 0.000 , -52000.000 , -8000.000,
		0.000 ,  0.000 ,0.000 , 10.000 ,  70.000 , 1.000 ,     -0.000 ,    -0.000,
		0.000 ,  0.000 ,0.000 ,200.000 , 230.000 , 1.000 , -60000.000 ,-69000.000,
		0.000 ,  0.000 ,0.000 ,250.000 , 100.000 , 1.000 , -75000.000 ,-30000.000,
		0.000 ,  0.000 ,0.000 ,130.000 ,  20.000 , 1.000 ,     -0.000 ,    -0.000
	};
	float fVec[MATRIX_DIMENSION] = {0.000 , 0.000, 400.000, 400.000 , 0.000, 300.000 ,300.000 , 0.000};
	int js[MATRIX_DIMENSION];
	float fSolution[MATRIX_DIMENSION];
	int res;

	double t = (double)getTickCount();
	res = imatrix_solve_n( &fMatr[0], &fVec[0], &fSolution[0] ,&js[0],nDim); // !!!
	t = (double)getTickCount() - t;   
	printf("in %gms\n", t*1000/getTickFrequency()); 

	if(res)
	{
		printf("No solution!\n");
		return 1;
	}
	else
	{
		printf("Solution:\n");
		VectorPrint(nDim, fSolution);

	}

	return 0;
}