migongxunlu.c

实现迷宫自动寻路的一个机器人算法

//参看： http://c.chinaitlab.com/c/example/200902/777337.html

#include"time.h" 
#include "stdio.h" 
#include "graphics.h" 
#include "conio.h" 
#define null 0 

int a[11][15]={ {1,0,1,1,1,0,0,1,1,1,0,0,0,0,0},      /*定义迷宫*/ 
                {0,1,1,1,0,0,1,0,0,0,0,1,1,0,0},      /*0代表墙，1代表可以通行*/  
                {1,0,0,1,1,1,1,0,0,0,0,0,1,0,0}, 
                {0,0,1,0,0,0,0,1,0,0,0,1,0,1,1}, 
                {0,0,1,0,1,1,0,1,0,0,0,0,0,0,0}, 
                {1,1,0,0,1,0,0,0,1,0,1,1,0,0,0}, 
                {1,0,0,0,0,1,1,0,0,0,0,0,0,0,0}, 
                {1,1,0,0,1,0,0,1,0,0,0,0,0,1,0}, 
                {0,0,1,1,1,0,0,1,0,0,1,1,1,1,1}, 
                {1,1,0,0,0,0,0,1,1,1,0,0,0,0,1}, 
                {1,0,1,1,0,0,0,0,0,1,0,0,0,0,1} 
              }; 
struct moving      /*定义走动方向*/ 
{int x; 
int y; 
}move[9]={{0,0},{1,1},{0,1},{1,0},{-1,1},{1,-1},{-1,0},{0,-1},{-1,-1}}; 
typedef struct Position *PPosition; 
struct Position    /*定义结构体*/ 
{ 
  int x; 
  int y; 
  int direction; 
  PPosition link; 
}; 
struct LinkPosition    /*引入一层封装，其实就是好定义空栈罢了*/ 
{ 
  PPosition top; 
}; 
typedef struct LinkPosition *PLinkPosition; 

PLinkPosition createEmptyPosition(void)  /*建立空栈*/ 
{ 
  PLinkPosition plposition; 
  plposition=(PLinkPosition)malloc(sizeof(struct LinkPosition)); 
  plposition->top=null; 
  return(plposition); 
} 

void push(PLinkPosition plposition,int x,int y)    /*压栈*/ 
{ 
  PPosition p; 
  p=(PPosition)malloc(sizeof(struct Position)); 
  p->x=x; 
  p->y=y; 
  p->direction=1; 
  p->link=plposition->top; 
  plposition->top=p; 
} 

void pop(PLinkPosition plposition)  /*弹栈*/ 
{ 
  PPosition p; 
  p=plposition->top; 
  plposition->top=plposition->top->link; 
  free(p); 
} 

int judge(PLinkPosition plposition)  /*判断这个位置是不是栈中已有的*/ 
{int m,n; 
PPosition p; 
m=plposition->top->x; 
n=plposition->top->y; 
p=plposition->top->link; 
  while(p!=null) 
    {if(p->x!=m||p->y!=n)p=p->link; 
      else 
        {return(0); 
          break; 
        } 
    } 
return(1); 
} 

void maze(void)    /*画迷宫*/ 
{ 
int i,j; 
setbkcolor(1); 
setcolor(4); 
rectangle(90,50,540,380); 
for(i=80;i <380;i=i+30) 
  line(90,i,540,i); 
for(i=120;i <540;i=i+30) 
  line(i,50,i,380); 
for(i=0;i <11;i++) 
  {for(j=0;j <15;j++) 
      if(a[i][j]==1)floodfill(105+j*30,65+i*30,4); 
  }; 
} 

void erasermouse(PLinkPosition plposition)    /*擦除所走过的点*/ 
{ 

int x1,y1; 
x1=105+30*(plposition->top->y-1); 
y1=65+30*(plposition->top->x-1); 
setcolor(15); 
line(x1-10,y1,x1+10,y1); 
line(x1,y1-10,x1,y1+10); 
setcolor(4); 
} 

void mouse(PLinkPosition plposition)  /*画出当前位置*/ 
{ 
int x1,y1; 
x1=105+30*(plposition->top->y-1); 
y1=65+30*(plposition->top->x-1); 
line(x1-10,y1,x1+10,y1); 
line(x1,y1-10,x1,y1+10); 
} 

void TIMEDELAY()      /*时间延迟函数，这样动态实现时才可以看清*/ 
{ 
clock_t time; 
time=5+clock(); 
while(time>clock()); 
} 




