test2.cpp

关于无线传感器网络节点定位技术

#include<iostream>
#include<cmath>
#include<stdlib.h>
#include<time.h>
using namespace std;
const double pi=3.1415926;
struct node{double x,y,wucha,w;int ok;}a[111],a1[111];
struct node2{int n1,n2,n3;double w;}d[3333],o;
double e[111][111],totw;
bool c[111][111];
int n,l,v,m,p,q,i,j,chang,kuan;
double wucha,totwc;
double maxvalue(double x,double y);
double minvalue(double x,double y);

double maxvalue(double x,double y)
{
	return (x>y?x:y);
}

double minvalue(double x,double y)
{
	return (x>y?y:x);
}

int jiefangcheng(int i0,int i1,int i2,int i3,double w){
  double d1,d2,d3,y1,y2,x1,x2,t1,t2,t3,t4,o,x,y,a11,a12,a21,a22,zhi,p,q,b11,b12,b21,b22;
  if(e[i1][i2]+e[i2][i0]<e[i1][i0])return 0;
  p=2*(a[i2].x-a[i1].x);
  q=2*(a[i2].y-a[i1].y);
  o=e[i0][i1]*e[i0][i1]-e[i0][i2]*e[i0][i2]-a[i1].x*a[i1].x+a[i2].x*a[i2].x-a[i1].y*a[i1].y+a[i2].y*a[i2].y;
  if(abs(p)<1e-6){
    y2=y1=o/q;
    t1=1;
    t2=-2*a[i1].x;
    t3=a[i1].x*a[i1].x-e[i0][i1]*e[i0][i1]+(y1-a[i1].y)*(y1-a[i1].y);
    if(t2*t2-4*t1*t3<-1e-6)return 0;
    x1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
    x2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
    }else{
    t1=q*q/p/p+1;
    t2=-2*o*q/p/p+2*a[i1].x*q/p-2*a[i1].y;
    t3=o*o/p/p+a[i1].x*a[i1].x-2*a[i1].x*o/p+a[i1].y*a[i1].y-e[i0][i1]*e[i0][i1];
    if(t2*t2-4*t1*t3<-1e-6)return 0;
    y1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
    y2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
    x1=(o-q*y1)/p;
    x2=(o-q*y2)/p;
    }
  d1=(x1-a[i3].x)*(x1-a[i3].x)+(y1-a[i3].y)*(y1-a[i3].y);
  d2=(x2-a[i3].x)*(x2-a[i3].x)+(y2-a[i3].y)*(y2-a[i3].y);
  d3=e[i0][i3]*e[i0][i3];
  if(abs(d1-d3)<abs(d2-d3)){x=x1;y=y1;}else{x=x2;y=y2;}


  a1[i0].x+=w*x;
  a1[i0].y+=w*y;
  a1[i0].w+=w;
  ++a1[i0].ok;
  return 1;
}



void calw(){
  int i,j,k;
  double p,a1,a2,a3,dis1,dis2,dis3,mn,mx;
  for(i=1;i<=m-2;i++)
    for(j=i+1;j<=m-1;j++)
      for(k=j+1;k<=m;k++){
        ++l;
        d[l].n1=i;d[l].n2=j;d[l].n3=k;
        dis1=e[i][j];
        dis2=e[i][k];
        dis3=e[j][k];
        a1=acos((dis1*dis1-dis2*dis2-dis3*dis3)/2/dis2/dis3);
        a2=acos((dis2*dis2-dis1*dis1-dis3*dis3)/2/dis1/dis3);
        a3=acos((dis3*dis3-dis2*dis2-dis1*dis1)/2/dis2/dis1);
        mn=minvalue(minvalue(a1,a2),a3);
        mx=maxvalue(maxvalue(a1,a2),a3);
        d[l].w=1-(mx-mn)/pi;

        }
        
  for(i=1;i<=l;i++)
    for(j=i+1;j<=l;j++)if(d[i].w<d[j].w){o=d[i];d[i]=d[j];d[j]=o;}
  if(20>l)v=l;else v=20;

  totw=0;
  for(i=1;i<=v;i++){
    if(d[i].w>1e-3){
      totw+=d[i].w;
    }else 
    --v;
    }

}


void cal(int k){
  int i,j,l,tmp;
  for(i=1;i<=v;i++){
    if(e[d[i].n1][k]<e[d[i].n2][k])swap(d[i].n1,d[i].n2);
    if(e[d[i].n2][k]<e[d[i].n3][k])swap(d[i].n2,d[i].n3);
    if(e[d[i].n1][k]<e[d[i].n2][k])swap(d[i].n1,d[i].n2);
    //n1,n2,n3按与k的距离从大到小排序 
    
    if(jiefangcheng(k,d[i].n1,d[i].n2,d[i].n3,d[i].w));else 
    if(jiefangcheng(k,d[i].n1,d[i].n3,d[i].n2,d[i].w));else 
    if(jiefangcheng(k,d[i].n2,d[i].n3,d[i].n1,d[i].w));

    
//    jiefangcheng(k,d[i].n1,d[i].n2,d[i].n3);
    }

  
   a1[k].x=a1[k].x/a1[k].w;
   a1[k].y=a1[k].y/a1[k].w;
   //a1[k].x=a1[k].x/v;
   //a1[k].y=a1[k].y/v;
  //解得未知点为a1[k]   
}


int main(){

  srand(time(0));
  printf("请输入总节点个数，基站个数，误差值：");
  cin>>n>>m>>wucha;//m为基站个数 
  printf("请输入网络区域的长、宽：");
  cin>>chang>>kuan;
  ++chang;++kuan;
  freopen("output3.txt","w",stdout);
//m=10;wucha=0.3;
  for(i=1;i<=n;i++){
    while(1){
      p=rand()%chang;
      q=rand()%kuan;
      if(c[p][q]==0){
        c[p][q]=1;
        break;
        }
      }
    a[i].x=p;
    a[i].y=q;  
    }

  for(i=1;i<=n;i++)
    for(j=1;j<=n;j++)
      e[i][j]=sqrt((a[i].x-a[j].x)*(a[i].x-a[j].x)+(a[i].y-a[j].y)*(a[i].y-a[j].y))-wucha*10;

  calw();

  
  for(i=m+1;i<=n;i++)cal(i);
  
  for(i=m+1;i<=n;i++)if(a1[i].ok>0)a1[i].wucha=sqrt((a1[i].x-a[i].x)*(a1[i].x-a[i].x)+(a1[i].y-a[i].y)*(a1[i].y-a[i].y));
  for(i=m+1;i<=n;i++)if(a1[i].ok>0)totwc+=a1[i].wucha;
  
  printf("总误差:      "); 
  cout<<totwc<<endl;
   for(i=1;i<=m;i++)
	  a1[i].ok=1;
 // for(i=m+1;i<=n;i++)if(a1[i].ok){
   for(i=1;i<=n;i++)if(a1[i].ok){
    //printf("第%d个点的原始坐标:          ",i);
    cout<<a[i].x<<' '<<a[i].y<<' '<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
    //printf("算出的第%d个点的坐标和误差:  ",i);
    //cout<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
    }else cout<<"Cannot Find."<<endl;
  
  return(0); 
}