该楼层疑似违规已被系统折叠 隐藏此楼查看此楼

%三边测量的定位算法

%dA,dB,dC为A,B,C到未知节点(假定坐标[x,y]未知)的模拟测量距离

A = [0,0];

B = [25,25*sqrt(3)];

C = [50,0];

dA=51/sqrt(3);

dB=51/sqrt(3);

dC=51/sqrt(3);

%A,B,C为三个选定的信标节点,节点坐标已知(为便于防真及验证,代码中采用的等边三角形)

%定义未知坐标x,y为符号变量

syms x y;

%距离方程,以信标节点为圆心,信标节点到未知节点的测量距离为半径作三个圆

f1 = (A(1)-x)^2+(A(2)-y)^2-dA^2;

f2 = (B(1)-x)^2+(B(2)-y)^2-dB^2;

f3 = (C(1)-x)^2+(C(2)-y)^2-dC^2;

%任两个方程联立,求任两圆交点

s1 = solve(f1,f2); %求A,B两圆的交点

s2 = solve(f2,f3); %求B,C两圆的交点

s3 = solve(f1,f3); %求A,C两圆的交点

%将结果(符号变量)转换为双精度数值

x1 = double(s1.x);

y1 = double(s1.y);

x2 = double(s2.x);

y2 = double(s2.y);

x3 = double(s3.x);

y3 = double(s3.y);

%选择内侧的三个交点

%两圆相交于两点,距第三个圆心近的为选定交点Pab,Pbc,Pac

d1(1) = sqrt(((C(1)-x1(1))^2+(C(2)-y1(1))^2));

d1(2) = sqrt(((C(1)-x1(2))^2+(C(2)-y1(2))^2));

if d1(1) <= d1(2)

Pab(1) = x1(1);

Pab(2) = y1(1);

else

Pab(1) = x1(2);

Pab(2) = y1(2);

end

d2(1) = sqrt(((A(1)-x2(1))^2+(A(2)-y2(1))^2));

d2(2) = sqrt(((A(1)-x2(2))^2+(A(2)-y2(2))^2));

if d2(1) <= d2(2)

Pbc(1) = x2(1);

Pbc(2) = y2(1);

else

Pbc(1) = x2(2);

Pbc(2) = y2(2);

end

d3(1) = sqrt(((B(1)-x3(1))^2+(B(2)-y3(1))^2));

d3(2) = sqrt(((B(1)-x3(2))^2+(B(2)-y3(2))^2));

if d3(1) <= d3(2)

Pac(1) = x3(1);

Pac(2) = y3(1);

else

Pac(1) = x3(2);

Pac(2) = y3(2);

end

%求三个圆内侧三个交点Pab,Pbc,Pac的质心,即为未知节点P,完成定位

P(1) = (Pab(1)+Pbc(1)+Pac(1))/3;

P(2) = (Pab(2)+Pbc(2)+Pac(2))/3;

转为c的代码如下

编译链接环境VC6.0

代码如下：

//三边测量的定位算法

//dA,dB,dC为A,B,C到未知节点(假定坐标[x,y]未知)的模拟测量距离

#include

#include

struct point_t

{

double x;

double y;

};

struct circle_t

{

struct point_t center;//圆心

double r;//半径

};

int double_equals(double const a, double const b)

{

static const double ZERO = 1e-9;

return fabs(a - b) < ZERO;

}

double distance_sqr(struct point_t const* a, struct point_t const* b)

{

return (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y);

}

double distance(struct point_t const* a, struct point_t const* b)

{

return sqrt(distance_sqr(a, b));

}

double distance_pow(double x1,double y1,double x2,double y2)

{

return pow((x1-x2),2) + pow((y1-y2),2);

}

int insect(struct circle_t circles[], struct point_t points[])

{

double d, a, b, c, p, q, r;

double cos_value[2], sin_value[2];

if (double_equals(circles[0].center.x, circles[1].center.x)

&& double_equals(circles[0].center.y, circles[1].center.y)

&& double_equals(circles[0].r, circles[1].r))

{

return -1;

}

d = distance(&circles[0].center, &circles[1].center);

if (d > circles[0].r + circles[1].r

|| d < fabs(circles[0].r - circles[1].r))

{

return 0;

}

a = 2.0 * circles[0].r * (circles[0].center.x - circles[1].center.x);

b = 2.0 * circles[0].r * (circles[0].center.y - circles[1].center.y);

c = circles[1].r * circles[1].r - circles[0].r * circles[0].r

- distance_sqr(&circles[0].center, &circles[1].center);

p = a * a + b * b;

q = -2.0 * a * c;

if (double_equals(d, circles[0].r + circles[1].r)

|| double_equals(d, fabs(circles[0].r - circles[1].r))){

cos_value[0] = -q / p / 2.0;

sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);

points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;

points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;

if (!double_equals(distance_sqr(&points[0], &circles[1].center),circles[1].r * circles[1].r))

{

points[0].y = circles[0].center.y - circles[0].r * sin_value[0];

}

return 1;

}

r = c * c - b * b;

cos_value[0] = (sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;

cos_value[1] = (-sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;

sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);

sin_value[1] = sqrt(1 - cos_value[1] * cos_value[1]);

points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;

points[1].x = circles[0].r * cos_value[1] + circles[0].center.x;

points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;

points[1].y = circles[0].r * sin_value[1] + circles[0].center.y;

if (!double_equals(distance_sqr(&points[0], &circles[1].center),circles[1].r * circles[1].r))

{

points[0].y = circles[0].center.y - circles[0].r * sin_value[0];

}

if (!double_equals(distance_sqr(&points[1], &circles[1].center),circles[1].r * circles[1].r))

{

points[1].y = circles[0].center.y - circles[0].r * sin_value[1];

}

if (double_equals(points[0].y, points[1].y)&& double_equals(points[0].x, points[1].x))

{

if (points[0].y > 0)

{

points[1].y = -points[1].y;

}

else

{

points[0].y = -points[0].y;

}

}

return 2;

}

