ガウスの消元は4点を求めます
3010 ワード
/*
[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("
");
}
printf("
"); */
}
//
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("
");
}*/
}
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
", dp.x, dp.y);
}
/*printf("
");
for(i=0; i<N; i++)
{
printf("\t%f",x[i]);
}
printf("
");*/
system("pause");
return 0;
}