API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Dec 29th, 2014
172
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 11.80 KB | None | 0 0
raw download clone embed print report
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <math.h>
  4. #include <sys/time.h>                // for gettimeofday()
  5.  
  6.  
  7. struct timeval t1,t2;
  8. int N, M, x1s, y1s, x2s, y2s, Mx, My,cost1,cost2,NODES;
  9. char Area[100][100];
  10. char Area2[100][100];
  11. int robot1[100][2];
  12. int robot2[100][2];
  13. int flag1;
  14.  
  15. typedef struct Node {
  16.     struct Node *parent;
  17.     int x1;
  18.     int y1;
  19.     double g;
  20.     double h;
  21.     double cost;
  22.     int expandable;
  23.     struct Node *child1;
  24.     struct Node *child2;
  25.     struct Node *child3;
  26.     struct Node *child4;
  27.     struct Node *nextfront;
  28.     struct Node *nextclosed;
  29. }SearchGraphNode;
  30.  
  31. SearchGraphNode *headclosed, *headfront, *new1, *q;
  32.  
  33. void displayfront()
  34. {
  35.     SearchGraphNode *temp = headfront;
  36.  
  37.     if ( headfront == NULL )
  38.         printf( "QUEUE IS EMPTY\n" );
  39.     else
  40.         {
  41.             printf( "QUEUE IS:\n" );
  42.  
  43.             while ( temp != NULL )
  44.                 {
  45.                     printf( "\t%f [%d,%d]",temp->cost,temp->x1,temp->y1);
  46.                     temp = temp->nextfront;
  47.                 }
  48.         }
  49.     printf("\n");
  50. }
  51.  
  52.  
  53.  
  54. void insertfront(SearchGraphNode *new)
  55. {
  56.     if ( headfront == NULL || new->cost <= headfront->cost )
  57.         {
  58.             new->nextfront = headfront;
  59.             headfront = new;
  60.         }
  61.     else
  62.     {
  63.         q = headfront;
  64.  
  65.             while ( q->nextfront!= NULL && q->nextfront->cost < new->cost )
  66.             q = q->nextfront;
  67.        
  68.         new->nextfront = q->nextfront;
  69.         q->nextfront = new;
  70.      } 
  71. }
  72.  
  73. void delfront()
  74. {
  75.     if ( headfront == NULL )
  76.         {
  77.             printf( "\nQUEUE UNDERFLOW\n" );
  78.         }
  79.     else
  80.         {
  81.             new1 = headfront;
  82.             headfront = headfront->nextfront;
  83.         }
  84. }
  85.  
  86.  
  87. void calc_heuristic(SearchGraphNode *node)
  88. {
  89.    int r1,r2;
  90.    double f;
  91.  
  92.    if(node->parent==NULL) node->g=0; else node->g = node->parent->g + 1;
  93.    
  94.    //ypologismos h
  95.    r1=node->x1;    
  96.    r2=node->y1;
  97.    
  98.    f = ((r1-Mx)*(r1-Mx)+(r2-My)*(r2-My));
  99.    f = sqrt(f);
  100.    node->h=f;
  101.    node->cost=node->g + node->h;
  102. }
  103.  
  104.  
  105.  
  106. int searchclosed(int item1,int item2)
  107. {
  108.   int n=1;
  109.   int boo=0;   
  110.   struct Node *node;
  111.   node=headclosed;
  112.   if (node==NULL) return 0;
  113.   else
  114.   {
  115.     while(node->nextclosed!=NULL)
  116.     {
  117.         if((node->x1==item1)&&(node->y1==item2))      
  118.            boo=1;  
  119.    
  120.     if(boo) return 1;  
  121.        
  122.     node=node->nextclosed;
  123.     }
  124.   return 0;
  125.   }
  126. }
  127.  
  128. void addBegclosed(int num1, int num2)  
  129. {  
  130.    struct Node *temp;  
  131.  
  132.    temp=(struct Node *)malloc(sizeof(struct Node));  
  133.    temp->x1 = num1;  
  134.    temp->y1 = num2;  
  135.    
  136.    if (headclosed == NULL)  
  137.    {  
  138.       //List is Empty  
  139.       headclosed=temp;  
  140.       headclosed->nextclosed=NULL;  
  141.    }  
  142.    else  
  143.    {  
  144.       temp->nextclosed=headclosed;  
  145.       headclosed=temp;  
  146.    }  
  147. }  
  148.  
  149. void expandup(SearchGraphNode *node)
  150. {  
  151.     int x=node->x1;
  152.     int y=(node->y1) -1;
  153.     if(y>0)
  154.     {        
  155.         if(Area[x-1][y-1]=='O')
  156.         {  
  157.             struct Node *temp;     
  158.             temp=(struct Node *)malloc(sizeof(struct Node));  
  159.             NODES++;
  160.             temp->parent=node;
  161.             temp->x1=x;
  162.             temp->y1=y;
  163.             calc_heuristic(temp);
  164.             node->child1=temp;
  165.             insertfront(temp);
  166.         }
  167.     }
  168. }
  169.  
  170. void expanddown(SearchGraphNode *node)
  171. {  
  172.     int x=node->x1;
  173.     int y=(node->y1) + 1;
  174.     if(y<N)
  175.     {  
  176.         if((Area[x-1][y-1]=='O'))
  177.         {  
  178.             struct Node *temp1;    
  179.             temp1=(struct Node *)malloc(sizeof(struct Node));  
  180.             temp1->parent=node;        
  181.             NODES++;
  182.             temp1->x1=x;
  183.             temp1->y1=y;
  184.             calc_heuristic(temp1);         
  185.             node->child2=temp1;    
  186.             insertfront(temp1);
  187.         }
  188.     }
  189. }
  190. void expandright(SearchGraphNode *node)
  191. {
  192.     int x=(node->x1) + 1;  
  193.     int y=node->y1;        
  194.     if(x<=M)
  195.     {  
  196.         if(Area[x-1][y-1]=='O')
  197.         {  
  198.             struct Node *temp2;    
  199.             temp2=(struct Node *)malloc(sizeof(struct Node));  
  200.             temp2->parent=node;                
  201.             NODES++;
  202.             temp2->x1=x;
  203.             temp2->y1=y;
  204.             node->child3=temp2;    
  205.             calc_heuristic(temp2);
  206.             insertfront(temp2);
  207.         }
  208.         }
  209. }
  210. void expandleft(SearchGraphNode *node)
  211. {
  212.  
  213.     int x=(node->x1)-1;
  214.     int y=node->y1;
  215.     if(x>0)
  216.     {  
  217.         if(Area[x-1][y-1]=='O')
  218.         {  
  219.             struct Node *temp3;    
  220.             temp3=(struct Node *)malloc(sizeof(struct Node));  
  221.             temp3->parent=node;        
  222.             NODES++;
  223.             temp3->x1=x;
  224.             temp3->y1=y;
  225.             node->child4=temp3;    
  226.             calc_heuristic(temp3);     
  227.             insertfront(temp3);
  228.         }
  229.     }
  230. }
  231.  
  232. int checkiffinal(int f1x,int f1y,SearchGraphNode *node)
  233. {
  234.     int x=node->x1;
  235.     int y=node->y1;
  236.     int dif=abs(x-f1x)+abs(y-f1y);
  237.     if(dif<=1) return 1;
  238.     else
  239.     return 0;  
  240. }
  241.  
  242. void printtrack1(SearchGraphNode *node)
  243. {
  244.   int n=0;
  245.   cost1=node->g;
  246.   SearchGraphNode *temp;
  247.   temp=node;
  248.   n=cost1-1;   
  249.   while(temp->parent!=NULL)
  250.   {
  251.     robot1[n][0]=temp->x1;
  252.     robot1[n][1]=temp->y1;
  253.         temp=temp->parent;
  254.         n--;
  255.   }    
  256.   int i;
  257. }
  258.  
  259. void printtrack2(SearchGraphNode *node)
  260. {
  261.   int n=0;
  262.   cost2=node->g;
  263.   SearchGraphNode *temp;
  264.   temp=node;
  265.   n=cost2-1;
  266.   while(temp->parent!=NULL)
  267.   {
  268.     robot2[n][0]=temp->x1;
  269.     robot2[n][1]=temp->y1;
  270.         temp=temp->parent;
  271.         n--;
  272.   }    
  273.   int i;
  274. }
  275.  
  276.  
  277. void Astar()
  278. {
  279.     headclosed=NULL;
  280.     headfront=NULL;
  281.     SearchGraphNode *current;
  282.     current=(struct Node *)malloc(sizeof(struct Node));  
  283.     NODES++;
  284.     current->x1=x1s;
  285.     current->y1=y1s;
  286.     current->parent=NULL;
  287.     calc_heuristic(current);
  288.     insertfront(current);
  289.  
  290.  
  291.     while(!checkiffinal(Mx,My,current))
  292.     {
  293.         delfront();
  294.        
  295.         if(!searchclosed(current->x1,current->y1))     
  296.         {  
  297.             expandup(current);
  298.             expanddown(current);
  299.             expandleft(current);
  300.             expandright(current);
  301.             addBegclosed(current->x1,current->y1); 
  302.         }      
  303.  
  304.         if (headfront==NULL) break; else current=headfront;
  305.        
  306.     }
  307.  
  308.     if (flag1 == 1) {
  309.         printtrack1(current);
  310.     }
  311.  
  312.     if (flag1 == 2)
  313.     {
  314.         printtrack2(current);
  315.  
  316.     }
  317. }
  318.  
  319.  
  320. main()
  321. {
  322. FILE *fp=fopen("inp.txt","r");
  323. int i, j;
  324. long long elapsedTime;
  325.  
  326.  
  327. //ReadinRobot A moves from [%d,%d] to [%d,%d]\n",robot1[i-1][0],robot1[i-1][1],robot1[i][0],robot1[i][1]);  g the input
  328. fscanf(fp,"%d",&N);
  329. fscanf(fp,"%d\n",&M);
  330. fscanf(fp,"%d",&x1s);
  331. fscanf(fp,"%d\n",&y1s);
  332. fscanf(fp,"%d",&x2s);
  333. fscanf(fp,"%d\n",&y2s);
  334. fscanf(fp,"%d",&Mx);
  335. fscanf(fp,"%d\n",&My);
  336.  
  337. for(i=0;i<M;i++)
  338. {
  339.   for(j=0;j<N;j++)
  340.   {
  341.     fscanf(fp,"%c",&Area[i][j]);
  342.     Area2[i][j]=Area[i][j];
  343.   }
  344.  
  345.  fscanf(fp,"\n");
  346. }
  347.  
  348. t2.tv_sec = t1.tv_sec = t2.tv_usec = t1.tv_usec = 0.0;
  349. // start timer
  350. gettimeofday(&t1, NULL);
  351.  
  352. Area2[x1s-1][y1s-1]='A';
  353. Area2[x2s-1][y2s-1]='B';
  354.  
  355. //Main Code
  356.  
  357. NODES=0;
  358. flag1=1;
  359. Astar();
  360. int temp1,temp2;
  361. temp1=x1s;
  362. temp2=y1s;
  363. x1s=x2s;
  364. y1s=y2s;
  365. flag1=2;
  366. Astar();
  367. x1s=temp1;
  368. y1s=temp2;
  369.  
  370. int min;
  371. if (cost1<cost2) min=cost1; else min=cost2;
  372. int counter=0;
  373. //Main algorithm of checking points of conflict with the other robot
  374.  
  375.  
  376. if((robot1[0][0]!=x2s)||(robot1[0][1]!=y2s)){
  377.     printf("Robot A moves from [%d,%d] to [%d,%d]\n",x1s,y1s,robot1[0][0],robot1[0][1]);   
  378.     Area2[robot1[0][0]-1][robot1[0][1]-1]='A';
  379. }
  380. else
  381.  {
  382.     printf("Robot A understands that he will collide with Robot B in [%d,%d], so he changes direction\n",x2s,y2s);
  383.     Area[x2s-1][y2s-1]='X';
  384.         flag1=1;
  385.         Astar();
  386.     Area[x2s-1][y2s-1]='O';
  387.         printf("Robot A moves from [%d,%d] to [%d,%d]\n",x1s,y1s,robot1[0][0],robot1[0][1]);   
  388.     Area2[robot1[0][0]-1][robot1[0][1]-1]='A';
  389. }
  390. counter++;
  391. if((robot1[0][0]!=robot2[0][0])||(robot1[0][1]!=robot2[0][1])){
  392.     printf("Robot B moves from [%d,%d] to [%d,%d]\n",x2s,y2s,robot2[0][0],robot2[0][1]);
  393.     Area2[robot2[0][0]-1][robot2[0][1]-1]='B';   
  394. }
  395. else
  396.     {
  397.     printf("Robot B understands that he will collide with Robot A in [%d,%d], so he changes direction\n",robot2[0][0],robot2[0][1]);
  398.         Area[robot2[0][0]-1][robot2[0][1]-1]='X';
  399.         temp1=x1s;
  400.     temp2=y1s;
  401.     x1s=x2s;
  402.     y1s=y2s;
  403.     flag1=2;
  404.     Astar();
  405.     x1s=temp1;
  406.     y1s=temp2;
  407.     Area[robot2[0][0]-1][robot2[0][1]-1]='O';
  408.     printf("Robot B moves from [%d,%d] to [%d,%d]\n",x2s,y2s,robot2[0][0],robot2[0][1]);
  409.         Area2[robot2[0][0]-1][robot2[0][1]-1]='B';   
  410.     }
  411. counter++;  
  412. for(i=1;i<min;i++)
  413. {
  414. if((robot1[i][0]!=robot2[i-1][0])||(robot1[i][1]!=robot2[i-1][0])){
  415.  printf("Robot A moves from [%d,%d] to [%d,%d]\n",robot1[i-1][0],robot1[i-1][1],robot1[i][0],robot1[i][1]);
  416.  Area2[robot1[i][0]-1][robot1[i][1]-1]='A';  
  417. }
  418. else
  419.  {
  420.     printf("Robot A understands that he will collide with Robot B in [%d,%d], so he changes direction\n",robot1[i][0],robot1[i][0]);
  421.     Area[robot1[i][0]-1][robot1[i][1]-1]='X';
  422.         flag1=1;
  423.         Astar();
  424.     Area[robot1[i][0]-1][robot1[i][0]-1]='O';
  425.         printf("Robot A moves from [%d,%d] to [%d,%d]\n",robot1[i-1][0],robot1[i-1][1],robot1[i][0],robot1[i][1]); 
  426.     Area2[robot1[i][0]-1][robot1[i][1]-1]='A';   
  427.  }
  428. counter++;
  429. if((robot1[i][0]!=robot2[i][0])||(robot1[i][1]!=robot2[i][1])){
  430.     printf("Robot B moves from [%d,%d] to [%d,%d]\n",robot2[i-1][0],robot2[i-1][1],robot2[i][0],robot2[i][1]);
  431.     Area2[robot2[i][0]-1][robot2[i][1]-1]='B';       
  432. }
  433. else
  434.     {
  435.     printf("Robot B understands that he will collide with Robot A in [%d,%d], so he changes direction\n",robot2[i][0],robot2[i][1]);
  436.         Area[robot2[i][0]-1][robot2[i][1]-1]='X';
  437.         temp1=x1s;
  438.     temp2=y1s;
  439.     x1s=x2s;
  440.     y1s=y2s;
  441.     flag1=2;
  442.     Astar();
  443.     x1s=temp1;
  444.     y1s=temp2;
  445.     Area[robot2[i][0]-1][robot2[i][1]-1]='O';
  446.     printf("Robot B moves from [%d,%d] to [%d,%d]\n",robot2[i-1][0],robot2[i-1][1],robot2[i][0],robot2[i][1]);
  447.         Area2[robot2[i][0]-1][robot2[i][1]-1]='B';       
  448.     }
  449. counter++;
  450. }
  451.  
  452. if(min==cost1){
  453.     printf("Robot A moves from [%d,%d] to [%d,%d] and reaches meeting point\n",robot1[min-1][0],robot1[min-1][1],Mx,My);
  454.     Area2[Mx-1][My-1]='A';         
  455.     counter++;
  456.     for(i=min;i<cost2-1;i++){
  457.     printf("Robot B moves from [%d,%d] to [%d,%d] \n",robot2[i-1][0],robot2[i-1][1],robot2[i][0],robot2[i][1]);
  458.     Area2[robot2[i][0]-1][robot2[i][1]-1]='B';         
  459.     counter++;
  460.     }
  461.     printf("Robot B moves from [%d,%d] to [%d,%d] and reaches meeting point\n",robot2[cost2-2][0],robot2[cost2-2][1],robot2[cost2-1][0],robot2[cost2-1][1]);
  462. counter++;
  463. Area2[robot2[cost2-1][0]-1][robot2[cost2-1][1]-1]='B';       
  464. }
  465. else
  466.   {
  467.     printf("Robot A moves from [%d,%d] to [%d,%d]\n",robot1[min-1][0],robot1[min-1][1],robot1[min][0],robot1[min][1]);
  468.     Area2[robot1[min][0]-1][robot1[min][1]-1]='A';         
  469.     counter++;
  470.     printf("Robot B moves from [%d,%d] to [%d,%d] and reaches meeting point\n",robot2[min-1][0],robot2[min-1][1],Mx,My);   
  471.     Area2[robot2[min-1][0]-1][robot2[min-1][1]-1]='B';         
  472.     counter++;
  473.     for(i=min+1;i<cost1-1;i++){
  474.     printf("Robot A moves from [%d,%d] to [%d,%d] \n",robot1[i-1][0],robot1[i-1][1],robot1[i][0],robot1[i][0]);
  475.     counter++;
  476.     Area2[robot1[i][0]-1][robot1[i][1]-1]='A';         
  477.     }  
  478.     printf("Robot A moves from [%d,%d] to [%d,%d] and reaches meeting point\n",robot1[cost1-2][0],robot1[cost1-2][1],robot1[cost1-1][0],robot1[cost1-1][0]);
  479.     Area2[robot1[cost1-1][0]-1][robot1[cost1-1][1]-1]='A';             
  480.     counter++;     
  481.   }
  482.  
  483. // stop timer
  484. gettimeofday(&t2, NULL);
  485. elapsedTime = 0.0;
  486.  
  487. // compute and print the elapsed time in millisec
  488. elapsedTime = (t2.tv_sec - t1.tv_sec) * 1000000.0;      // sec to us
  489. elapsedTime += (t2.tv_usec - t1.tv_usec) ;   // us
  490. printf("--------------------------------\n");
  491. printf("Time of execution = %lld us\n",elapsedTime);  
  492.  
  493. printf("--------------------------------\n");
  494. printf("A SOLUTION WAS FOUND IN %d STEPS\n",counter);
  495. printf("--------------------------------\n");
  496. printf("RobotA starting point [%d,%d],Robot B starting point [%d,%d] and final meeting point [%d,%d]\n",x1s,y1s,x2s,y2s,Mx,My);
  497. printf("--------------------------------\n");
  498. printf(" THE FIRST MAP \n\n");
  499.  
  500. for(i=0;i<M;i++)
  501. {
  502.   for(j=0;j<N;j++)
  503.   {
  504.     printf("%c",Area[i][j]);
  505.   }
  506. printf("\n");
  507. }
  508.  
  509. printf("--------------------------------\n");
  510. printf(" THE FINAL MAP \n\n");
  511.  
  512. for(i=0;i<M;i++)
  513. {
  514.   for(j=0;j<N;j++)
  515.   {
  516.     printf("%c",Area2[i][j]);
  517.   }
  518. printf("\n");
  519. }
  520.  
  521. printf("--------------------------------\n");
  522. printf("TOTAL NUMBER OF NODES PRODUCED: %d\n",NODES);
  523.  
  524. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!