加载中…
个人资料
  • 博客等级:
  • 博客积分:
  • 博客访问:
  • 关注人气:
  • 获赠金笔:0支
  • 赠出金笔:0支
  • 荣誉徽章:
正文 字体大小:

DV-Hop定位算法C++实现

(2009-06-12 19:05:37)
标签:

杂谈

分类: localization
DV-Hop的定位算法,已知网络中几个锚节点就可以计算出网络中其他点的坐标,但这种算法有时由于网络密度的稀疏,可能误差会有一些。
#include<stdio.h>
#include<stdlib.h>
#include<time.h>
#include<math.h>
#include<iostream.h>
#define N   100   // 总节点数
#define R   0.5   // 最大通信距离
#define MAX    // 希望的每一个节点上通过的最大路径数
double cordination[200] =    // 确定的节点坐标
{
0.219600, -0.389400, -0.712000, -0.419000 , -0.690900, -0.232900, 0.702400, -0.155200, 0.742600, -0.644400,
-0.884500, -0.125000, -0.453200, 0.561600, -0.876600, -0.245000, 0.424800, 0.775800, -0.887000, -0.154400,
0.283600, 0.931100, 0.056000, -0.298900, 0.504900, -0.872400, -0.204100, 0.468400, -0.253000, 0.242200,
-0.821500, 0.780000, -0.895500, 0.869400, 0.173800, -0.063200, 0.589000, -0.272600, 0.880000, -0.176000,
-0.421400, -0.859900, -0.844000, 0.925000, 0.067400, -0.557200, 0.252100, -0.676200, 0.713300, 0.065000,
0.432600, -0.124500, 0.346000, 0.218400, -0.346200, 0.948000, -0.784000, -0.596000, -0.128800, 0.481100,
0.862400, -0.267800, 0.976800, 0.113600, -0.165100, -0.096200, -0.137600, -0.478400, -0.424200, 0.374000,
-0.856000, 0.695000, -0.285800, -0.104000, 0.021600, -1.000000, 0.274800, -0.052000, -0.372800, -0.281600,
-0.270000, 0.780600, -0.262500, 0.848500, -0.572000, -0.892800, -0.880000, -0.436800, -0.641400, -0.301900,
-0.270400, 0.288800, -0.712500, 0.614800, 0.805600, -0.735000, 0.706300, 0.651000, -0.820400, -0.856000,
0.601700, 0.147600, -0.064400, 0.432000, 0.797800, 0.549500, 0.388100, 0.454100, 0.555900, -0.382000,
-0.086000, -0.646000, 0.366000, -0.175900, -0.091200, -0.734000, 0.050600, 0.126000, -0.768400, -0.561600,
-0.785600, -0.015600, 0.174000, -0.888800, 0.058700, 0.480000, -0.992000, 0.996000, 0.046000, -0.855300,
-0.660000, -0.061600, 0.762500, -0.428800, 0.682000, 0.631700, -0.120000, 0.784600, -0.153600, -0.806600,
-0.519400, 0.778900, -0.502700, 0.059500, 0.113700, 0.616200, 0.563400, 0.129000, -0.886400, -0.692500,
0.544000, -0.932000, 0.908000, 0.259500, -0.546100, -0.136400, -0.778700, 0.977100, 0.748800, -0.088400,
-0.986200, -0.619800, 0.053000, -0.766000, -0.082400, 0.339200, 0.566900, -0.159200, -0.631200, -0.034800,
0.768400, 0.515200, 0.697900, -0.156800, -0.841900, -0.045500, -0.236500, -0.065600, -0.958400, -0.677600,
-0.831400, -0.856500, -0.269100, -0.718000, -0.318000, -0.389500, 0.292700, -0.181600, -0.054400, -0.864000,
0.792400, -0.625600, 0.716600, 0.546000, -0.776000, 0.959600, -0.365800, 0.197200, -0.168000, 0.660000
};
struct Point      // 等分点结构
{
 double x;
 double y;
 int K;   // 等分数
};
struct Node                  // 节点结构
{
 int  id;                 // 标号
 double  position_x;      // 横坐标
 double  position_y;      // 纵坐标
 char neb[N];             // 邻接状态表
 int count;               // 路径经过该节点的次数
 int next_id;             // 下一跳节点号
 int hop;                 // 到目标节点需要的跳数
    double total_distance;   // 到目标节点需要传播的距离
 double disoff;           // 下一跳节点偏离等分点的距离
};
void  Init(Node *nod, int *id, int num);  // 初始化节点
void  Print_Node(Node *nod, FILE *fp);     // 打印初始化的节点信息
void  CreateNet(Node *nod, int num);     // 构建节点网络
void  Print_Net(Node *nod, FILE *fp);     // 打印某一个节点的网络连接情况
Point Compute_DividePoint(double x1, double y1, double x2, double y2);  // 计算等分点坐标
void  SearchRoutine(Node *nod, FILE *fp);   // 寻找合理路径
void  Print_RoutNum(Node *nod, FILE *fp);   // 打印节点上通过的路径数
void  Compute_repeat(Node *nod, FILE *fp);   // 计算重复往返路径数(现在可以保证不会出现重复往返路径)
void  Compute_hop_disoff(Node *nod, FILE *fp);  // 计算每个节点到目标节点需要经历的跳数和下一跳节点与等分点偏移距离
void  Compute_totalhop(Node *nod, FILE *fp);  // 计算所有节点到目标节点的总跳数
void  Init_new(Node *nod, int *id);  // 使用确定的坐标初始化节点
void  Compute_totaldisoff(Node *nod, FILE *fp);   // 计算所有节点的下一跳节点与相应等分点的偏移距离
void main()
{
 Node nod[N];    // 节点数组
    int id = 0;            // 节点id号
    FILE *fp1;
 FILE *fp2;
    FILE *fp3;
 FILE *fp4;
 char filename1[] = "output.txt";
 char filename2[] = "routine.txt";
 char filename3[] = "node1.txt";
 char filename4[] = "capability.txt";
 if((fp1=fopen(filename1, "w")) == NULL)   // 文件"output.txt"存放节点网络信息
 {
  printf("Can not open the output file!\n");
  exit(1);
 }
 if((fp2=fopen(filename2, "w")) == NULL)   // 文件"routine.txt"存放路由信息
 {
  printf("Can not open the output file!\n");
  exit(1);
 }
    if((fp3=fopen(filename3, "w")) == NULL)   // 文件"node1.txt"存放初始节点位置
 {
  printf("Can not open the output file!\n");
  exit(1);
 }
    if((fp4=fopen(filename4, "w")) == NULL)   // 文件"capability.txt"存放路由性能信息
 {
  printf("Can not open the output file!\n");
  exit(1);
 }
    else
 {
  srand((unsigned)time(NULL));    // 初始化随机数种子
     Init(nod, &id, N);            // 随机初始化未知节点
        //Init_new(nod, &id);             // 使用确定的坐标初始化节点
        Print_Node(nod, fp3);           // 打印初始化的节点信息
     CreateNet(nod, N);              // 创建节点网络
     Print_Net(nod, fp1);            // 打印节点网络信息
  SearchRoutine(nod, fp2);        // 寻找合适路由,并输出路径
  Print_RoutNum(nod, fp4);        // 输出每个节点上通过的路径数
  Compute_hop_disoff(nod, fp4);   // 计算每个节点到目标节点的路径数与下一跳节点与等分点偏移距离
  Compute_totalhop(nod, fp4);     // 计算所有节点到目标节点的总路径数
  Compute_totaldisoff(nod, fp4);  // 计算所有节点的下一跳节点与相应等分点的偏移距离
 }
}
double AverageRandom(double min,double max)
{
 int minInteger = (int)(min*10000);
    int maxInteger = (int)(max*10000);
    int randInteger = rand()*rand();
    int diffInteger = maxInteger - minInteger;
    int resultInteger = randInteger % diffInteger + minInteger;
    return resultInteger/10000.0;
}
void Init(Node *nod, int *id, int num) 
{
 int i;
 for(i=0; i<num; i++)
 {
  nod[*id].id = *id + 1;     // id号从1-num
     nod[*id].position_x = AverageRandom(-1,1);
  nod[*id].position_y = AverageRandom(-1,1);
  nod[*id].count = 1;
  nod[*id].next_id = 1000;  // 下一跳节点号初始化为一个不存在的节点号
     nod[*id].hop = 0;         // 到目标节点跳数初始化为0
  nod[*id].total_distance = 0;   // 到目标节点需要传播的距离初始化为0
  (*id)++; 
 }
}
void Init_new(Node *nod, int *id)
{
 int i;
 for(i=0; i<200; i+=2)
 {
  nod[*id].id = *id + 1;     // id号从1-num
     nod[*id].position_x = cordination[i];
  nod[*id].position_y = cordination[i+1];
  nod[*id].count = 1;
  nod[*id].next_id = 1000;  // 下一跳节点号初始化为一个不存在的节点号
     nod[*id].hop = 0;         // 到目标节点跳数初始化为0
  nod[*id].total_distance = 0;   // 到目标节点需要传播的距离初始化为0
  (*id)++; 
 }
}
void Print_Node(Node *nod, FILE *fp)
{
 int i;
 
 for(i=0; i<N; i++)
 {
     fprintf(fp, " %f\t %f \n",
       nod[i].position_x, nod[i].position_y);
        fprintf(fp, "\n");
 
}
void CreateNet(Node *nod, int num)
{
 int i;
 int j;
 double distance;
 for(i=0; i<num; i++)
 {
  for(j=0; j<num; j++)
  {
   distance = sqrt(pow((nod[i].position_x-nod[j].position_x), 2)
    + pow((nod[i].position_y-nod[j].position_y), 2));  // 计算两节点间的距离
   if(distance <=R)
   {
    nod[i].neb[j] = 'Y';     // 在通信范围内,'Y'表示有连接
   }
   else
   {
    nod[i].neb[j] = 'N';     // 不在通信范围内,'N'表示无连接
   }
  }
 }
}
void Print_Net(Node *nod, FILE *fp)
{
 int i;;
 int j;
 for(i=0; i<N; i++)
 {
  fprintf(fp, "在node%d通信范围内的节点 : \n", nod[i].id);
     for(j=0; j<N; j++)
  {
   if(nod[i].neb[j] != 'N')
   {
    fprintf(fp, "to node %d : %c\n", j+1, nod[i].neb[j]);
   }
   else
   {
             continue;
   }
  }
  fprintf(fp, "\n");
 }
}
Point Compute_DividePoint(double x1, double y1, double x2, double y2)
{
 Point p;
 double distance;
 double k1;
 int K;
 distance = sqrt(pow((x1-x2), 2) + pow((y1-y2), 2));   // 计算两节点间的距离
 k1=distance / R;
 K = (int)k1 + 1;        // 计算等分数
   
 p.x = x1 + (double)(x2-x1)/K; 
   
    p.y = y1 + (double)(y2-y1)/K;
    return p;   // 返回坐标数组
}
void SearchRoutine(Node *nod, FILE *fp)
{
 
 int i;
 int j;
 int min;
 int flag = 0;
    double temp_distan;
 int k;
 int z;
 Point p;      // 等分点
 double distan[N] ;
    double dMin;
 for(i=0; i<N; i++)    
 {
  fprintf(fp, "Node %d to sink :\n", nod[i].id);
  temp_distan = sqrt(pow((nod[i].position_x-0), 2)+pow((nod[i].position_y-0), 2)); // 计算两节点间的距离
  if(temp_distan < R)   // 可以直接到达目标节点
  {
   fprintf(fp, "to sink directly.\n");
            fprintf(fp, "\n");
   nod[i].next_id = 0;    // 表示下一跳节点即为目标节点
   nod[i].disoff = 0;  // 下一跳节点就是目标节点
  }
        else      // 需要经历中继节点
  {
     
   p = Compute_DividePoint(nod[i].position_x, nod[i].position_y, 0, 0);
   
            temp_distan = sqrt(pow((nod[i].position_x-p.x), 2)+pow((nod[i].position_y-p.y), 2));
     
   for(k=0; k<N; k++) 
   {
    
    if(nod[i].neb[k] == 'N') 
    {
     distan[k] = 999999;
    }
                if(nod[i].neb[k] == 'Y') 
    {
     distan[k] = sqrt(pow((nod[k].position_x-p.x),2) + pow((nod[k].position_y-p.y),2));  //计算通信半径内的点到等分点的距离
         
   }
    
            dMin = distan[0];   // 首先假定第一个点距等分点距离最短
   min = 0;
   flag = 0;
   z = 2;
   
   
            while(flag == 0)
   {
    for(j=0; j<N; j++)
    {
     
     if((nod[j].next_id!=1000) && (nod[j].next_id==(i+1)))
     {
      continue;
     }
     
     
     if((nod[j].count<z) && (j!=i) && (distan[j]<temp_distan))    
     {
      flag = 1;       // 找到了一个合适的节点,置标志位,退出循环
      if(distan[j] < dMin)
      {
       dMin = distan[j];
          min = j;
      
     }
    }
    z++;
               
    
       
    if((flag==0) && (z==MAX))       
    {
     for(j=0; j<N; j++)
     {
      
         if((nod[j].next_id!=1000) && (nod[j].next_id==(i+1)))
      {
           continue;
      }
         if((distan[j]<dMin) && (j!=i) && (distan[j]<temp_distan))
      {
       dMin = distan[j];
          min = j;
      
     }
     break;
    }
     // while循环结束
   nod[min].count++;   // 该节点通过的路径数加1
   nod[i].next_id = min + 1;   // 置下一跳节点号
   nod[i].disoff = sqrt(pow((nod[min].position_x-p.x),2) + pow((nod[min].position_y-p.y),2));
   fprintf(fp, "next node is %d:\n", nod[i].next_id);    
   fprintf(fp, "\n");
  }//else结束
 } //for循环结束
}
void Print_RoutNum(Node *nod, FILE *fp)
{
 int i;
 int totalsum = 0;
 fprintf(fp, "\n节点经历的路径数: \n");
 for(i=0; i<N; i++)
 {
     fprintf(fp, "node %d : %d \n", nod[i].id, nod[i].count);
  totalsum += nod[i].count;
 }
 fprintf(fp, "\n");
 fprintf(fp, "总路径数: %d", totalsum);
}
void Compute_repeat(Node *nod, FILE *fp)
{
 int count = 0;
 int i;
 for(i=0; i<N; i++)
 {
  if(nod[i].next_id == nod[nod[i].next_id-1].next_id)
  {
   count++;
  }
 }
 fprintf(fp, "\n重复往返路径数: %d\n", count);
}
void Compute_hop_disoff(Node *nod, FILE *fp)
{
 int i;
 int flag;
 fprintf(fp, "\n各节点到目标节点的跳数及下一跳等分点偏移:\n");
 for(i=0; i<N; i++)
 {
  flag = nod[i].next_id;
  nod[i].hop = 1;
  while(flag != 0)
  {
   flag = nod[flag-1].next_id;
   nod[i].hop++;
   if(nod[i].hop == 50)   // 假定若跳数达到50时还未到目标节点,则与目标节点肯定无可能路径
   {
    nod[i].hop = 999;  // 置为一个很大的数
    break;             // 防止死循环
   }
  }
  if(nod[i].hop == 999)
  {
   fprintf(fp, "节点 %d: 没有可能路径!; 下一跳等分点偏移  %f\n", nod[i].id, nod[i].disoff);
  }
  else
  {
            fprintf(fp, "节点 %d: 跳数 %d;  下一跳等分点偏移  %f\n",
         nod[i].id, nod[i].hop, nod[i].disoff);
  }
 }
}
void Compute_totalhop(Node *nod, FILE *fp)
{
 int i;
 int totalhop = 0;
 for(i=0; i<N; i++)
 {
  if(nod[i].hop != 999)
  {
   totalhop += nod[i].hop;
  }
 }
 fprintf(fp, "\n总跳数: %d\n", totalhop);
}
void Compute_totaldisoff(Node *nod, FILE *fp)
{
 int i;
 double totaldisoff = 0;
 for(i=0; i<N; i++)
 {
  totaldisoff += nod[i].disoff;
 }
 fprintf(fp, "总偏移距离: %f\n", totaldisoff);
}

0

阅读 收藏 喜欢 打印举报/Report
  

新浪BLOG意见反馈留言板 欢迎批评指正

新浪简介 | About Sina | 广告服务 | 联系我们 | 招聘信息 | 网站律师 | SINA English | 产品答疑

新浪公司 版权所有