三点定位,
已知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;
}
分享到:
相关推荐
javascript+html5实现三点定位算法。源码分享。基于rssi的三点定位算法,是已知三个点的坐标和未知点到这三个点的rssi的信号值,求解未知点的坐标。
根据当前点距离空间三个确定点(x,y,z坐标已知)的距离,计算出当前点空间坐标{x,y,z}
定位算法 三点定位MATLAB代码。 三点定位的MATLAB定位,定位算法,直接就能用。
应用于激光雷达数据的定位算法,在传统的三点定位算法基础上加入了权函数,计算精度和实时性都有提升。激光雷达测得的到每一个反射板的距离都是一个数组,因此需要取其均值。
这是一个采用iBeacon技术的室内定位服务端程序,里面包含了三种定位算法,三边定位算法、加权三边定位算法和三角形加权质心定位算法。程序采用Spring框架,简化了JDBC和RMI的样板式代码,并配置了Druid数据库连接池...
简单的三点定位算法.doc
通过被定位到三个及以上的点的距离,计算被定位点所在坐标。
为进一步提高无线传感器网络节点的定位精度,提出一种基于改进人群搜索优化的无线传感器网络三点定位算法.首先设计一种三点估计未知节点坐标模型;然后利用该模型求取未知节点的坐标,并将计算出的节点位置作为人群搜索...
matlab中三点法wifi定位,给三个随机的点,Trilateration(三边测量)是一种常用的定位算法: 1.已知三点位置(x1,y1)(x2,y2)(x3,y3) 2.已知未知点(x0,y0)到三点距离d1,d2,d3 以d1,d2,d3为半径作三个圆,根据毕达哥拉斯...
已知三个点的坐标,某未知点与三个已知点的距离,求未知点的坐标。利用三点的其距离,构成圆形的交点,是唯一一点,就是要求的未知点。
三边测量、三点定位、质心定位算法
空间3点投影定位算法,是关于导航和定位的,适合单片机
梯度下降法 低功耗蓝牙信标定位算法 实现三点定位、多点加权定位、多点梯度下降法定位、航迹导航等。.zip
三角质心算法: ...用于三点定位计算坐标,已测试相对三角算法来说提高了精准对, 可以增加锚节点数量,分别代入算法,获得多组坐标,再平均计算,则可进一步提高精确度。程序已经应用现有项目,无bug
Zigbee三点定位算法上位机,VS打开,主要由C++串口助手和MFC界面程序两部分组成
三点定位的MATLAB定位,定位算法,直接就能用。
为此,在结合RFID射频技术、无线载波技术与超低频电磁波技术的基础上,以TODO三点定位算法为基础建立数学模型,加入RSSI定位算法,利用高斯滤波有效地滤掉干扰信号,通过加入CL质心算法以缩小定位区域,使人员定位更为准确...
基于TOF的三点定位坐标解算算法,用于三维空间中的三角测量计算,根据球体(代表信号发射源或接收器的范围)的交点来确定位置。