main() 
{ 
PLinkPosition plposition,answer; 
PPosition p; 
int i,j,d,m,n,m1,n1,x1,y1,driver=VGA,mode=VGAHI; 
int c[13][17]={0};            
clock_t times; 
initgraph(&driver,&mode,""); 
for(i=1;i <12;i++) 
  for(j=1;j <16;j++)                            
    c[i][j]=a[i-1][j-1];            /*c数组就是给迷宫外层加了一层墙*/ 
plposition=createEmptyPosition(); 
plposition->top->link=null; 
push(plposition,1,1);                /*压入初始点*/ 
maze(); 
mouse(plposition); 
TIMEDELAY(); 
erasermouse(plposition); 
a:                                            /*主结构判断并找出路径*/ 
for(i=plposition->top->direction;i <=9;i++) 
{ 
  if(i==9) 
    {c[plposition->top->x][plposition->top->y]=0; 
    pop(plposition); 
    mouse(plposition); 
    TIMEDELAY(); 
    erasermouse(plposition); 
    goto a; 
    } 
  m=plposition->top->x; 
  n=plposition->top->y; 
  m1=m+move[i].x; 
  n1=n+move[i].y; 
  push(plposition,m1,n1); 
  if(c[m1][n1]==0) 
    {pop(plposition); 
      plposition->top->direction++; 
      continue; 
    } 
  if(judge(plposition)==0) 
    { 
      pop(plposition); 
      continue; 
    } 
  mouse(plposition); 
  TIMEDELAY(); 
  erasermouse(plposition); 
  if(m1==11&&n1==15)break; 
  goto a; 
} 
answer=createEmptyPosition();        /*由于栈中元素弹出时是从出口到入口的路径，所以把它们反过来*/ 
p=plposition->top; 
while(p!=null) 
    {m=p->x; 
    n=p->y; 
    push(answer,m,n); 
    p=p->link; 
    } 
p=answer->top; 
for(i=1;i <25;i++) 
printf("\n"); 

while(p->link!=null) 
    {printf("(%d,%d)-->",p->x,p->y);p=p->link;} 
printf("(%d,%d)",p->x,p->y); 

}

/////////////
栈方式： 

# define m2 50 
# define n2 50 
# define maxlen 200                /*栈长度*/ 
# define true 1 
# define false 0 
# define null 0 
# include "stdio.h" 
# include "graphics.h" 
# include "stdlib.h" 
# include "dos.h" 
int m,n; 

typedef struct                    
{ int x,y,dir;}elemtype; 
typedef struct 
{ elemtype stack[maxlen];        
  int top; 
}sqs; 
typedef struct                    

{ int dx,dy;}moved; 
void inimaze (int maze[][n2])    
{ int i,j; 
  for(i=1;i <=m;i++) 
    { 
      for(j=1;j <=n;j++)maze[i][j]=rand()/16383; 
    } 
for (i=0,j=0;i <=m+1;i++) 
    maze[i][j]=1; 
for (i=0,j=n+1;i <=m+1;i++) 
    maze[i][j]=1; 
for (i=0,j=0;j <=n+1;j++) 
    maze[i][j]=1; 
for (i=m+1,j=0;j <=n+1;j++) 
    maze[i][j]=1; 
} 
void picture (int maze[][n2])                    
{ 
  int i,j; 
  setbkcolor(BLACK); 
  for(i=0;i <m+2;i++) 
  { for (j=0;j <n+2;j++) 
    { if(maze[i][j]==1) 
  {  setfillstyle(1,LIGHTBLUE); 
    bar (70+j*20,20+i*20,88+j*20,38+i*20); 
  } 
else 
  {  setfillstyle(1,WHITE); 
    bar (70+j*20,20+i*20,88+j*20,38+i*20); 
        } 
    } 
  } 
outtextxy(90,460,"press any key to start"); 
getch(); 
} 

void inimove(moved move[])      

{ move[0].dx=0;move[0].dy=0; 
  move[1].dx=0;move[1].dy=1; 
  move[2].dx=1;move[2].dy=1; 
  move[3].dx=1;move[3].dy=0; 
  move[4].dx=1;move[4].dy=-1; 
  move[5].dx=0;move[5].dy=-1; 
  move[6].dx=-1;move[6].dy=-1; 
  move[7].dx=-1;move[7].dy=0; 
  move[8].dx=-1;move[8].dy=1; 
} 
void inistack(sqs *s)                

