三点定位的算法

三点定位，
已知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; 
}