%##################################################%%  RFUSP%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%parametersclear;DX=100;DY=100;DZ=100;nodeNumber=1000;comR=100;anchorN=9;derror=1.1;lim=1e-2;cur=1000;%DX*(1-derror);len=sqrt(DX^2+DY^2+DZ^2);cur=10;perror=1-(derror-1); %0.996;optR=sqrt((DX*DY/nodeNumber))/sqrt(2);%anchorN=anchorN+1;%%%%%%%%%init%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Countanchor=0;flag=1;for i=1:nodeNumber    node(i).ID=i;   if(Countanchor<anchorN)     node(i).type=0; % 0 is anchor,1 is normal;    Countanchor=Countanchor+1;   else       node(i).type=1;       end   node(i).tPosition.x=rand(1,1)*(DX);   node(i).tPosition.y=rand(1,1)*(DY);   node(i).tPosition.z=rand(1,1)*(DZ);   if node(i).type==0	%    switch flag        case 1     node(i).tPosition.x=rand(1,1)*DX;     node(i).tPosition.y=0;     flag=2;        case 2      node(i).tPosition.y=rand(1,1)*DY;      node(i).tPosition.x=0;         flag=3;        case 3     node(i).tPosition.x=rand(1,1)*DX;     node(i).tPosition.y=DY;     flag=4;        case 4       node(i).tPosition.y=rand(1,1)*DY;     node(i).tPosition.x=DX;      flag=1;    end              %}       node(i).ePosition.x= node(i).tPosition.x;     node(i).ePosition.y= node(i).tPosition.y;    node(i).ePosition.z= node(i).tPosition.z;    node(i).tPosition.z=0;	node(i).finish=1;    % hold on;   % plot(node(i).tPosition.x,node(i).tPosition.y,'r*');   else     node(i).ePosition.x=-1;     node(i).ePosition.y=-1;     node(i).ePosition.z=-1;     node(i).finish=0;     end   node(i).Heigh=(DZ)-node(i).tPosition.z;   node(i).test=0;end%%%%%%%plot the node%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	for i=1:nodeNumber if(node(i).type==0)     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'r*'); else    plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'+'); end hold on;end grid on;%%%%%%%%%%%%%%%%%%topoly establishment%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%}%{connectivity=0; for i=1:nodeNumber    k=0;    nodeA=node(i);     for j=1:nodeNumber         nodeB=node(j);         H=abs(nodeA.Heigh-nodeB.Heigh);       if((j~=i)&& dst(nodeA,nodeB)<=comR && H<comR)         k=k+1;           neighbor(i).near(k)=j;          end     end      node(i).neighbors=k;      connectivity=connectivity+k; endconnect=0; for i=1:nodeNumber     if node(i).neighbors==0         connect=connect+1;         node(i).finish=-1;       i;     end end connect connectivity=connectivity/nodeNumber%}%{%%%%%%%%%%%%%%%%%plot 3Dtopo%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for i=1:nodeNumber    A=node(i);    for j=1:node(i).neighbors       B=node(neighbor(i).near(j));        plotneighbor(A,B);        %plotplane(A,B);    endend%}judge=zeros(anchorN,anchorN);for i=1:anchorN    for j=1:anchorN        if(i~=j)            judge(i,j)=pdst(node(i),node(j));        end    endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for i=1:nodeNumber    if node(i).type~=0&&node(i).finish~=1       for j=1:anchorN          dd=dst(node(i),node(j))*derror;          H=abs(node(i).Heigh-node(j).Heigh)*derror;          r(j)=sqrt(dd^2-H^2);        end      found=0;      a1=-1;      a2=-1;         for k=1:anchorN-1          for j=k+1:anchorN             if(k~=j)               d1=r(k)+r(j);               d2=abs(r(k)-r(j));               ca1=d1/judge(k,j);                ca2=d2/judge(k,j);                if ca1>=perror&&ca1<=1+(1-perror) || ca2>=perror&&ca2<=1+(1-perror)              % if round(ca1*100/100)==1 || round(ca2*100/100)==1                     a1=k;a2=j;                      node(i).test=max([a1 a2]);                      node(i).err=min([abs(d1-judge(k,j)) abs(d2-judge(k,j))]);                       x1=node(a1).tPosition.x;                      y1=node(a1).tPosition.y;                      x2=node(a2).tPosition.x;                     y2=node(a2).tPosition.y;                  % if x1~=x2 || y1~=y2                     found=1;                    break;                   %end                 end             end          end          if found==1              break;          end                   end         if found==1         x1=node(a1).tPosition.x;        y1=node(a1).tPosition.y;       x2=node(a2).tPosition.x;      y2=node(a2).tPosition.y;       r1=r(a1);       r2=r(a2);       %[node(i).ePosition.x,node(i).ePosition.y]=mypoint(x1,y1,x2,y2,r1,r2);       [node(i).ePosition.x,node(i).ePosition.y]=cal(x1,y1,x2,y2,r1,r2);       node(i).ePosition.z=abs(node(a1).tPosition.z+(node(a1).Heigh-node(i).Heigh));       node(i).finish=1;       %%%%%%refine%%%%%%%%%%%%%%%%%%%%%%%%%%%%       end                 end    end    %%%%%%%%%%%%%count%%%%%%%%%%%%%%%%%%%%%%%%%  %  myerror=0;  outR=0;  locait=0;  test1=0;  myerror1=0;  err=0;  for i=1:nodeNumber    if (node(i).type==1 && node(i).finish==1)         myd=edst(node(i),node(i));         test1=test1+node(i).test;         if myd<cur         locait=locait+1;         err=err+node(i).err;        myerror1=myerror1+myd;       else         node(i).finish=3;         outR=outR+1;       end    end  end  ratio=locait/(nodeNumber-anchorN)  test=test1/locaitmyerror=myerror1/locait  outR;  err=err/locait  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    %{if derror<1while outR~=0  for i=1:nodeNumber    z=1:3;      if (node(i).type==1 && node(i).finish==3)          c=0;          for j=1:nodeNumber              if (i~=j && node(j).finish==1)              d1=abs(node(i).tPosition.x-node(j).tPosition.x);              d2=abs(node(i).tPosition.y-node(j).tPosition.y);               d3=abs(node(i).tPosition.z-node(j).tPosition.z);               if d1<comR || d2<comR || d3<comR                   c=c+1;                  z(c)=j;               end              end              if c==3                  break;              end          end            if c==3             A=zeros(2,2);              B=zeros(2,1);              ex=zeros(2,1);             last=z(3);              x3=node(last).ePosition.x;              y3=node(last).ePosition.y;               z3=node(last).ePosition.z;               d3=pdst(node(last),node(i))*(derror);              for k=1:2                  id=z(k);                  x1=node(id).ePosition.x;                 y1=node(id).ePosition.y;                 z1=node(id).ePosition.z;                 d1=pdst(node(id),node(i))*(derror);                 A(k,1)=2*(x1-x3);A(k,2)=2*(y1-y3);                 B(k,1)=(x1^2-x3^2)+(y1^2-y3^2)+(d3^2-d1^2);                k=k+1;                 end            end         % [ex,n]=locatCacul(A,B,0.001,1);           if rank(A)==rank([A B])            ex=A\B;           % if (ex(1,1)>0&&ex(1,1)<DX)&& (ex(2,1)>0&&ex(2,1)<DY)              node(i).ePosition.z=abs(node(last).tPosition.z+(node(last).Heigh-node(i).Heigh));              node(i).ePosition.x=abs(ex(1,1));               node(i).ePosition.y=abs(ex(2,1));                              %node(i).ePosition.z=abs(ex(3,1));              if node(i).finish==3               node(i).finish=1;               outR=outR-1;              end           end         % else           end  end   end   end %%%%%%%%%%%%%%count%%%%%%%%%%%%%%%%%%%%%%%%%  myerror=0;  outR=0;  locait=0;  for i=1:nodeNumber    if (node(i).type==1 && node(i).finish==1)         myd=edst(node(i),node(i));         if myd/DX<cur         locait=locait+1;        myerror=myerror+myd;       else         node(i).finish=3;       end    end  end  ratio=locait/(nodeNumber-anchorN)  myerror=myerror/locait/100  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %}%%%%%%%%%%%%%%%%%%%plot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  for i=1:nodeNumber if(node(i).type==0)     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'^'); else    if(node(i).finish==1)    plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'o');    ploterr(node(i),node(i));    plot3(node(i).ePosition.x,node(i).ePosition.y,node(i).ePosition.z,'r*');    else     plot3(node(i).tPosition.x,node(i).tPosition.y,node(i).tPosition.z,'kx');        end end hold on;end grid on;%}%{for i=1:anchorN    A=node(i);    for j=1:anchorN       B=node(j);        plotplane(A,B);endend%}