最短路径搜索A*算法

#include<stdio.h> #include<math.h> #include"Queue.h" //#include"ANode.h" #define MAXSIZE 100 //地图最大边长 #define STEP 5 //步长，代价为1 int MAP_H,MAP_W;//地图实际大小 char map[MAXSIZE][MAXSIZE]; //ANode nodes[MAXSIZE][MAXSIZE]; //int start_x,start_y,end_x,end_y; Queue open,closed; ANode root,start_Anode,end_Anode; ANode *path[MAXSIZE]; unsigned int index=0; //QNode start_Qnode; unsigned int HVALUE(ANode *node) { node->h=(node->x-end_Anode.x)*(node->x-end_Anode.x)+(node->y-end_Anode.y)*(node->y-end_Anode.y); // node->h=fabs(node->x-end_Anode.x)+fabs(node->y-end_Anode.y); return node->h; } /*bool tryLeft(ANode *curNode) { if(curNode->x!=0 && map[curNode->y][curNode->x-1]!='o' && !(curNode->y=curNode->parent->y && curNode->x-1==curNode->parent->x)) return true; else return false; } bool tryRight(ANode *curNode) { if(curNode->x!=(MAP_W-1) &&map[curNode->y][curNode->x+1]!='o' && !(curNode->y==curNode->parent->y && curNode->x+1==curNode->parent->x)) return true; else return false; } bool tryUp(ANode *curNode) { if(curNode->y!=0 &&map[curNode->y-1][curNode->x]!='o' && !(curNode->x==curNode->parent->x && curNode->y-1==curNode->parent->y)) return true; else return false; } bool tryDown(ANode *curNode) { if(curNode->y!=(MAP_H-1) &&map[curNode->y+1][curNode->x]!='o' && !(curNode->x==curNode->parent->x && curNode->y+1==curNode->parent->y)) return true; else return false; }*/ void readMap(char *path); void init(); void makePath(ANode *oldNode); /*ANode* bestNeighbour(ANode *node,unsigned int dire) { ANode *curNode=(ANode*)(malloc(sizeof(ANode))); // ANode *neighbour[4]; switch(dire){ case 0://向上 if(node->y==0)//该节点已经是最上面的了 return NULL; if((node->x==node->parent->x) && (curNode->y-1==node->parent->y))//如果curNode是node的父亲 return NULL; curNode->x=node->x; curNode->y=node->y-1; curNode->ch=map[curNode->y][curNode->x]; curNode->g=node->g+STEP; curNode->h=HVALUE(curNode); curNode->f=curNode->g+curNode->h; curNode->parent=node; return curNode; break; case 1://向下 if(node->y==MAP_H-1)//该节点已经在地图的最下面 break; case 2://向左 if(node->x==0)//该节点已经在地图的最左边了 break; case 3://向右 if(node->x==MAP_W-1)//该节点已经在地图的最右边了 break; default: break; } //neighbour[i]= }*/ void showNode(ANode *node) { printf("x=%d,y=%d,ch=%c,g=%d,h=%d,f=%d\n",node->x,node->y,node->ch,node->g,node->h,node->f); //showNode(node.parent); } void main() { readMap("map2.txt"); init(); while(!IsQEmpty(open))//open表非空 { ANode *bestNode=(ANode*)(malloc(sizeof(ANode))); *bestNode=DeQueue(open); // showNode(bestNode); if(bestNode->x==end_Anode.x && bestNode->y==end_Anode.y)//当与目标点相等时，即到达目标点时 { printf("路径已经找到！\n"); //end_Anode.parent=bestNode; ANode *resu=bestNode; while(resu->parent->x!=-1 && resu->parent->y!=-1) { showNode(resu); //printf("%d",&resu); resu=resu->parent; } break; } ANode *curNode=NULL; curNode=(ANode*)(malloc(sizeof(ANode))); if(!curNode) { printf("新节点内存分配失败，退出！\n"); exit(1); } if(!((bestNode->x+1==bestNode->parent->x) && (bestNode->y==bestNode->parent->y))&&map[bestNode->y][bestNode->x+1]!='o'&&bestNode->x!=(MAP_W-1)) {//向右扩展 curNode->x=bestNode->x+1; curNode->y=bestNode->y; curNode->ch=map[curNode->y][curNode->x]; curNode->g=bestNode->g+STEP; curNode->h=HVALUE(curNode); curNode->f=curNode->g+curNode->h; curNode->parent=bestNode; QNode *p=IsHave(open,curNode); // if(p) // showNode((&p->data)); if(p&&p->data.f>curNode->f)//如果这个节点已经生成在open表中,且表中节点的f值大于当前节点 { p->data.g=curNode->g; p->data.f=curNode->f; p->data.parent=bestNode; } p=IsHave(closed,curNode); if(p&&p->data.f>curNode->f)//如果节点已扩展在closed表中，且表中节点的f值>curNode.f {//则更新closed表中该节点的f值和父指针，并从closed表中移出到open表中 p->data.g=curNode->g; p->data.f=curNode->f; EnQueue(open,p->data); DelQNode(closed,p); } else//未生成，未扩展，既不在closed表也不在open表，插入open表中 { // ANode node_East=*curNode; EnQueue(open,*curNode); } }//if向右扩展 //如果bestNode不是从上面的节点扩展生成的且地图中未标记为障碍物不可走并且bestNode不是地图最下面的点 if(!((bestNode->x==bestNode->parent->x) && (bestNode->y+1==bestNode->parent->y))&&map[bestNode->y+1][bestNode->x]!='o'&&bestNode->y!=(MAP_H-1)) {//向下扩展 curNode->x=bestNode->x; curNode->y=bestNode->y+1; curNode->ch=map[curNode->y][curNode->x]; curNode->g=bestNode->g+STEP; curNode->h=HVALUE(curNode); curNode->f=curNode->g+curNode->h; curNode->parent=bestNode; QNode *p=IsHave(open,curNode); if(p&&p->data.f>curNode->f)//如果这个节点已经生成在open表中,且表中节点的f值大于当前节点 { p->data.g=curNode->g; p->data.f=curNode->f; p->data.parent=bestNode; } p=IsHave(closed,curNode); if(p&&p->data.f>curNode->f)//如果节点已扩展在closed表中，且表中节点的f值>curNode.f {//则更新closed表中该节点的f值和父指针，并从closed表中移出到open表中 p->data.g=curNode->g; p->data.f=curNode->f; EnQueue(open,p->data); DelQNode(closed,p); } else//未生成，未扩展，既不在closed表也不在open表，插入open表中 { // ANode node_South=*curNode; EnQueue(open,*curNode); } // EnQueue(closed,*bestNode,true);//将已经扩展过的节点bestNode放入closed表中 }//if向下扩展 //如果bestNode不是从上面的节点扩展生成的且地图中未标记为障碍物不可走并且bestNode不是地图最上面的点 if(!((bestNode->x==bestNode->parent->x) && (bestNode->y-1==bestNode->parent->y))&&map[bestNode->y-1][bestNode->x]!='o'&&bestNode->y!=0) {//向上扩展 curNode->x=bestNode->x; curNode->y=bestNode->y-1; curNode->ch=map[curNode->y][curNode->x]; curNode->g=bestNode->g+STEP; curNode->h=HVALUE(curNode); curNode->f=curNode->g+curNode->h; curNode->parent=bestNode; QNode *p=IsHave(open,curNode); // if(p) // showNode((&p->data)); if(p&&p->data.f>curNode->f)//如果这个节点已经生成在open表中,且表中节点的f值大于当前节点 { p->data.g=curNode->g; p->data.f=curNode->f; p->data.parent=bestNode; } p=IsHave(closed,curNode); if(p&&p->data.f>curNode->f)//如果节点已扩展在closed表中，且表中节点的f值>curNode.f {//则更新closed表中该节点的f值和父指针，并从closed表中移出到open表中 p->data.g=curNode->g; p->data.f=curNode->f; EnQueue(open,p->data); DelQNode(closed,p); } else//未生成，未扩展，既不在closed表也不在open表，插入open表中 { // ANode node_North=*curNode; EnQueue(open,*curNode); } // EnQueue(closed,*bestNode,true);//将已经扩展过的节点bestNode放入closed表中 }//if向上扩展 if(!((bestNode->x-1==bestNode->parent->x) && (bestNode->y==bestNode->parent->y))&&map[bestNode->y][bestNode->x-1]!='o'&&bestNode->x!=0) {//向左扩展 curNode->x=bestNode->x-1; curNode->y=bestNode->y; curNode->ch=map[curNode->y][curNode->x]; curNode->g=bestNode->g+STEP; curNode->h=HVALUE(curNode); curNode->f=curNode->g+curNode->h; curNode->parent=bestNode; QNode *p=IsHave(open,curNode); if(p&&p->data.f>curNode->f)//如果这个节点已经生成在open表中,且表中节点的f值大于当前节点 { p->data.g=curNode->g; p->data.f=curNode->f; p->data.parent=bestNode; } p=IsHave(closed,curNode); if(p&&p->data.f>curNode->f)//如果节点已扩展在closed表中，且表中节点的f值>curNode.f {//则更新closed表中该节点的f值和父指针，并从closed表中移出到open表中 p->data.g=curNode->g; p->data.f=curNode->f; EnQueue(open,p->data); DelQNode(closed,p); } else//未生成，未