三点定位的算法

标签： 算法 | 发表时间：2014-10-28 20:22 | 作者：haoningabc
出处：http://www.iteye.com
三点定位，
已知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; 
}


已有 0 人发表留言，猛击->> 这里<<-参与讨论


ITeye推荐



相关 [算法] 推荐：

缓存算法

- lostsnow - 小彰
没有人能说清哪种缓存算法由于其他的缓存算法. （以下的几种缓存算法，有的我也理解不好，如果感兴趣，你可以Google一下  ）. 大家好，我是 LFU，我会计算为每个缓存对象计算他们被使用的频率. 我是LRU缓存算法，我把最近最少使用的缓存对象给踢走. 我总是需要去了解在什么时候，用了哪个缓存对象.

BFPRT算法

- zii - 小彰
BFPRT算法的作者是5位真正的大牛（Blum 、 Floyd 、 Pratt 、 Rivest 、 Tarjan），该算法入选了在StackExchange上进行的当今世界十大经典算法，而算法的简单和巧妙颇有我们需要借鉴学习之处. BFPRT解决的问题十分经典，即从某n个元素的序列中选出第k大（第k小）的元素，通过巧妙的分析，BFPRT可以保证在最坏情况下仍为线性时间复杂度.

贪心算法

- Shan - 博客园-首页原创精华区
顾名思义，贪心算法总是作出在当前看来最好的选择. 也就是说贪心算法并不从整体最优考虑，它所作出的选择只是在某种意义上的局部最优选择. 当然，希望贪心算法得到的最终结果也是整体最优的. 虽然贪心算法不能对所有问题都得到整体最优解，但对许多问题它能产生整体最优解. 如单源最短路经问题，最小生成树问题等.

缓存算法

- 成 - FeedzShare
来自: 小彰 - FeedzShare  . 发布时间:2011年09月25日,  已有 2 人推荐. 没有人能说清哪种缓存算法由于其他的缓存算法. （以下的几种缓存算法，有的我也理解不好，如果感兴趣，你可以Google一下  ）. 大家好，我是 LFU，我会计算为每个缓存对象计算他们被使用的频率.

K-Means 算法

- - 酷壳 - CoolShell.cn
最近在学习一些数据挖掘的算法，看到了这个算法，也许这个算法对你来说很简单，但对我来说，我是一个初学者，我在网上翻看了很多资料，发现中文社区没有把这个问题讲得很全面很清楚的文章，所以，把我的学习笔记记录下来，分享给大家. k-Means 算法是一种  cluster analysis 的算法，其主要是来计算数据聚集的算法，主要通过不断地取离种子点最近均值的算法.

查找算法：

- - CSDN博客推荐文章
从数组的第一个元素开始查找，并将其与查找值比较，如果相等则停止，否则继续下一个元素查找，直到找到匹配值. 注意：要求被查找的数组中的元素是无序的、随机的. 比如，对一个整型数组的线性查找代码：. // 遍历整个数组，并分别将每个遍历元素与查找值对比. 要查找的值在数组的第一个位置. 也就是说只需比较一次就可达到目的，因此最佳情况的大O表达式为：O(1).

排序算法

- - 互联网 - ITeye博客
排序算法有很多，所以在特定情景中使用哪一种算法很重要. 为了选择合适的算法，可以按照建议的顺序考虑以下标准： .     对于数据量较小的情形，（1）（2）差别不大，主要考虑（3）；而对于数据量大的，（1）为首要.  一、冒泡（Bubble）排序——相邻交换 .  二、选择排序——每次最小/大排在相应的位置 .

联接算法

- - CSDN博客数据库推荐文章
本文摘自《锋利的SQL》： http://item.jd.com/10380652.html. 在Microsoft SQLServer Management Studio中执行查询时，如果选定工具栏中的 按钮，可以看到为查询生成的执行计划. 执行计划以图形方式显示了SQL Server查询优化器选择的数据检索方法，如表扫描、排序、哈希匹配等.

理解EM算法

- Chin - 我爱自然语言处理
EM(Expectation-Maximization)算法在机器学习和自然语言处理应用非常广泛，典型的像是聚类算法K-means和高斯混合模型以及HMM(Hidden Markov Model). 笔者觉得讲EM算法最好的就是斯坦福大学Andrew Ng机器学习课的讲课笔记和视频. 本文总结性的给出普遍的EM算法的推导和证明，希望能够帮助接触过EM算法但对它不是很明白的人更好地理解这一算法.

Memcached的LRU算法

- Eric - 平凡的世界
最近计划对Memcached做一些尝试性的改造，主要是针对Memcached在处理过期数据的时候进行改造，以实现在一个缓存的过期时间达到的时候，可以对该缓存的数据进行一个验证和存储的处理. 这个需求，主要是为了解决MySQL的写入瓶颈，通过延期、合并写入请求来减少MySQL的并发写入量. 现在逐渐记录出来和有需要的朋友一起讨论.