三点定位，
已知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;
}
分享到：
相关推荐
在IT领域，空间三点定位算法是一种常见的几何计算问题，它主要应用于GPS导航、地理信息系统（GIS）、机器人定位、无人机导航等诸多场景。这个算法基于三角几何原理，通过已知三个固定点的坐标（x, y, z）以及目标点...
javascript+html5实现三点定位算法。源码分享。基于rssi的三点定位算法，是已知三个点的坐标和未知点到这三个点的rssi的信号值，求解未知点的坐标。
三点定位算法是基于几何原理，通过至少三个已知参考点来确定未知目标位置的一种技术。在二维空间中，如果知道三个点的坐标，可以通过解三角形的方法找到第四点的位置；在三维空间中，同样利用三角原理，但需要解决的...
QT三点定位算法是一种在计算机图形学、导航系统和地理信息系统中常见的定位技术。它基于几何原理，通过已知三个或更多点与待定位点之间的距离信息来确定未知点的精确位置。这种算法在IT领域中有着广泛的应用，尤其是...
应用于激光雷达数据的定位算法，在传统的三点定位算法基础上加入了权函数，计算精度和实时性都有提升。激光雷达测得的到每一个反射板的距离都是一个数组，因此需要取其均值。
简单的三点定位算法 简单的三点定位算法是利用三个或四个已知点的坐标和与未知点的距离来计算未知点的坐标。该算法有广泛的应用，如在GPS、wireless sensor networks、computer vision等领域。 在该算法中，我们...
这是一个采用iBeacon技术的室内定位服务端程序，里面包含了三种定位算法，三边定位算法、加权三边定位算法和三角形加权质心定位算法。程序采用Spring框架，简化了JDBC和RMI的样板式代码，并配置了Druid数据库连接池...
在实现三点定位算法时，C语言提供了一种直接且高效的方式。程序可能包含以下元素： 1. 数据结构：定义坐标点的数据结构，如`struct Point {int x, y;}`。 2. 函数定义：编写函数来处理向量运算，如计算向量、求解...
matlab中三点法wifi定位，给三个随机的点，Trilateration(三边测量)是一种常用的定位算法: 1.已知三点位置(x1,y1)(x2,y2)(x3,y3) 2.已知未知点(x0,y0)到三点距离d1,d2,d3 以d1,d2,d3为半径作三个圆,根据毕达哥拉斯...
在三点定位问题中，我们通常有三个已知点A、B和C，它们的坐标是(x1, y1), (x2, y2)和(x3, y3)，以及一个待求点P。通过计算质心，我们可以找到P的坐标。公式如下： 1. 计算质心的x坐标： \( G_x = \frac{x1 + x2 + ...
已知三个点的坐标,某未知点与三个已知点的距离,求未知点的坐标。利用三点的其距离，构成圆形的交点，是唯一一点，就是要求的未知点。
Zigbee三点定位算法是基于三角测量原理的一种无线定位方法。在三个已知位置的Zigbee节点（也称为信标）的帮助下，通过测量目标节点与这三个信标之间的信号强度或时间差，可以计算出目标节点的精确位置。这种算法的...
本篇文章将详细探讨"三边测量"、"三点定位"和"质心定位算法"这三种重要的定位方法，并阐述它们的基本原理、应用场景以及各自的特点。 首先，我们来看"三边测量"定位算法。这种算法基于多边形几何学原理，通过测量...
4. **定位算法**：采用适当的定位算法（如最小二乘法）找到满足所有距离估计的最佳位置。 5. **误差分析**：分析定位误差，评估算法的精度和鲁棒性。 6. **结果可视化**：利用`matplotlib`绘制节点位置和定位轨迹，...
本资料主要涉及了四种不同的定位算法：三点定位、多点加权定位、多点梯度下降法以及航迹导航。 1. 三点定位： 三点定位是最基础的定位方法，基于三角测量原理。至少需要三个已知位置的信标来确定一个未知点的位置...
空间3点投影定位算法，是关于导航和定位的，适合单片机
总之，“三点定位MATLAB代码”提供了一个实际操作的空间定位实例，对于学习和掌握定位算法以及MATLAB编程技巧具有重要意义。通过研究这段代码，你可以提升自己的数学建模能力，同时加深对MATLAB软件的运用。