三点定位的算法
三点定位，
已知a,b,c三个顶点的x,y坐标
和三个点都z坐标的距离，la，lb,lc
求z点的坐标
原理就是围绕a,b,c 三个点画圆，三个圆焦点的部分就是所求
但是，由于三个点的距离可能不准，不一定会有结果，
所以是三个圆环的焦点，环的宽度开始为0，没有取到则加1
运行
gcc -lm test.c
test.c代码如下
已知a,b,c三个顶点的x,y坐标
和三个点都z坐标的距离，la，lb,lc
求z点的坐标
原理就是围绕a,b,c 三个点画圆，三个圆焦点的部分就是所求
但是，由于三个点的距离可能不准，不一定会有结果，
所以是三个圆环的焦点，环的宽度开始为0，没有取到则加1
运行
gcc -lm test.c
test.c代码如下
#include "stdio.h"
#include "stdlib.h"
#include <math.h>
//坐标
double ax,ay,bx,by,cx,cy,zx,zy;
//距离
double la,lb,lc;
double cx1,cy1,cx2,cy2,cx3,cy3,cx4,cy4;
int main(void)
{
int i = 0,j = 0,n = 0;
int okflag = 0;
int err = 0;
int aberr = 0;
int acerr = 0;
int abcerr = 0;
int len;
int abrequery = 0;
int acrequery = 0;
int requery1 = 0;
int loopnum = 1;
int option = 0;
double cax,cay,cbx,cby,ccx,ccy;
double cax_abeyance1[6000],cay_abeyance1[6000];
double cbx_abeyance1[6000],cby_abeyance1[6000];
double ccx_abeyance1[6000],ccy_abeyance1[6000];
double cabx_abeyance1[10],caby_abeyance1[10],cabx_abeyance2[10],caby_abeyance2[10];
double cacx_abeyance1[10],cacy_abeyance1[10],cacx_abeyance2[10],cacy_abeyance2[10];
//遍历圆周上所有点
printf("use 'Enter' to divide x y\n");
printf("input A coordinate:\n");
scanf("%lf%lf",&ax,&ay);
if((ax > 1000)||(bx > 1000))
{
printf("figure is too big! input A coordinate again:\n");
scanf("%lf%lf",&ax,&ay);
}
printf("input B coordinate:\n");
scanf("%lf%lf",&bx,&by);
if((bx > 1000)||(bx > 1000))
{
printf("figure is too big! input B coordinate again:\n");
scanf("%lf,%lf",&bx,&by);
}
printf("input C coordinate:\n");
scanf("%lf%lf",&cx,&cy);
if((cx > 1000)||(cx > 1000))
{
printf("figure is too big! input C coordinate again:\n");
scanf("%lf%lf",&cx,&cy);
}
printf("input distance from A to Z:\n");
scanf("%lf",&la);
if(la > 1000)
{
printf("figure is too big! inputdistance from A to Z again:\n");
scanf("%lf",&la);
}
printf("input distance from B to Z:\n");
scanf("%lf",&lb);
if(lb > 1000)
{
printf("figure is too big! inputdistance from B to Z again:\n");
scanf("%lf",&lb);
}
printf("input distance from C to Z:\n");
scanf("%lf",&lc);
if(lc > 1000)
{
printf("figure is too big! inputdistance from C to Z again:\n");
scanf("%lf",&lc);
}
printf("\n");
printf("A(%lf,%lf)\n",ax,ay);
printf("B(%lf,%lf)\n",bx,by);
printf("C(%lf,%lf)\n",cx,cy);
printf("AZ = %lf\n",la);
printf("BZ = %lf\n",lb);
printf("CZ = %lf\n",lc);
//INPUT:
for(zx = ax -la;zx <= ax + la;zx++)
{
do
{
for(zy = ay -la;zy <= ay + la;zy++)
{
cax = zx - ax;
cax = fabs(cax);
cax = pow(cax,2);
cay = zy - ay;
cay = fabs(cay);
cay = pow(cay,2);
if(((cax + cay) > (la*la - err))&&((cax + cay) < (la*la + err)))
{
okflag = 1;
cax_abeyance1[i] = zx;
cay_abeyance1[i] = zy;
n++;
}
}
if(!okflag)err++;
} while (okflag == 0);
i++;
err = 0;
okflag = 0;
}
printf("query...\n");
i = 0;
for(zx = bx -lb;zx <= bx + lb;zx++)
{
do
{
for(zy = by -lb;zy <= by + lb;zy++)
{
cbx = zx - bx;
cbx = fabs(cbx);
cbx = pow(cbx,2);
cby = zy - by;
cby = fabs(cby);
cby = pow(cby,2);
if(((cbx + cby) > (lb*lb - err))&&((cbx + cby) < (lb*lb + err)))
{
okflag = 1;
cbx_abeyance1[i] = zx;
cby_abeyance1[i] = zy;
}
}
if(!okflag)err++;
} while (okflag == 0);
i++;
err = 0;
okflag = 0;
}
printf("query...\n");
getchar();
i = 0;
for(zx = cx -lc;zx <= cx + lc;zx++)
{
do
{
for(zy = cy -lc;zy <= cy + lc;zy++)
{
ccx = zx - cx;
ccx = fabs(ccx);
ccx = pow(ccx,2);
ccy = zy - cy;
ccy = fabs(ccy);
ccy = pow(ccy,2);
if(((ccx + ccy) > (lc*lc - err))&&((ccx + ccy) < (lc*lc + err)))
{
okflag = 1;
ccx_abeyance1[i] = zx;
ccy_abeyance1[i] = zy;
n++;
}
}
if(!okflag)err++;
} while (okflag == 0);
i++;
err = 0;
okflag = 0;
}
printf("query a b c over!\n");
printf("press Enter to continue!\n");
//必须加不然运行出错，但不会停止
getchar();
do
{
//找ab 交点，因为距离的偏差，所以坐标会有偏差
aberr =0;
do
{
for(i = 0;i <= 2*la;i++)
{
for(j = 0;j <= 2*lb;j++)
{
if(cax_abeyance1[i] == cbx_abeyance1[j])
{
if((cay_abeyance1[i] > (cby_abeyance1[j] - aberr - abcerr))&&(cay_abeyance1[i] < (cby_abeyance1[j] + aberr + abcerr)))
{
cabx_abeyance1[abrequery] = cax_abeyance1[i];
caby_abeyance1[abrequery] = cay_abeyance1[i];
printf("cabx_abeyance1[%d]=%f",abrequery,cabx_abeyance1[abrequery]);
printf("caby_abeyance1[%d]=%f\n\n",abrequery,caby_abeyance1[abrequery]);
abrequery++;
}
}
}
}
if(abrequery < loopnum)aberr++;
}while(abrequery < loopnum);
acerr =0;
//找ac 交点
do
{
for(i = 0;i <= 2*la;i++)
{
for(j = 0;j <= 2*lc;j++)
{
if(cax_abeyance1[i] == ccx_abeyance1[j])
{
if((cay_abeyance1[i] > (ccy_abeyance1[j] - acerr - abcerr))&&(cay_abeyance1[i] < (ccy_abeyance1[j] + acerr + abcerr)))
{
cacx_abeyance1[acrequery] = cax_abeyance1[i];
cacy_abeyance1[acrequery] = cay_abeyance1[i];
acrequery++;
}
}
}
}
if(acrequery < loopnum)acerr++;
}while(acrequery < loopnum);
//找交点的交点
for(i = 0;i < abrequery;i++)
{
for(j = 0;j < acrequery;j++)
{
if(cabx_abeyance1[i] == cacx_abeyance1[j])
{
requery1 = 1;
printf("final result is (%f,%f)\n",cabx_abeyance1[i],caby_abeyance1[i]);
}
}
}
if(requery1 == 0)
{
loopnum++;
abcerr++;
}
if(loopnum == 10)
{
printf("chech input figure is right\n");
}
}while(requery1 ==0);
printf("finish!\n");
getchar();
return 0;
}