高斯消元求四点

/*
矩形四点变换公式

[a,b]是矩形
[x,y]是四边形

a=(Ax+By+C)/(Gx+Hy+1)
b=(Dx+Ey+F)/(Gx+Hy+1)

| x1 y1  1  0  0  0 -a1*x1 -a1*y1 |   | A |   | a1 | 
| x2 y1  1  0  0  0 -a2*x2 -a2*y1 |   | B |   | a2 | 
| x1 y2  1  0  0  0 -a3*x1 -a3*y2 |   | C |   | a3 | 
| x2 y2  1  0  0  0 -a4*x2 -a4*y2 |   | D |   | a4 | 
|  0  0  0 x1 y1  1 -b1*x1 -b1*y1 | * | E | = | b1 | 
|  0  0  0 x2 y1  1 -b2*x2 -b2*y1 |   | F |   | b2 | 
|  0  0  0 x1 y2  1 -b3*x1 -b3*y2 |   | G |   | b3 | 
|  0  0  0 x2 y2  1 -b4*x2 -b4*y2 |   | H |   | b4 | 
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#define N 8
#define M 9

typedef struct Tag_point
{
	int x;
	int y;
}POINT;

//高斯消元法解线性方程
void Gauss(double a[N][M], double x[N], int n)
{ 
	int i,j,k,t; 
	double l,p; 
	for(k=0;k<n-1;k++) 
	{ 
		t=k; 
		//寻找绝对值最大的
		for(i=k+1;i<n;i++) 
		{
			if(fabs(a[i][k])>fabs(a[t][k])) 
			{
				t=i; 
			}
		}
		if(t!=k)
		{
			//整行交换
			for(j=k;j<=n;j++)
			{
				p=a[k][j]; 
				a[k][j]=a[t][j]; 
				a[t][j]=p; 
			} 
		}
		//消元
		for(i=k+1;i<n;i++) 
		{ 
			l=a[i][k]/a[k][k]; 
			for(j=k;j<=n;j++) 
			{ 
				a[i][j]-=l*a[k][j]; 
			} 
		}

		//打印
		/*for(i=0;i<n;i++) 
		{ 
			for(j=0;j<=n;j++) printf("\t%f",a[i][j]); 
				printf("\n"); 
		}
		printf("\n"); */
	} 

	//代入求解
	x[n-1]=a[n-1][n]/a[n-1][n-1]; 
	for(i=n-2;i>=0;i--) 
	{ 
		for(j=i+1;j<n;j++)
		{
			a[i][n]-=a[i][j]*x[j]; 
		}
		x[i]=a[i][n]/a[i][i]; 
	} 
} 

void FillMatrix(POINT rpa[4], POINT dpa[4], double mt[N][M])
{
	int i, j;

	memset(mt, 0, sizeof(double)*N*M);
	for(i=0; i < 4; i++)
	{
		mt[i][0] = mt[i+4][3] = rpa[i].x;
		mt[i][1] = mt[i+4][4] = rpa[i].y;
		mt[i][2] = mt[i+4][5] = 1;

		mt[i][6] = rpa[i].x * dpa[i].x * -1;
		mt[i][7] = rpa[i].y * dpa[i].x * -1;
		mt[i][8] = dpa[i].x;
		
		mt[i+4][6] = rpa[i].x * dpa[i].y * -1;
		mt[i+4][7] = rpa[i].y * dpa[i].y * -1;
		mt[i+4][8] = dpa[i].y;
	}

	/*for(i=0;i<N;i++) 
	{ 
		for(j=0; j < M; j++) printf("%.0f ", mt[i][j]); 
			printf("\n"); 
	}*/
}

void GetPoint(POINT *prp, POINT *pdp, double x[N])
{
	double xt, yt;

	xt = prp->x;
	yt = prp->y;

	pdp->x = (int)(((x[0]*xt + x[1]*yt + x[2])/(x[6]*xt + x[7]*yt + 1))+0.5); //四舍五入取整
	pdp->y = (int)(((x[3]*xt + x[4]*yt + x[5])/(x[6]*xt + x[7]*yt + 1))+0.5);
}

int main()
{
	double mt[N][M] ={0};
	double x[N] = {0}; //方程的未知参数
	POINT rpa[4] = {0,0,
					0,100,
					100,100,
					100,0};
	POINT dpa[4] = {10,10,
					10,1000,
					1010,1010,
					1010,10};
	POINT rp = {0};
	POINT dp = {0};

	memset(x, 0, sizeof(x));
	FillMatrix(rpa, dpa, mt);
	Gauss(mt,x,N); //求出参数

	while(1)
	{
		scanf("%d%d", &rp.x, &rp.y);
		GetPoint(&rp, &dp, x); //利用参数取点
		printf("x = %d, y = %d\n", dp.x, dp.y);
	}

	/*printf("\n");
	for(i=0; i<N; i++)
	{
		printf("\t%f",x[i]);
	}
	printf("\n");*/

	system("pause");
	return 0;
}