开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 通讯/手机编程 > 3G开发 > 查看源码
test3.cpp
资源名称：position(WSNs).rar　[点击查看]
上传用户：vipking888
上传日期：2015-02-03
资源大小：93k
文件大小：8k
源码类别：

3G开发

开发平台：

Visual C++

test3.cpp：源码内容
  1. #include<iostream>
  2. #include<cmath>
  3. #include<time.h>
  4. #include<stdlib.h>
  5. using namespace std;
  6. const double pi=3.1415926;
  7. //struct node{double x,y,wucha;bool ok;}a[111],a1[111];
  8. struct node{double x,y,wucha,w;int ok;}a[111],a1[111];
  9. struct node2{int n1,n2,n3;double w;}d[3333],o;
  10. double e[111][111],totw;
  11. bool c[111][111];
  12. int i,j,n,m,s,p,q,k,l,v,chang,kuan;
  13. double totwc,wucha;
  14. double (*r)[2];//矩阵指针
  15. double maxvalue(double x,double y);
  16. double minvalue(double x,double y);
  18. double maxvalue(double x,double y)
  19. {
  20. return (x>y?x:y);
  21. }
  23. double minvalue(double x,double y)
  24. {
  25. return (x>y?y:x);
  26. }
  28. int jiefangcheng(int i0,int i1,int i2,int i3){
  29.   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;
  30.   if(e[i1][i2]+e[i2][i0]<e[i1][i0])return 0;
  31.   p=2*(a[i2].x-a[i1].x);
  32.   q=2*(a[i2].y-a[i1].y);
  33.   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;
  34.   if(abs(p)<1e-6){
  35.     y2=y1=o/q;
  36.     t1=1;
  37.     t2=-2*a[i1].x;
  38.     t3=a[i1].x*a[i1].x-e[i0][i1]*e[i0][i1]+(y1-a[i1].y)*(y1-a[i1].y);
  39.     if(t2*t2-4*t1*t3<-1e-6)return 0;
  40.     x1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
  41.     x2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
  42.     }else{
  43.     t1=q*q/p/p+1;
  44.     t2=-2*o*q/p/p+2*a[i1].x*q/p-2*a[i1].y;
  45.     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];
  46.     if(t2*t2-4*t1*t3<-1e-6)return 0;
  47.     y1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
  48.     y2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
  49.     x1=(o-q*y1)/p;
  50.     x2=(o-q*y2)/p;
  51.     }
  52.   d1=(x1-a[i3].x)*(x1-a[i3].x)+(y1-a[i3].y)*(y1-a[i3].y);
  53.   d2=(x2-a[i3].x)*(x2-a[i3].x)+(y2-a[i3].y)*(y2-a[i3].y);
  54.   d3=e[i0][i3]*e[i0][i3];
  55.   if(abs(d1-d3)<abs(d2-d3)){x=x1;y=y1;}else{x=x2;y=y2;}
  57.   a1[i0].x=x;
  58.   a1[i0].y=y;
  59.   a1[i0].ok=1;
  60.   return 1;
  61. }
  64. int jiefangcheng1(int i0,int i1,int i2,int i3,double w){
  65.   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;
  66.   if(e[i1][i2]+e[i2][i0]<e[i1][i0])return 0;
  67.   p=2*(a[i2].x-a[i1].x);
  68.   q=2*(a[i2].y-a[i1].y);
  69.   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;
  70.   if(abs(p)<1e-6){
  71.     y2=y1=o/q;
  72.     t1=1;
  73.     t2=-2*a[i1].x;
  74.     t3=a[i1].x*a[i1].x-e[i0][i1]*e[i0][i1]+(y1-a[i1].y)*(y1-a[i1].y);
  75.     if(t2*t2-4*t1*t3<-1e-6)return 0;
  76.     x1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
  77.     x2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
  78.     }else{
  79.     t1=q*q/p/p+1;
  80.     t2=-2*o*q/p/p+2*a[i1].x*q/p-2*a[i1].y;
  81.     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];
  82.     if(t2*t2-4*t1*t3<-1e-6)return 0;
  83.     y1=(-t2+sqrt(t2*t2-4*t1*t3))/2/t1;
  84.     y2=(-t2-sqrt(t2*t2-4*t1*t3))/2/t1;
  85.     x1=(o-q*y1)/p;
  86.     x2=(o-q*y2)/p;
  87.     }
  88.   d1=(x1-a[i3].x)*(x1-a[i3].x)+(y1-a[i3].y)*(y1-a[i3].y);
  89.   d2=(x2-a[i3].x)*(x2-a[i3].x)+(y2-a[i3].y)*(y2-a[i3].y);
  90.   d3=e[i0][i3]*e[i0][i3];
  91.   if(abs(d1-d3)<abs(d2-d3)){x=x1;y=y1;}else{x=x2;y=y2;}
  94.   a1[i0].x+=w*x;
  95.   a1[i0].y+=w*y;
  96.   a1[i0].w+=w;
  97.   ++a1[i0].ok;
  98.   return 1;
  99. }
  101. void calw(){
  102.   int i,j,k;
  103.   double p,a1,a2,a3,dis1,dis2,dis3,mn,mx;
  104.   for(i=1;i<=m-2;i++)
  105.     for(j=i+1;j<=m-1;j++)
  106.       for(k=j+1;k<=m;k++){
  107.         ++l;
  108.         d[l].n1=i;d[l].n2=j;d[l].n3=k;
  109.         dis1=e[i][j];
  110.         dis2=e[i][k];
  111.         dis3=e[j][k];
  112.         a1=acos((dis1*dis1-dis2*dis2-dis3*dis3)/2/dis2/dis3);
  113.         a2=acos((dis2*dis2-dis1*dis1-dis3*dis3)/2/dis1/dis3);
  114.         a3=acos((dis3*dis3-dis2*dis2-dis1*dis1)/2/dis2/dis1);
  115.         mn=minvalue(minvalue(a1,a2),a3);
  116.         mx=maxvalue(maxvalue(a1,a2),a3);
  117.         d[l].w=1-(mx-mn)/pi;
  119.         }
  120.         
  121.   for(i=1;i<=l;i++)
  122.     for(j=i+1;j<=l;j++)if(d[i].w<d[j].w){o=d[i];d[i]=d[j];d[j]=o;}
  123.   if(20>l)v=l;else v=20;
  125.   totw=0;
  126.   for(i=1;i<=v;i++){
  127.     if(d[i].w>1e-3){
  128.       totw+=d[i].w;
  129.     }else 
  130.     --v;
  131.     }
  133. }
  135. void cal(int k){
  136.   int i,j,p,b[5],tmp=0;
  137.   bool c[111],hasans=0;
  138.   while(1){
  139.     ++tmp;
  140.     memset(c,0,sizeof(c));
  141.     for(i=1;i<=3;i++){
  142.       while(1){p=rand()%m+1;if(c[p]==0){c[p]=1;break;}}    
  143.       b[i]=p;
  144.       }
  145.     for(i=1;i<=3;i++)for(j=i+1;j<=3;j++)if(e[b[i]][k]<e[b[j]][k]){p=b[i];b[i]=b[j];b[j]=p;}
  146.     if(jiefangcheng(k,b[1],b[2],b[3])){hasans=1;break;}else 
  147.     if(jiefangcheng(k,b[1],b[3],b[2])){hasans=1;break;}else 
  148.     if(jiefangcheng(k,b[2],b[3],b[1])){hasans=1;break;}
  149.     if(tmp>10)break; 
  150.   }
  151.   
  152. }
  154. void cal1(int k){
  155.   int i,j,l,tmp;
  156.   for(i=1;i<=v;i++){
  157.     if(e[d[i].n1][k]<e[d[i].n2][k])swap(d[i].n1,d[i].n2);
  158.     if(e[d[i].n2][k]<e[d[i].n3][k])swap(d[i].n2,d[i].n3);
  159.     if(e[d[i].n1][k]<e[d[i].n2][k])swap(d[i].n1,d[i].n2);
  160.     //n1,n2,n3按与k的距离从大到小排序 
  161.     
  162.     if(jiefangcheng1(k,d[i].n1,d[i].n2,d[i].n3,d[i].w));else 
  163.     if(jiefangcheng1(k,d[i].n1,d[i].n3,d[i].n2,d[i].w));else 
  164.     if(jiefangcheng1(k,d[i].n2,d[i].n3,d[i].n1,d[i].w));
  166.     
  167. //    jiefangcheng(k,d[i].n1,d[i].n2,d[i].n3);
  168.     }
  170.   
  171.    a1[k].x=a1[k].x/a1[k].w;
  172.    a1[k].y=a1[k].y/a1[k].w;
  173.    //a1[k].x=a1[k].x/v;
  174.    //a1[k].y=a1[k].y/v;
  175.   //解得未知点为a1[k]   
  176. }
  178. //极大释然法
  179. void lateration()
  180. {
  182. }
  184. //初始矩阵
  185. void matrix_init()
  186. {
  187. //分配一个指针数组，其首地址保存在pMatrix中
  188.     double **pMatrix = new double*[m];
  190.     //为指针数组的每个元素分配一个数组
  191.     for (int i = 0; i < m; i++)
  192.     {
  193. pMatrix[i] = new double[2];
  194. }
  195.  for (int k = 0; k < m; k++)
  196.     {
  197. *(*(pMatrix+0))=2*(a[k].x -a[m].x);
  198. *(*(pMatrix+1))=2*(a[k].y -a[m].y);
  199. printf("%fn",*(*(pMatrix+1)));
  200. pMatrix++;
  201.     }
  202.     //以上是分配，以下是释放
  203.     for (int j = 0; j < m; j++)
  204.         delete [2] pMatrix[j];
  205.     delete [m] pMatrix;
  206. /*
  207. double (*p)[2]=new double[m][2]; 
  208.   //r=p;
  209.   for(i=0;i<m;i++)
  210.   {
  212.   }
  213.   for(i=0;i<m;i++)
  214.   {
  215. //   printf("%f,%fn",*(*(r+i)),*(*(r+i)+1));
  216.   }
  217.   delete[] p;
  218.  */
  219. }
  220.         
  221. /// 矩阵的转置
  222.         void MatrixInver(double (*a)[2], double (*b)[2])
  223.         {
  224.             for (int i = 0; i < m; i++)
  225. {
  226. *(*(b+i)) = *(*(a+i));
  227. *(*(b+i)+1) = *(*(a+i)+1);
  228. }
  229.         }
  232. int main(){
  233.   
  234.   srand(time(0));
  235.   printf("请输入总节点个数，基站个数，误差值：");
  236.   cin>>n>>m>>wucha;//m为基站个数 
  237.   printf("请输入网络区域的长、宽：");
  238.   cin>>chang>>kuan;
  239.   ++chang;++kuan;
  241.   
  242. //m=10;wucha=0.3;
  243.   for(i=1;i<=n;i++){
  244.     while(1){
  245.       p=rand()%chang;
  246.       q=rand()%kuan;
  247.       if(c[p][q]==0){
  248.         c[p][q]=1;
  249.         break;
  250.         }
  251.       }
  252.     a[i].x=p;
  253.     a[i].y=q;  
  254.     }
  255.     
  256. //a[1].x=41;a[1].y=85;a[2].x=72;a[2].y=38;a[3].x=80;a[3].y=69;a[4].x=96;a[4].y=22;
  257.   for(i=1;i<=n;i++)
  258.     for(j=1;j<=n;j++)
  259.       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;
  261.   
  262.    
  263.   for(i=m+1;i<=n;i++)cal(i);
  264.   
  265.   for(i=m+1;i<=n;i++)if(a1[i].ok)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));
  266.   for(i=m+1;i<=n;i++)if(a1[i].ok)totwc+=a1[i].wucha;
  267.   
  268.   freopen("output1.txt","w",stdout);
  269.   printf("总误差:      "); 
  270.   cout<<totwc<<endl;
  271.   for(i=1;i<=m;i++)
  272.   a1[i].ok=1;
  273.  // for(i=m+1;i<=n;i++)if(a1[i].ok){
  274.   for(i=1;i<=n;i++)if(a1[i].ok){
  275.     //printf("第%d个点的原始坐标:          ",i);
  276.     cout<<a[i].x<<' '<<a[i].y<<' '<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
  277.     //printf("算出的第%d个点的坐标和误差:  ",i);
  278.     //cout<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
  279.     }else cout<<"Cannot Find."<<endl;
  280.  
  281. for(i=0;i<=110;i++)
  282. {
  283. a1[i].x=0;
  284. a1[i].y=0;
  285. a1[i].wucha=0;
  286. }
  288. totwc=0;
  289.  matrix_init();
  290.  freopen("output3.txt","w",stdout);
  291.  calw();
  292.  for(i=m+1;i<=n;i++)cal1(i);
  294.   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));
  295.   for(i=m+1;i<=n;i++)if(a1[i].ok>0)totwc+=a1[i].wucha;
  296.   
  297.   printf("总误差:      "); 
  298.   cout<<totwc<<endl;
  299.    for(i=1;i<=m;i++)
  300.   a1[i].ok=1;
  301.  // for(i=m+1;i<=n;i++)if(a1[i].ok){
  302.    for(i=1;i<=n;i++)if(a1[i].ok){
  303.     //printf("第%d个点的原始坐标:          ",i);
  304.     cout<<a[i].x<<' '<<a[i].y<<' '<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
  305.     //printf("算出的第%d个点的坐标和误差:  ",i);
  306.     //cout<<a1[i].x<<' '<<a1[i].y<<' '<<a1[i].wucha<<endl;
  307.     }else cout<<"Cannot Find."<<endl;
  310.   
  311.   return(0); 
  312. }