{ s->top=-1;} 

int push(sqs *s,elemtype t)          

{  int i,j; 
  if (s->top==maxlen-1)return(false); 
  else 
  { 
      i=t.x;j=t.y; 
      setfillstyle(1,GREEN); 
      bar (70+j*20,20+i*20,88+j*20,38+i*20); 
      s->stack[++s->top]=t; 
      return(true); 
  } 
} 
elemtype pop(sqs *s)            

{ elemtype elem; 
    if (s->top <0) 
    { 
      elem.x=null; 
      elem.y=null; 
      elem.dir=null; 
      return(elem); 
    } 
  else 
    { int i,j; 
      i=s->stack[s->top].x;j=s->stack[s->top].y; 
      setfillstyle(1,RED); 
      bar (70+j*20,20+i*20,88+j*20,38+i*20); 
      s->top--; 
      return(s->stack[s->top+1]); 
    } 
} 
void path(int maze[][n2],moved move[],sqs *s)    

{  int i,j,dir,x,y,f; 
  elemtype elem; 
  i=1;j=1;dir=0; 
  maze[1][1]=0;                              

do 
    {  x=i+move[dir].dx; 
      y=j+move[dir].dy; 
      if (maze[x][y]==0) 
            { elem.x=x;elem.y=y;elem.dir=dir; 
      f=push(s,elem); 
      delay(15000); 
              if (f==false) printf("栈长度太短"); 
      i=x;j=y;dir=0;maze[x][y]=-1; 
    } 
      else 
    {  if (dir <9) dir++;    

else 
  { 
      elem=pop(s); 
      if (elem.x!=null) 
      { 
      i=elem.x; 
      j=elem.y; 
      dir=elem.dir+1; 
      } 
                } 
}}while(!((s->top==-1)&&(dir>=7)||(x==m)&&(y==n)&&(maze[x][y]==-1)));  

if(s->top==-1) 
  printf("      !!!  no pass  !!!    "); 
  else { elem.x=x;elem.y=y;elem.dir=dir; 
        f=push(s,elem); 
setfillstyle(1,GREEN); 
bar (70+j*20,20+i*20,88+j*20,38+i*20); 
getch(); 
} 
} 
void start() 
{ int h; 
  for(h=5;h <=18;h++) 
  { 
    setfillstyle(1,h); 
    bar (80,80,520,180); 
    setcolor(h+2); 
    settextstyle(TRIPLEX_FONT,HORIZ_DIR,4); 
    outtextxy(150,90,"!WELCOME TO MAZE!"); 
    delay(15000); 
    } 
    for(h=0;h <13;h++)printf("\n"); 
    settextstyle(SMALL_FONT,HORIZ_DIR,6); 
    setcolor(LIGHTCYAN); 
    printf("\t\t\t\t\t\t");printf("    "); 
    outtextxy(90,200,"input the length of the maze(0-30):"); 
    scanf("%d",&m); 
    for(h=0;h <2;h++)printf("\n"); 
    printf("\t\t\t\t\t\t");printf("    "); 
    outtextxy(90,250,"input the wideth of the maze(0-30):"); 
    scanf("%d",&n); 
} 
void main() 
{ 
sqs *s; 
int maze[m2][n2]; 
moved move[8]; 
initgraph(VGA,VGAHI,""); 
start(); 
system("cls"); 
inimaze(maze); 
picture(maze); 
s=(sqs*)malloc(sizeof(sqs)); 
inistack(s); 
inimove(move); 
path(maze,move,s); 
}

/////////////////
递归： 

