关于自动寻路简单算法

#include<stdio.h>
#include<math.h>
#include<stdlib.h>
#include<time.h>
#define MAPX 8  //this  is  the size  of the map  
#define MAPY 8  //this  is  the size  of the map  

typedef struct coord
{    int line_x;
    int line_y;
}COORD;// define struct  node

int printmap(int a[][MAPY],int x,int y);
COORD path(int map[MAPX][MAPY],int start_x,int start_y,int end_x,int end_y);
COORD pop_now();
void push(int x,int y);
int isline(COORD now,COORD next);
int isend(COORD now,int x,int y);
void roadblock(int [MAPX][MAPY]);


int sp = 0;      //stack  pointer
COORD line[100];// stack



 void main()
 {
     int map[MAPX][MAPY]={0},start_x,start_y,end_x,end_y,count=1,i;
     COORD point_line_now,point_line_next; 
     roadblock(map);//set  roadblock
     printmap(map,MAPX,MAPY);// print map
     printf("please input starting point x and y:"); 
     scanf("%d,%d",&start_x,&start_y); // input star node
     printf("please input ending point x and y:");
     scanf("%d,%d",&end_x,&end_y); //input end node

     push(start_x,start_y);//  push  star node

         while(1)
         { point_line_now =  path(map,pop_now().line_x,pop_now().line_y,end_x,end_y); //new node
          point_line_next = path(map,point_line_now.line_x,point_line_now.line_y,end_x,end_y);// new node  is next node
          
            if(isline(pop_now(),point_line_next)) // isline(): whether two nodes are the same parent. if yes push  next node  else push  new node
                push(point_line_next.line_x,point_line_next.line_y);
            else
                push(point_line_now.line_x,point_line_now.line_y);

            count++;             // count node numbers

           if(isend(pop_now(),end_x,end_y))// isend:judge node whether or not end
                break;
        }
   push(end_x,end_y);// push the last one 
   count++;  //  node numbers +1

     for(i=0;i<count;i++)
        map[line[i].line_x][line[i].line_y]=2;  //nodes in maps

     printf("路线：\n");
     printmap(map,MAPX,MAPY); //print  map
 }


void  roadblock(int map[][MAPY]) 
//roadblock:set roadblock
{ 
int i,n;
printf("input roadblock numbers:");
scanf("%d",&n);
  srand(time(NULL));
  for(i=0;i<n;i++)
    {
     map[rand()%8][rand()%7]=1;
    }
}




int isline(COORD now,COORD next)
{
   int i,j,k=0;
   for(i=-1;i<2;i++)
     for(j=-1;j<2;j++)
       if(now.line_x+i == next.line_x  && now.line_y+j == next.line_y)
            k=1; 
    return k;
}


int isend(COORD now,int x,int y)
{int i,j,k=0;
   for(i=-1;i<2;i++)
     for(j=-1;j<2;j++)
       if(x+i == now.line_x  && y+j == now.line_y)
            k=1; 
    return k;

}


COORD path(int map[MAPX][MAPY],int start_x,int start_y,int end_x,int end_y)
//Calculate the path
{
    double length;
    int i,j,k=0;
    int line_x,line_y;
    int temp=0;
    COORD point;
    for(i=-1;i<2;i++)
        for(j=-1;j<2;j++)
        {
            if(map[start_x+i][start_y+j] != 1 )//过滤障碍
              if(start_x+i >= 0) //过滤上边缘
               if(start_y+j >= 0) //过滤左边缘       
                if(start_x+i < MAPX)//过滤下边缘
                if(start_y+j < MAPY)//过滤右边缘
                     if(i!=0 ||j!=0)//过滤自己
                     {
                         if(temp == 0)
                             {length = sqrt(pow(start_x+i-end_x,2)+pow(start_y+j-end_y,2));
                            line_x = start_x+i ;
                            line_y = start_y+j ;
                            temp = 1;
                        
                            }
                         else
                             if(length > sqrt(pow(start_x+i-end_x,2)+pow(start_y+j-end_y,2)))
                             {length =sqrt(pow(start_x+i-end_x,2)+pow(start_y+j-end_y,2));
                            line_x = start_x+i ;
                            line_y = start_y+j ;                        
                             }
                     }
            
        }    
 point.line_x=line_x;
 point.line_y=line_y;

 return point;

}






COORD pop_now()
{
  COORD point;
   if(sp > 0)
      point = line[sp-1];
   else
      printf("error:stack empty!");    
   return point;
}



void push(int x,int y)
{ 

    if(sp>100)
        printf("error：stack full ! ");
    else
        {line[sp].line_x = x;
        line[sp].line_y = y;
        }
  sp++;
}


int printmap(int a[][MAPY],int x,int y)
{int i,j,m;
for(i=0;i<x;i++)
    {for(m=0;m<y;m++)
        printf("---");
    printf("\n");
 
    for(j=0;j<y;j++)
        if(a[i][j]==0)
            printf("%2c|",' ');
        else if(a[i][j]==1)
            printf("%2c|",'$');
        else
            printf("%2c|",'*');
    printf("\n");
    }
return 0;
}

 

 

 

转载于:https://www.cnblogs.com/chengtou/p/8983238.html