高斯消元法的其中一种伪代码:
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
这个算法和之前谈及的有点儿不同,它由绝对值最大的部分开始做起,这样可以改善算法上的稳定性。将经过调换后的第一列作为起点,这算法由左至右地计算。每作出以下两个步骤,才跳到下一列:
- 定出每列的最后一个非0的数,将每行的数字除以该数,使到每行的第一个数成为1;
- 将每行的数字减去第一行的第一个数的某个倍数。
所有步骤完成后,这个矩阵会变成一个行梯阵式,再用代入法就可解决这个方程组。
#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;
}