void main()

{

point_t Pab;

point_t Pbc;

point_t Pac;

point_t A;

point_t B;

point_t C;

double dA,dB,dC;

A.x = 0.0; //圆心

A.y = 0.0;

B.x = 25.0; //圆心

B.y = 25.0*sqrt(3);

C.x = 50.0; //圆心

C.y = 0.0;

dA = 51.0/sqrt(3);//半径

dB = 51.0/sqrt(3);//半径

dC = 51.0/sqrt(3);//半径

//A,B,C为三个选定的信标节点,节点坐标已知(为便于防真及验证,代码中采用的等边三角形)

//定义未知坐标x,y为符号变量

//距离方程,以信标节点为圆心,信标节点到未知节点的测量距离为半径作三个圆

struct circle_t circles_AB[2];

struct point_t points_AB[2];

circles_AB[0].center.x = A.x;

circles_AB[0].center.y = A.y;

circles_AB[0].r = dA;

circles_AB[1].center.x = B.x;

circles_AB[1].center.y = B.y;

circles_AB[1].r = dB;

switch (insect(circles_AB, points_AB))

{

case -1:

printf("THE CIRCLES ARE THE SAME\n");

break;

case 0:

printf("NO INTERSECTION\n");

break;

case 1:

printf("(%.3lf %.3lf)\n", points_AB[0].x, points_AB[0].y);

break;

case 2:

printf("求A,B两圆的交点为\n");

printf("(%.3lf %.3lf) (%.3lf %.3lf)\n",points_AB[0].x, points_AB[0].y,

points_AB[1].x, points_AB[1].y);

}

//printf("(%.3lf %.3lf %.3lf)\n", A.x, A.y,dA);

//printf("(%.3lf %.3lf %.3lf)\n", B.x, B.y,dA);

//printf("(%.3lf %.3lf %.3lf)\n", C.x, C.y,dA);

double points_AB_0 = distance_pow(points_AB[0].x,points_AB[0].y,C.x,C.y);

double points_AB_1 = distance_pow(points_AB[1].x,points_AB[1].y,C.x,C.y);

if(points_AB_0 < points_AB_1)

{

Pab.x = points_AB[0].x;

Pab.y = points_AB[0].y;

}

else

{

Pab.x = points_AB[1].x;

Pab.y = points_AB[1].y;

}

struct circle_t circles_BC[2];

struct point_t points_BC[2];

circles_BC[0].center.x = B.x;

circles_BC[0].center.y = B.y;

circles_BC[0].r = dB;

circles_BC[1].center.x = C.x;

circles_BC[1].center.y = C.y;

circles_BC[1].r = dC;

switch (insect(circles_BC, points_BC))

{

case -1:

printf("THE CIRCLES ARE THE SAME\n");

break;

case 0:

printf("NO INTERSECTION\n");

break;

case 1:

printf("(%.3lf %.3lf)\n", points_BC[0].x, points_BC[0].y);

break;

case 2:

printf("求B,C两圆的交点为\n");

printf("(%.3lf %.3lf) (%.3lf %.3lf)\n",points_BC[0].x, points_BC[0].y,

points_BC[1].x, points_BC[1].y);

}

double points_BC_0 = distance_pow(points_BC[0].x,points_BC[0].y,A.x,A.y);

double points_BC_1 = distance_pow(points_BC[1].x,points_BC[1].y,A.x,A.y);

if(points_BC_0 < points_BC_1)

{

Pbc.x = points_BC[0].x;

Pbc.y = points_BC[0].y;

}

else

{

Pbc.x = points_BC[1].x;

Pbc.y = points_BC[1].y;

}

struct circle_t circles_AC[2];

struct point_t points_AC[2];

circles_AC[0].center.x = A.x;

circles_AC[0].center.y = A.y;

circles_AC[0].r = dA;

circles_AC[1].center.x = C.x;

circles_AC[1].center.y = C.y;

circles_AC[1].r = dC;

switch (insect(circles_AC, points_AC))

{

case -1:

printf("THE CIRCLES ARE THE SAME\n");

break;

case 0:

printf("NO INTERSECTION\n");

break;

case 1:

printf("(%.3lf %.3lf)\n", points_AC[0].x, points_AC[0].y);

break;

case 2:

printf("求A,C两圆的交点为\n");

printf("(%.3lf %.3lf) (%.3lf %.3lf)\n",points_AC[0].x, points_AC[0].y,

points_AC[1].x, points_AC[1].y);

}

double points_AC_0 = distance_pow(points_AC[0].x,points_AC[0].y,B.x,B.y);

double points_AC_1 = distance_pow(points_AC[1].x,points_AC[1].y,B.x,B.y);

if(points_AC_0 < points_AC_1)

{

Pac.x = points_AC[0].x;

Pac.y = points_AC[0].y;

}

else

{

Pac.x = points_AC[1].x;

Pac.y = points_AC[1].y;

}

double P_1,P_2;

P_1 = (Pab.x + Pbc.x + Pac.x)/3.0;

P_2 = (Pab.y + Pbc.y + Pac.y)/3.0;

printf("三个圆内侧三个交点Pab,Pbc,Pac的质心为\n");

printf("(%.3lf %.3lf)\n", P_1, P_2);

}