calcdest.cc
上传用户:rrhhcc
上传日期:2015-12-11
资源大小:54129k
文件大小:17k
源码类别:

通讯编程

开发平台:

Visual C++

  1. extern "C" {
  2. #include <assert.h>
  3. #include <fcntl.h>
  4. #include <math.h>
  5. #include <stdio.h>
  6. #include <stdlib.h>
  7. #include <string.h>
  8. #include <sys/time.h>
  9. #include <sys/types.h>
  10. #include <sys/uio.h>
  11. #include <unistd.h>
  12. /* #include <err.h>*/
  13. };
  14. #include "../../../tools/rng.h"
  15. #include "setdest.h"
  16. //#define DEBUG
  17. #define SANITY_CHECKS
  18. //#define SHOW_SYMMETRIC_PAIRS
  19. #define GOD_FORMAT "$ns_ at %.12f "$god_ set-dist %d %d %d"n"
  20. #define GOD_FORMAT2 "$god_ set-dist %d %d %dn"
  21. #define NODE_FORMAT "$ns_ at %.12f "$node_(%d) setdest %.12f %.12f %.12f"n"
  22. #define NODE_FORMAT2 "$node_(%d) setdest %.12f %.12f %.12fn"
  23. #define NODE_FORMAT3 "$node_(%d) set %c_ %.12fn"
  24. #undef INFINITY
  25. #define INFINITY 0x00ffffff
  26. #define min(x,y) ((x) < (y) ? (x) : (y))
  27. #define max(x,y) ((x) > (y) ? (x) : (y))
  28. #define ROUND_ERROR 1e-9
  29. static int count = 0;
  30. /* ======================================================================
  31.    Function Prototypes
  32.    ====================================================================== */
  33. void usage(char**);
  34. void init(void);
  35. double uniform(void);
  36. void dumpall(void);
  37. void ComputeW(void);
  38. void floyd_warshall(void);
  39. void show_diffs(void);
  40. void show_routes(void);
  41. void show_counters(void);
  42. /* ======================================================================
  43.    Global Variables
  44.    ====================================================================== */
  45. double         RANGE = 250.0; // transmitter range in meters
  46. double TIME = 0.0; // my clock;
  47. double MAXTIME = 0.0; // duration of simulation
  48. double MAXX = 0.0;
  49. double MAXY = 0.0;
  50. double MAXSPEED = 0.0;
  51. double PAUSE = 0.0;
  52. u_int32_t NODES = 0;
  53. u_int32_t RouteChangeCount = 0;
  54. u_int32_t       LinkChangeCount = 0;
  55. u_int32_t DestUnreachableCount = 0;
  56. Node *NodeList = 0;
  57. u_int32_t *D1 = 0;
  58. u_int32_t *D2 = 0;
  59. FILE            *in_file;
  60. FILE            *out_file;
  61. /* ======================================================================
  62.    Random Number Generation
  63.    ====================================================================== */
  64. #define M 2147483647L
  65. #define INVERSE_M ((double)4.656612875e-10)
  66. char random_state[32];
  67. RNG *rng;
  68. double
  69. uniform()
  70. {
  71. count++;
  72. return rng->uniform_double();
  73. }
  74. /* ======================================================================
  75.    Misc Functions...
  76.    ====================================================================== */
  77. void
  78. usage(char **argv)
  79. {
  80. fprintf(stderr,
  81. "nusage: %st[-n <nodes>] n",
  82. argv[0]);
  83. fprintf(stderr,
  84. "tt[-t <simulation time>]n");
  85. fprintf(stderr,
  86. "tt-i <input_file> -o <output file>n");
  87. fprintf(stderr,
  88. "tt #nodes, max time, and range read from scenario file if possiblenn");
  89. }
  90. void
  91. init()
  92. {
  93. /*
  94.  * Initialized the Random Number Generation
  95.  */
  96. /*
  97.  * Allocate memory for globals
  98.  */
  99. NodeList = new Node[NODES];
  100. if(NodeList == 0) {
  101. perror("new");
  102. exit(1);
  103. }
  104. D1 = new u_int32_t[NODES * NODES];
  105. if(D1 == 0) {
  106. perror("new");
  107. exit(1);
  108. }
  109. memset(D1, 'xff', sizeof(u_int32_t) * NODES * NODES);
  110. D2 = new u_int32_t[NODES * NODES];
  111. if(D2 == 0) {
  112. perror("new");
  113. exit(1);
  114. }
  115. memset(D2, 'xff', sizeof(u_int32_t) * NODES * NODES);
  116. }
  117. void
  118. OpenAndReadHeader(char *in_filename, char *out_filename)
  119.   /* assumes all initialization comments are at top of file with no
  120.      no lines that start with other than '#' */
  121. {
  122.   char buf[256];
  123.   
  124.   in_file = fopen(in_filename,"r");
  125.   out_file = fopen(out_filename,"w");
  126.   
  127.   if (NULL == in_file) 
  128. fprintf(stderr, "*** can't open inputfile %s",in_filename);
  129.   if (NULL == out_file) 
  130. fprintf(stderr,"can't open outputfile %s",out_filename);
  131.   
  132.   while (!feof(in_file)) {
  133.     *buf = fgetc(in_file);
  134.     ungetc(*buf,in_file);
  135.     if (*buf != '#') break;
  136.     fgets(buf, sizeof(buf), in_file);
  137.     /* check to see if we need data from the line */
  138.     sscanf(buf, "# nodes: %d, max time: %lf", &NODES, &MAXTIME);
  139.     sscanf(buf, "# nominal range: %lf", &RANGE);
  140.     
  141.     fprintf(out_file, "%s", buf);
  142.   }
  143.   NODES += 1; // correct for 1-based indexs
  144.   fflush(out_file);
  145. }
  146. void
  147. ReadInMovementPattern()
  148. {
  149.   char buf[256];
  150.   u_int n;
  151.   double x,y,z,t,s;
  152.   struct setdest *setdest;
  153.   while (!feof(in_file)) {
  154.     fgets(buf, sizeof(buf), in_file);
  155.     fprintf(out_file, "%s", buf);
  156.     if (*buf == '#') continue;
  157.     if (*buf == 'n') continue;
  158.     /* check to see if we need data from the line */
  159.     if (2 == sscanf(buf,"$node_(%d) set Z_ %lf", &n, &z)) 
  160.       {
  161. assert(n < NODES);
  162. NodeList[n].position.Z = z;
  163.       }
  164.     else if (2 == sscanf(buf,"$node_(%d) set X_ %lf", &n, &x)) 
  165.       {
  166. assert(n < NODES);
  167. NodeList[n].position.X = x;
  168.       }
  169.     else if (2 == sscanf(buf,"$node_(%d) set Y_ %lf", &n, &y)) 
  170.       {
  171. assert(n < NODES);
  172. NodeList[n].position.Y = y;
  173.       }
  174.     else if (5 == sscanf(buf,"$ns_ at %lf "$node_(%d) setdest %lf %lf %lf"", 
  175. &t, &n, &x, &y, &s)) 
  176.       {
  177. assert(n < NODES);
  178. assert(t <= MAXTIME);
  179. setdest = (struct setdest *)malloc(sizeof(*setdest));
  180. assert(setdest);
  181. setdest->X = x; setdest->Y = y; setdest->Z = 0;
  182. setdest->time = t;
  183. setdest->speed = s;
  184. if (NodeList[n].traj.lh_first 
  185.     && t > NodeList[n].traj.lh_first->time)
  186.   {
  187.     printf("setdest's must be in anti-chronological order in input file!n");
  188.     printf("failed on node %dn",n);
  189.     exit(-1);
  190.   }
  191. LIST_INSERT_HEAD(&NodeList[n].traj,setdest,traj);
  192.       }
  193.     else 
  194.       {
  195. printf("unparsable line: '%s'", buf);
  196. continue;
  197.       }
  198.   }  
  199.   fflush(out_file);  
  200. }
  201. extern "C" char *optarg;
  202. int
  203. main(int argc, char **argv)
  204. {
  205. char ch;
  206. char *in_filename = NULL;
  207. char *out_filename = NULL;
  208. while ((ch = getopt(argc, argv, "n:t:i:o:")) != EOF) {       
  209. switch (ch) { 
  210. case 'n':
  211. NODES = atoi(optarg) + 1;
  212. break;
  213. case 't':
  214. MAXTIME = atof(optarg);
  215. break;
  216. case 'i':
  217. in_filename = optarg;
  218. break;
  219. case 'o':
  220. out_filename = optarg;
  221. break;
  222. default:
  223. usage(argv);
  224. exit(1);
  225. }
  226. }
  227. if (NULL == in_filename || NULL == out_filename) {
  228.         usage(argv);
  229. exit(1);
  230. }
  231. OpenAndReadHeader(in_filename, out_filename);
  232. if(NODES == 0 || MAXTIME == 0.0) {
  233. usage(argv);
  234. exit(1);
  235. }
  236. // A more portable solution for random number generation
  237.         rng = new RNG;
  238.         rng->set_seed(RNG::HEURISTIC_SEED_SOURCE); 
  239. init();
  240. ReadInMovementPattern();
  241. while(TIME <= MAXTIME) {
  242. double nexttime = 0.0;
  243. u_int32_t i;
  244. for(i = 1; i < NODES; i++) {
  245. NodeList[i].Update();
  246. }
  247. for(i = 1; i < NODES; i++) {
  248. NodeList[i].UpdateNeighbors();
  249. }
  250. for(i = 1; i < NODES; i++) {
  251. Node *n = &NodeList[i];
  252. if(n->time_transition > 0.0) {
  253. if(nexttime == 0.0)
  254. nexttime = n->time_transition;
  255. else
  256. nexttime = min(nexttime, n->time_transition);
  257. }
  258. if(n->time_arrival > 0.0) {
  259. if(nexttime == 0.0)
  260. nexttime = n->time_arrival;
  261. else
  262. nexttime = min(nexttime, n->time_arrival);
  263. }
  264. }
  265. floyd_warshall();
  266. #ifdef DEBUG
  267. show_routes();
  268. #endif
  269.   show_diffs();
  270. #ifdef DEBUG
  271. dumpall();
  272. #endif
  273. if (nexttime <= TIME + ROUND_ERROR) 
  274.   TIME = MAXTIME + 1;   /* we're done */
  275. else 
  276.   TIME = nexttime;
  277. #ifdef OLD
  278. assert(nexttime > TIME + ROUND_ERROR);
  279. TIME = nexttime;
  280. #endif
  281. }
  282. show_counters();
  283. int of;
  284. if ((of = open(".rand_state",O_WRONLY | O_TRUNC | O_CREAT, 0777)) < 0)
  285.   fprintf(stderr,"open rand state");
  286. for (unsigned int i = 0; i < sizeof(random_state); i++)
  287.   random_state[i] = 0xff & (int) (uniform() * 256);
  288. if (write(of,random_state, sizeof(random_state)) < 0)
  289.   fprintf(stderr,"writing rand state");
  290. close(of);
  291. }
  292. /* ======================================================================
  293.    Node Class Functions
  294.    ====================================================================== */
  295. u_int32_t Node::NodeIndex = 0;
  296. Node::Node()
  297. {
  298. u_int32_t i;
  299. index = NodeIndex++;
  300. if(index == 0)
  301. return;
  302. route_changes = 0;
  303.         link_changes = 0;
  304.         /*
  305.          * For the first PAUSE seconds of the simulation, all nodes
  306.          * are stationary.
  307.          */
  308. time_arrival = TIME;
  309. time_update = TIME;
  310. time_transition = 0.0;
  311. position.X = position.Y = position.Z = 0.0;
  312. destination.X = destination.Y = destination.Z = 0.0;
  313. direction.X = direction.Y = direction.Z = 0.0;
  314. speed = 0.0;
  315. neighbor = new Neighbor[NODES];
  316. if(neighbor == 0) {
  317. perror("new");
  318. exit(1);
  319. }
  320. LIST_INIT(&traj);
  321. for(i = 1; i < NODES; i++) {
  322. neighbor[i].index = i;
  323. neighbor[i].reachable = (index == i) ? 1 : 0;
  324. neighbor[i].time_transition = 0.0;
  325. }
  326. }
  327. void
  328. Node::RandomPosition()
  329. {
  330. position.X = uniform() * MAXX;
  331. position.Y = uniform() * MAXY;
  332. position.Z = 0.0;
  333. }
  334. void
  335. Node::RandomDestination()
  336. {
  337. destination.X = uniform() * MAXX;
  338. destination.Y = uniform() * MAXY;
  339. destination.Z = 0.0;
  340. assert(destination != position);
  341. }
  342. void
  343. Node::RandomSpeed()
  344. {
  345. speed = uniform() * MAXSPEED;
  346. assert(speed != 0.0);
  347. }
  348. void
  349. Node::Update()
  350. {
  351.         struct setdest *setdest = traj.lh_first;
  352. position += (speed * (TIME - time_update)) * direction;
  353. if(TIME == time_arrival) {
  354.   if (NULL == setdest) 
  355.     {
  356.       destination = position;
  357.       direction.X = direction.Y = direction.Z = 0.0;
  358.       speed = 0.0;
  359.       time_arrival = MAXTIME + 1;
  360.     } 
  361.   else 
  362.     {
  363.       vector v;
  364.       destination.X = setdest->X;
  365.       destination.Y = setdest->Y;
  366.       speed = setdest->speed;
  367.       if (0.0 == speed)
  368. { // it's a pause at the current location
  369.   if (LIST_NEXT(setdest,traj))
  370.     time_arrival = LIST_NEXT(setdest,traj)->time;
  371.   else
  372.     time_arrival = MAXTIME + 1;
  373. }
  374.       else 
  375. { // we're moving somewhere, when do we get there?
  376.   v = destination - position;
  377.   direction = v / v.length();
  378.   time_arrival = TIME + v.length() / speed;
  379. }
  380.       LIST_REMOVE(setdest,traj);
  381.       free(setdest);
  382.     }
  383. }
  384. time_update = TIME;
  385. time_transition = 0.0;
  386. }
  387. void
  388. Node::UpdateNeighbors()
  389. {
  390. static Node *n2;
  391. static Neighbor *m1, *m2;
  392. static vector D, B, v1, v2;
  393. static double a, b, c, t1, t2, Q;
  394. static u_int32_t i, reachable;
  395. v1 = speed * direction;
  396. /*
  397.  *  Only need to go from INDEX --> N for each one since links
  398.  *  are symmetric.
  399.  */
  400. for(i = index+1; i < NODES; i++) {
  401. m1 = &neighbor[i];
  402. n2 = &NodeList[i];
  403. m2 = &n2->neighbor[index];
  404. assert(i == m1->index);
  405. assert(m1->index == n2->index);
  406. assert(index == m2->index);
  407.                 assert(m1->reachable == m2->reachable);
  408.                 reachable = m1->reachable;
  409. /* ==================================================
  410.    Determine Reachability
  411.    ================================================== */
  412. { vector d = position - n2->position;
  413. if(d.length() < RANGE) {
  414. #ifdef SANITY_CHECKS
  415. if(TIME > 0.0 && m1->reachable == 0)
  416. assert(RANGE - d.length() < ROUND_ERROR);
  417. #endif
  418. m1->reachable = m2->reachable = 1;
  419. }
  420. // Boundary condition handled below.
  421. else {
  422. #ifdef SANITY_CHECKS
  423. if(TIME > 0.0 && m1->reachable == 1)
  424. assert(d.length() - RANGE < ROUND_ERROR);
  425. #endif
  426. m1->reachable = m2->reachable = 0;
  427. }
  428. #ifdef DEBUG
  429.                         fprintf(stdout, "# %.6f (%d, %d) %.2fmn",
  430.                                 TIME, index, m1->index, d.length());
  431. #endif
  432. }
  433. /* ==================================================
  434.    Determine Next Event Time
  435.    ================================================== */
  436. v2 = n2->speed * n2->direction;
  437. D = v2 - v1;
  438. B = n2->position - position;
  439. a = (D.X * D.X) + (D.Y * D.Y) + (D.Z * D.Z);
  440. b = 2 * ((D.X * B.X) + (D.Y * B.Y) + (D.Z * B.Z));
  441. c = (B.X * B.X) + (B.Y * B.Y) + (B.Z * B.Z) - (RANGE * RANGE);
  442. if(a == 0.0) {
  443. /*
  444.  *  No Finite Solution
  445.  */
  446. m1->time_transition= 0.0;
  447. m2->time_transition= 0.0;
  448. goto  next;
  449. }
  450. Q = b * b - 4 * a * c;
  451. if(Q < 0.0) {
  452. /*
  453.  *  No real roots.
  454.  */
  455. m1->time_transition = 0.0;
  456. m2->time_transition = 0.0;
  457. goto next;
  458. }
  459. Q = sqrt(Q);
  460. t1 = (-b + Q) / (2 * a);
  461. t2 = (-b - Q) / (2 * a);
  462. // Stupid Rounding/Boundary Cases
  463. if(t1 > 0.0 && t1 < ROUND_ERROR) t1 = 0.0;
  464. if(t1 < 0.0 && -t1 < ROUND_ERROR) t1 = 0.0;
  465. if(t2 > 0.0 && t2 < ROUND_ERROR) t2 = 0.0;
  466. if(t2 < 0.0 && -t2 < ROUND_ERROR) t2 = 0.0;
  467. if(t1 < 0.0 && t2 < 0.0) {
  468. /*
  469.  *  No "future" time solution.
  470.  */
  471. m1->time_transition = 0.0;
  472. m2->time_transition = 0.0;
  473. goto next;
  474. }
  475. /*
  476.  * Boundary conditions.
  477.  */
  478. if((t1 == 0.0 && t2 > 0.0) || (t2 == 0.0 && t1 > 0.0)) {
  479. m1->reachable = m2->reachable = 1;
  480. m1->time_transition = m2->time_transition = TIME + max(t1, t2);
  481. }
  482. else if((t1 == 0.0 && t2 < 0.0) || (t2 == 0.0 && t1 < 0.0)) {
  483. m1->reachable = m2->reachable = 0;
  484. m1->time_transition = m2->time_transition = 0.0;
  485. }
  486. /*
  487.  * Non-boundary conditions.
  488.  */
  489. else if(t1 > 0.0 && t2 > 0.0) {
  490. m1->time_transition = TIME + min(t1, t2);
  491. m2->time_transition = TIME + min(t1, t2);
  492. }
  493. else if(t1 > 0.0) {
  494. m1->time_transition = TIME + t1;
  495. m2->time_transition = TIME + t1;
  496. }
  497. else {
  498. m1->time_transition = TIME + t2;
  499. m2->time_transition = TIME + t2;
  500. }
  501. /* ==================================================
  502.    Update the transition times for both NODEs.
  503.    ================================================== */
  504. if(time_transition == 0.0 || (m1->time_transition &&
  505.    time_transition > m1->time_transition)) {
  506. time_transition = m1->time_transition;
  507. }
  508. if(n2->time_transition == 0.0 || (m2->time_transition &&
  509.    n2->time_transition > m2->time_transition)) {
  510. n2->time_transition = m2->time_transition;
  511. }
  512.         next:
  513.                 if(reachable != m1->reachable && TIME > 0.0) {
  514.                         LinkChangeCount++;
  515.                         link_changes++;
  516.                         n2->link_changes++;
  517.                 }
  518. }
  519. }
  520. void
  521. Node::Dump()
  522. {
  523. Neighbor *m;
  524. u_int32_t i;
  525. fprintf(stdout,
  526. "Node: %dtpos: (%.2f, %.2f, %.2f) dst: (%.2f, %.2f, %.2f)n",
  527. index, position.X, position.Y, position.Z,
  528. destination.X, destination.Y, destination.Z);
  529. fprintf(stdout, "tdir: (%.2f, %.2f, %.2f) speed: %.2fn",
  530. direction.X, direction.Y, direction.Z, speed);
  531. fprintf(stdout, "tArrival: %.2f, Update: %.2f, Transition: %.2fn",
  532. time_arrival, time_update, time_transition);
  533. for(i = 1; i < NODES; i++) {
  534. m = &neighbor[i];
  535. fprintf(stdout, "tNeighbor: %d (%lx), Reachable: %d, Transition Time: %.2fn",
  536. m->index, (long) m, m->reachable, m->time_transition);
  537. }
  538. }
  539. /* ======================================================================
  540.    Dijkstra's Shortest Path Algoritm
  541.    ====================================================================== */
  542. void dumpall()
  543. {
  544. u_int32_t i;
  545. fprintf(stdout, "nTime: %.2fn", TIME);
  546. for(i = 1; i < NODES; i++) {
  547. NodeList[i].Dump();
  548. }
  549. }
  550. void
  551. ComputeW()
  552. {
  553. u_int32_t i, j;
  554. u_int32_t *W = D2;
  555. memset(W, 'xff', sizeof(int) * NODES * NODES);
  556. for(i = 1; i < NODES; i++) {
  557. for(j = i; j < NODES; j++) {
  558. Neighbor *m = &NodeList[i].neighbor[j];
  559. if(i == j)
  560. W[i*NODES + j] = W[j*NODES + i] = 0;
  561. else
  562. W[i*NODES + j] = W[j*NODES + i] = m->reachable ? 1 : INFINITY;
  563. }
  564. }
  565. }
  566. void
  567. floyd_warshall()
  568. {
  569. u_int32_t i, j, k;
  570. ComputeW(); // the connectivity matrix
  571. for(i = 1; i < NODES; i++) {
  572. for(j = 1; j < NODES; j++) {
  573. for(k = 1; k < NODES; k++) {
  574. D2[j*NODES + k] = min(D2[j*NODES + k], D2[j*NODES + i] + D2[i*NODES + k]);
  575. }
  576. }
  577. }
  578. #ifdef SANITY_CHECKS
  579. for(i = 1; i < NODES; i++)
  580. for(j = 1; j < NODES; j++) {
  581. assert(D2[i*NODES + j] == D2[j*NODES + i]);
  582. assert(D2[i*NODES + j] <= INFINITY);
  583. }
  584. #endif
  585. }
  586. /*
  587.  *  Write the actual GOD entries to a TCL script.
  588.  */
  589. void
  590. show_diffs()
  591. {
  592. u_int32_t i, j;
  593. for(i = 1; i < NODES; i++) {
  594. for(j = i + 1; j < NODES; j++) {
  595. if(D1[i*NODES + j] != D2[i*NODES + j]) {
  596. if(D2[i*NODES + j] == INFINITY)
  597. DestUnreachableCount++;
  598.                                 if(TIME > 0.0) {
  599.                                         RouteChangeCount++;
  600.                                         NodeList[i].route_changes++;
  601.                                         NodeList[j].route_changes++;
  602.                                 }
  603. if(TIME == 0.0) {
  604. fprintf(out_file, GOD_FORMAT2,
  605. i, j, D2[i*NODES + j]);
  606. #ifdef SHOW_SYMMETRIC_PAIRS
  607. fprintf(out_file, GOD_FORMAT2,
  608. j, i, D2[j*NODES + i]);
  609. #endif
  610. }
  611. else {
  612. fprintf(out_file, GOD_FORMAT, 
  613. TIME, i, j, D2[i*NODES + j]);
  614. #ifdef SHOW_SYMMETRIC_PAIRS
  615. fprintf(out_file, GOD_FORMAT, 
  616. TIME, j, i, D2[j*NODES + i]);
  617. #endif
  618. }
  619. }
  620. }
  621. }
  622. memcpy(D1, D2, sizeof(int) * NODES * NODES);
  623. }
  624. void
  625. show_routes()
  626. {
  627. u_int32_t i, j;
  628. fprintf(stdout, "#n# TIME: %.12fn#n", TIME);
  629. for(i = 1; i < NODES; i++) {
  630. fprintf(stdout, "# %2d) ", i);
  631. for(j = 1; j < NODES; j++)
  632. fprintf(stdout, "%3d ", D2[i*NODES + j] & 0xff);
  633. fprintf(stdout, "n");
  634. }
  635. fprintf(stdout, "#n");
  636. }
  637. void
  638. show_counters()
  639. {
  640. u_int32_t i;
  641. fprintf(out_file, "#n# Destination Unreachables: %dn#n",
  642. DestUnreachableCount);
  643. fprintf(out_file, "# Route Changes: %dn#n", RouteChangeCount);
  644.         fprintf(out_file, "# Link Changes: %dn#n", LinkChangeCount);
  645.         fprintf(out_file, "# Node | Route Changes | Link Changesn");
  646. for(i = 1; i < NODES; i++)
  647. fprintf(out_file, "# %4d |          %4d |         %4dn",
  648.                         i, NodeList[i].route_changes,
  649.                         NodeList[i].link_changes);
  650. fprintf(out_file, "#n");
  651. }