mypoint.m

Euclidean节点定位是由美国路特葛斯大学利用距离矢量路由和GPS定位原理提供出一种分布式自主定位算法

function [p1,p2]=mypoint(x1,y1,x2,y2,r1,r2)
 lim0=1e-9;
 ppi=acos(-1);
d=sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
     a=2.0*r1*(x1-x2);
     b=2.0*r1*(y1-y2);
     c=r2*r2-r1*r1-(x1-x2)*(x1-x2)-(y1-y2)*(y1-y2);
     p=a*a+b*b;
     q=-2.0*a*c;
     c1=-q/p/2.0;
     s1=sqrt(1-c1*c1);
     x=r1*c1+x1;
     y=r1*s1+y1;
     if(lim0<abs((x-x2)*(x-x2)+(y-y2)*(y-y2)-r2*r2))
      y=-r1*s1+y1;
     end
    x=abs(x);
    y=abs(y);
  
    if x>100
       x=100-(x-100);
   end
   if y>100
       y=100-(y-100);
   end
   p1=x;
   p2=y;