#include <stdlib.h> 
#include <stdio.h> 
#include <conio.h> 
#define N 18 
int aa[N][N];/*递归用的数组*/ 
int yes=0;/*判断是否找到路线的函数*/ 
int x[100][2],n=0;/*x数组是显示路线用的,n是它的下标,也就是走了几次*/ 
void fun1(int (*aa)[N],int (*a)[N]);/*重复赋值函数*/ 
int fun(int (*a)[N],int i,int j);/*递归找路算法函数*/ 
void begain(int (*t)[N]);/*开始的随机地图函数*/ 
void pr(int (*t)[N],int nn);/*输出地图函数*/ 
void win(int (*t)[N]);/*成功函数*/ 
void lose();/*失败函数*/ 
void main(void)/*主函数*/ 
{ 
int t[N][N]; 
begain(t);/*开始*/ 
pr(t,0);/*开始输入地图*/ 
fun(t,1,1);/*递归找路*/ 
if(yes) 
win(t);/*成功*/ 
else 
lose();/*失败*/ 
getch(); 
} 
void fun1(int (*aa)[N],int (*a)[N])/*为了不同方式的递归而循环8次*/ 
{ 
int i,j; 
for(i=0;i <N;i++) 
  for(j=0;j <N;j++) 
  aa[i][j]=a[i][j]; 
} 
int fun(int (*a)[N],int i,int j)/*递归找路*/ 
{ 
if(i==N-2&&j==N-2)/*达到目的了*/ 
  { 
  yes=1; 
  return; 
  } 
a[i][j]=1;/*走到的地方变为0*/ 
  fun1(aa,a); 
if(aa[i+1][j+1]==0&&!yes)/*右下,这里开始的8个if是8个方向的递归*/ 
{ 
fun(aa,i+1,j+1); 
  if(yes)/*如果到达目的了再把值给显示路线的数组*/ 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
/*这里开始的7个函数具体同上函数*/ 
  fun1(aa,a); 
if(aa[i+1][j]==0&&!yes)/*下边*/ 
{ 
fun(aa,i+1,j); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
  fun1(aa,a); 
if(aa[i][j+1]==0&&!yes)/*右边*/ 
{ 
fun(aa,i,j+1); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
fun1(aa,a); 
if(aa[i-1][j]==0&&!yes) 
{ 
  fun(aa,i-1,j); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
  fun1(aa,a); 
if(aa[i-1][j+1]==0&&!yes) 
{ 
fun(aa,i-1,j+1); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
  fun1(aa,a); 
if(aa[i+1][j-1]==0&&!yes) 
{ 
fun(aa,i+1,j-1); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
  fun1(aa,a); 
if(aa[i][j-1]==0&&!yes) 
{ 
fun(aa,i,j-1); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
  fun1(aa,a); 
if(aa[i-1][j-1]==0&&!yes) 
{ 
fun(aa,i-1,j-1); 
  if(yes) 
  {x[n][0]=i,x[n++][1]=j;return;} 
} 
} 
void begain(int (*t)[N])/*开始的随机地图*/ 
{ 
int i,j; 
system("cls"); 
randomize(); 
for(i=0;i <N;i++) 
{ 
  for(j=0;j <N;j++) 
  { 
    if(i==0||i==N-1||j==0||j==N-1) 
    t[i][j]=1; 
    else if(i==1&&j==1||i==N-2&&j==N-2) 
    t[i][j]=0; 
    else 
    t[i][j]=random(2); 
  } 
  } 
} 
void pr(int (*t)[N],int nn)/*输出地图*/ 
{ 
int i,j,ii; 
textcolor(RED); 
gotoxy(1,1); 
for(i=0;i <N;i++) 
  { 
  for(j=0;j <N;j++) 
  { 
  if(nn!=1)/*一开始的输出*/ 
    printf("%2d",t[i][j]); 
  else/*胜利后的输出*/ 
    { 
    for(ii=0;ii <n;ii++) 
    { 
    if(x[ii][0]==i&&x[ii][1]==j) 
      { 
      cprintf("%2d",t[i][j]); 
      break; 
      } 
    } 
    if(ii <n) 
      continue; 
    if(i==N-2&&j==N-2) 
      cprintf(" 0"); 
    else 
      printf("%2d",t[i][j]); 
    } 
  } 
  printf("\n"); 
  } 
} 
void win(int (*t)[N])/*找到路的话*/ 
{ 
int i,j,ii,jj; 
for(i=0;i <n-1;i++) 
  for(j=i+1;j <n;j++) 
  if(x[j][0]==x[i][0]&&x[j][1]==x[i][1]) 
    { 
    for(jj=j,ii=i;jj <n;jj++,ii++) 
      {x[ii][0]=x[jj][0];x[ii][1]=x[jj][1];} 
    n=n-(j-i); 
    } 
  printf("\nThe way is:\n"); 
  for(i=n-1;i>=0;i--)/*应该递归的情况所以应该是反过来输入路线*/ 
  printf("%3d%3d->",x[i][0],x[i][1]); 
  printf("%3d%3d\n",N-2,N-2); 
  t[1][1]=0; 
  pr(t,1); 
} 
void lose()/*没路的话*/ 
{ 
  printf("\nNot find way!\n"); 
}