简单的三点定位算法
/* 已知的ABCD四个点的固定X、Y坐标，已知Z点与ABCD点之间的距离，要求通过3
个或4个固定点(两个算法)计算出Z点的X、Y坐标 */
/****************************
B(0,100)______C(100,100) Z(200,100)
| |
| |
A(0,0) |_____|D(100,0) *****************************/
#include "Stdio.h" #include "Conio.h" #include
//坐标
double ax;
double ay;
double bx;
double by;
double cx;
double cy;
double dx;
double dy;
double zx,zy;
//距离
double la;
//double la = 224; double lb;
double lc;
double ld;
//
//double ax = 0; //double ay = 0; //double bx = 0; //double by = 100; //double cx = 100; //double cy = 100; //double dx = 100; //double dy = 0; //double zx,zy;
////距离
//double la = 223; ////double la = 224;
//double lb = 200;
//double lc = 100;
//double ld = 142;
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 requery = 0;
int abrequery = 0;
int acrequery = 0;
int requery1 = 0;
int loopnum = 1;
int option = 0;
double cax,cay,cbx,cby,ccx,ccy,cdx,cdy;
double cax_abeyance1[6000],cay_abeyance1[6000];
double cbx_abeyance1[6000],cby_abeyance1[6000];
double ccx_abeyance1[6000],ccy_abeyance1[6000];
double cdx_abeyance1[6000],cdy_abeyance1[6000];
double cax_abeyance2,cay_abeyance2;
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];
//遍历圆周上所有点
//问题一，N输入比输出多
printf("input 1 or 2 to select:\n");
printf("1.demo\n");
printf("2.test\n");
printf("3.auto\n");
scanf("%d",&option);
if(option == 1)
{
ax = 0;
ay = 0;
bx = 0;
by = 100;
cx = 100;
cy = 100;
dx = 100;
dy = 0;
//距离
la = 223;
//la = 224;
lb = 200;
lc = 100;
ld = 142;
printf("input coordinate of A B C and distance\n");
printf("for example\n");
printf("B(0,100)______C(100,100) Z(?,?)\n");
printf(" | |\n");
printf(" | |\n");
printf(" | |\n");
printf("A(0,0) |_____|D(100,0)\n");
printf("\n");
printf("AZ = 223,BZ = 200,CZ = 100,DZ = 142\n");
printf("\n");
}
else if(option == 2)
{
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);
}
else
{
;//跳过
}
//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);
//printf("cax_abeyance1[%d] = %f,cay_abeyance1[%d]
= %f\n",i,cax_abeyance1[i],i,cay_abeyance1[i]);
//getch();
//if((cax_abeyance1[i] == -200)||(cax_abeyance1[i] == 200) )
//{
//printf("cax_abeyance1 = %f,cay_abeyance1
= %f\n",cax_abeyance1[i],cay_abeyance1[i]);
//getch();
//}
i++;
err = 0;
okflag = 0;
//printf("query a...\n");
}
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++;
//printf("n = %d\n",n);
//printf("err = %d\n",err);
} while (okflag == 0);
//printf("cbx_abeyance1 = %f,cby_abeyance1 = %f\n",cbx_abeyance1[i],cby_abeyance1[i]);
//if((cbx_abeyance1[i] == -200)||(cbx_abeyance1[i] == 200) )
//{
//printf("cbx_abeyance1 = %f,cby_abeyance1
= %f\n",cbx_abeyance1[i],cby_abeyance1[i]);
//getch();
//}
i++;
//getch();
err = 0;
okflag = 0;
//printf("query b...\n");
}
printf("query...\n");
getchar();
i = 0;
for(zx = cx -lc;zx <= cx + lc;zx++)
{
//printf("ok!\n");
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);
//printf("ccx_abeyance1[%d] = %f,ccy_abeyance1[%d]
= %f\n",i,ccx_abeyance1[i],i,ccy_abeyance1[i]);
//getch();
//if((ccx_abeyance1[i] == 0)||(ccx_abeyance1[i] == 200) )
//{
//printf("ccx_abeyance1 = %f,ccy_abeyance1
= %f\n",ccx_abeyance1[i],ccy_abeyance1[i]);
//getch();
//}
i++;
err = 0;
okflag = 0;
//printf("query c...\n");
}
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)))
{
//cay_abeyance2 = cay_abeyance1[i];
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++;
//printf("cax_abeyance1[%d]=%f,cay_abeyance1[%d]=%f\n",i,cax_abeyance1[i],i,cay_abeyance1
[i]);
//printf("cbx_abeyance1[%d]=%f,cby_abeyance1[%d]=%f\n",j,cbx_abeyance1[j],j,cby_abeyance
1[j]);
//getchar();
}
}
}
}
if(abrequery < loopnum)aberr++;
//printf("err = %d\n",err);
//getchar();
}while(abrequery < loopnum);
//printf("aberr = %d\n",aberr);
//printf("query ab over!\n");
//printf("press Enter!\n");
//getchar();
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])
{
//cax_abeyance2 = cax_abeyance1[i];
//printf("cax_abeyance1[%d]=%f,cay_abeyance1[%d]=%f\n",i,cax_abeyance1[i],i,cay_abeyance1
[i]);
//printf("ccx_abeyance1[%d]=%f,ccy_abeyance1[%d]=%f\n",j,ccx_abeyance1[j],j,ccy_abeyance1
[j]);
//getchar();
//if(cay_abeyance1[i] == ccy_abeyance1[j])
if((cay_abeyance1[i] > (ccy_abeyance1[j] - acerr - abcerr))&&(cay_abeyance1[i] < (ccy_abeyance1[j] + acerr + abcerr)))
{
//cay_abeyance2 = cay_abeyance1[i];
cacx_abeyance1[acrequery] = cax_abeyance1[i];
cacy_abeyance1[acrequery] = cay_abeyance1[i];
//printf("cacx_abeyance1[%d]=%f",acrequery,cacx_abeyance1[acrequery]);
//printf("cacy_abeyance1[%d]=%f\n\n",acrequery,cacy_abeyance1[acrequery]);
acrequery++;
//printf("cax_abeyance1[%d]=%f,cay_abeyance1[%d]=%f\n",i,cax_abeyance1[i],i,cay_abeyance1
[i]);
//printf("ccx_abeyance1[%d]=%f,ccy_abeyance1[%d]=%f\n",j,ccx_abeyance1[j],j,ccy_abeyance1
[j]);
//getchar();
}
}
}
}
if(acrequery < loopnum)acerr++;
//printf("err = %d\n",err);
//getchar();
}while(acrequery < loopnum);
//printf("acerr = %d\n",acerr);
//printf("query ac over!\n");
//printf("press Enter!\n");
//getchar();
//printf("abrequery = %d\n",abrequery);
//printf("acrequery = %d\n",acrequery);
//printf("press Enter!\n");
//getchar();
//
//找交点的交点
for(i = 0;i < abrequery;i++)
{
for(j = 0;j < acrequery;j++)
{
//printf("cabx_abeyance1[%d] = %f\n",i,cabx_abeyance1[i]);
//printf("cacx_abeyance1[%d] = %f\n",i,cacx_abeyance1[i]);
if(cabx_abeyance1[i] == cacx_abeyance1[j])
{
requery1 = 1;
//printf("cabx_abeyance1[%d] = %f,caby_abeyance1[%d]
= %f\n",i,cabx_abeyance1[i],i,caby_abeyance1[i]);
//printf("cacx_abeyance1[%d] = %f,cacy_abeyance1[%d]
= %f\n",i,cacx_abeyance1[i],i,cacy_abeyance1[i]);
printf("final result is (%f,%f)\n",cabx_abeyance1[i],caby_abeyance1[i]);
//goto FINISH;
}
}
}
if(requery1 == 0)
{
loopnum++;
abcerr++;
}
if(loopnum == 10)
{
printf("chech input figure is right\n");
//goto INPUT;
}
//printf("abcerr = %d\n",abcerr);
//printf("loopnum = %d\n",loopnum);
//printf("finding...\n");
//getchar();
}while(requery1 ==0);
//FINISH:
printf("finish!\n");
getchar();
return 0;
}
/* pipixiong999@163.com 参数正确保证有解，精度1% */
本文档为【简单的三点定位算法】，请使用软件OFFICE或WPS软件打开。作品中的文字与图均可以修改和编辑，
图片更改请在作品中右键图片并更换，文字修改请直接点击文字进行修改，也可以新增和删除文档中的内容。
该文档来自用户分享，如有侵权行为请发邮件ishare@vip.sina.com联系网站客服，我们会及时删除。
[版权声明] 本站所有资料为用户分享产生，若发现您的权利被侵害，请联系客服邮件isharekefu@iask.cn，我们尽快处理。
本作品所展示的图片、画像、字体、音乐的版权可能需版权方额外授权，请谨慎使用。
网站提供的党政主题相关内容(国旗、国徽、党徽..)目的在于配合国家政策宣传，仅限个人学习分享使用，禁止用于任何广告和商用目的。
浙公网安备 33021202002483