#include<stdio.h> 
#include<stdlib.h> 
#define N 32 
#define W 10 
 
typedef struct STARNODE{ 
 int x;//節(jié)點的x,y坐標 
 int y; 
 int G;//該節(jié)點的G, H值 
 int H; 
 int is_snakebody;//是否為蛇身，是為1，否則為0； 
 int in_open_table;//是否在open_table中，是為1，否則為0； 
 int in_close_table;//是否在close_table中，是為1，否則為0； 
 struct STARNODE* ParentNode;//該節(jié)點的父節(jié)點 
} starnode, *pstarnode; 
 
starnode mapnode[N/2+2][N+4]; 
 
pstarnode opentable[N*N/2]; 
pstarnode closetable[N*N/2]; 
 
int opennode_count=0; 
int closenode_count=0; 
starnode food; 
 
//根據(jù)指針所指向的節(jié)點的F值，按大頂堆進行調(diào)整 
void heapadjust(pstarnode a[], int m, int n) 
{ 
 int i; 
 pstarnode temp=a[m]; 
 for(i=2*m;i<=n;i*=2) 
 { 
  if(i+1<=n && (a[i+1]->G+a[i+1]->H)>(a[i]->G+a[i]->H) ) 
  { 
   i++; 
  } 
  if((temp->G+temp->H)>(a[i]->G+a[i]->H)) 
  { 
   break; 
  } 
  a[m]=a[i]; 
  m=i; 
 } 
 a[m]=temp; 
} 
 
void swap(pstarnode a[],int m, int n) 
{ 
 pstarnode temp; 
 temp=a[m]; 
 a[m]=a[n]; 
 a[n]=temp; 
} 
 
 
void crtheap(pstarnode a[], int n) 
{ 
 int i; 
 for(i=n/2;i>0;i--) 
 { 
  heapadjust(a, i, n); 
 } 
} 
 
void heapsort(pstarnode a[], int n) 
{ 
 int i; 
 crtheap(a,n); 
 for(i=n;i>1;i--) 
 { 
  swap(a,1,i); 
  heapadjust(a, 1,i-1); 
 } 
} 
 
//x1, y1是鄰域點坐標 
//curtnode是當前點坐標 
void insert_opentable(int x1, int y1, pstarnode pcurtnode) 
{ 
 int i; 
 if(!mapnode[x1][y1].is_snakebody && !mapnode[x1][y1].in_close_table)//如果不是蛇身也不在closetable中 
 { 
  if(mapnode[x1][y1].in_open_table && mapnode[x1][y1].G>pcurtnode->G+W)//如果已經(jīng)在opentable中,但是不是最優(yōu)路徑 
  { 
   mapnode[x1][y1].G=pcurtnode->G+W;//把G值更新 
   mapnode[x1][y1].ParentNode=pcurtnode;//把該鄰點的雙親節(jié)點更新 
   //由于改變了opentable中一個點的F值，需要對opentable中的點的順序進行調(diào)整，以滿足有序 
   for(i=1;i<=opennode_count;i++) 
   { 
    if(opentable[i]->x==x1 && opentable[i]->y==y1) 
    { 
     break; 
    } 
    heapsort(opentable, i); 
   } 
  } 
  else//把該點加入opentable中 
  { 
   opentable[++opennode_count]=&mapnode[x1][y1]; 
 
   mapnode[x1][y1].G=pcurtnode->G+W; 
   mapnode[x1][y1].H=(abs(food.x-x1)+abs(food.y-y1))*W; 
   mapnode[x1][y1].in_open_table=1; 
   mapnode[x1][y1].ParentNode=pcurtnode; 
   heapsort(opentable, opennode_count); 
  } 
 } 
} 
 
//尋找當前點的四鄰域點，把符合條件的點加入opentable中 
void find_neighbor(pstarnode pcurtnode) 
{ 
 int x=pcurtnode->x; 
 int y=pcurtnode->y; 
 
 if(x+1<=N/2) 
 { 
  insert_opentable(x+1, y, pcurtnode); 
 } 
 if(x-1>=1) 
 { 
  insert_opentable(x-1, y, pcurtnode); 
 } 
 if(y+1<=N+1) 
 { 
  insert_opentable(x,y+1, pcurtnode); 
 } 
 if(y-1>=2) 
 { 
  insert_opentable(x,y-1, pcurtnode); 
 } 
} 
 
int search_road(pstarnode startnode, pstarnode endnode) 
{ 
 int is_search_road=0; 
 opennode_count=0; 
 closenode_count=0; 
 pstarnode pcurtnode; 
 
 opentable[++opennode_count]=startnode;//起始點加入opentable中 
 startnode->in_open_table=1; 
 startnode->ParentNode=NULL; 
 startnode->G=0; 
 startnode->H=(abs(endnode->x-startnode->x)+abs(endnode->y-startnode->y))*W; 
 
 if(startnode->x==endnode->x && startnode->y==endnode->y)//如果起點和終點重合 
 { 
  is_search_road=1; 
  return is_search_road; 
 } 
 
 while(1) 
 { 
  //取出opentable中第1個節(jié)點加入closetable中 
  pcurtnode=opentable[1]; 
  opentable[1]=opentable[opennode_count--]; 
 
  closetable[++closenode_count]=pcurtnode; 
  pcurtnode->in_open_table=0; 
  pcurtnode->in_close_table=1; 
 
  if(pcurtnode->x==endnode->x && pcurtnode->y==endnode->y) 
  { 
   is_search_road=1; 
   break; 
  } 
 
  find_neighbor(pcurtnode); 
 
  if(!opennode_count)//如果opentable已經(jīng)為空，即沒有找到路徑 
  { 
   break; 
  } 
 } 
 
 return is_search_road; 
} 
 
int main(void) 
{ 
 int i, j; 
 pstarnode startnode; 
 
 for(i=0;i<N/2+2;i++) 
  for(j=0;j<N+4;j++) 
  { 
   mapnode[i][j].G=0; 
   mapnode[i][j].H=0; 
   mapnode[i][j].in_close_table=0; 
   mapnode[i][j].in_open_table=0; 
   mapnode[i][j].is_snakebody=0; 
   mapnode[i][j].ParentNode=NULL; 
   mapnode[i][j].x=i; 
   mapnode[i][j].y=j; 
  } 
 
 startnode=&mapnode[5][10]; 
 food.x=5; 
 food.y=15; 
 mapnode[5][13].is_snakebody=1; 
 mapnode[6][13].is_snakebody=1; 
 mapnode[4][13].is_snakebody=1; 
 mapnode[4][12].is_snakebody=1; 
 mapnode[6][12].is_snakebody=1; 
 
 int flag; 
 flag=search_road(startnode, &food); 
 pstarnode temp=&mapnode[5][15]; 
 
 do{ 
  printf("%d %d\n",temp->x, temp->y); 
  temp=temp->ParentNode; 
 }while(temp); 
 
 return 0; 
}