A*初步实现

来源：互联网发布：淘宝网 pong ipad 编辑：程序博客网时间：2024/06/05 06:25
由前几天写的ACM代码改了下,昨天下午写的但再存储最短路径时有点问题,刚把最短路径存储改好了.加了个4方向还是8方向,在障碍物边上是否拐角绕行.这里面和<A*寻路初探>中有些区别,没对新扩展节点在OPENLIST中时做重新指向处理.道路不计成本.
//A* 障碍拐角 4方向8方向 
#include <iostream> 
#include <list> 
#include <vector> 
#include <algorithm> 
using namespace std;
int R,C;//地图行列 
int sr,sc,dr,dc;//起点 终点 
int map[100][100];//0是障碍1是可行 
bool turnCorner=true;//是否绕过拐角 
bool eight=true;//4方向和8方向扩展 
struct Node 
{
    int f,g,h;
    int r,c;
    int parent;
};
vector<Node> openList;
vector<Node> closeList;
vector<Node> bestList;
Node child;
int ABS(int a,int b)
{
    return max(a,b)-min(a,b);
}
bool dayu(Node & l,Node & r)
{
    return l.f>r.f;
}
bool check(Node value)
{
    if (value.r==child.r&&value.c==child.c)
    {
        return true;
    }
    return false;
}
void Init(Node &start)
{
    if (!openList.empty())
    {
        openList.clear();
    }
    openList.push_back(start);
    if (!closeList.empty())
    {
        closeList.clear();
    }
}
void BestPath(Node &best)
{
    Node mudi=best;
    if (!bestList.empty())
    {
        bestList.clear();
    }
    while (mudi.parent>-1)
    {
        bestList.push_back(mudi);
        mudi=closeList[mudi.parent];
    }
}
bool Reach(int r,int c)
{
    if (!map[r][c]||r<0||c<0||r>R||c>C)
        return false;
    return true;
}
void Generate(Node &node,int r,int c)
{
    //不再open和close中 
    child.r=r;
    child.c=c;
    vector<Node>::iterator posOP=find_if(openList.begin(),openList.end(),check);
    vector<Node>::iterator posCL=find_if(closeList.begin(),closeList.end(),check);
    if (posOP==openList.end()&&posCL==closeList.end())
    {
        child.parent=closeList.size()-1;
        child.g=node.g+1;   //GetCost(r,c)+node.g; 
        child.h=ABS(dr,r)+ABS(dc,c);    //(tr-r)*(tr-r)+(tc-c)*(tc-c) 
        child.f=child.g+child.h;
        openList.push_back(child);
        sort(openList.begin(),openList.end(),dayu);//openList.sort(dayu); 
    }
}
void kuozhan(Node &node)
{
    bool flag[4]={true};
    if (Reach(node.r-1,node.c))// 上  
    {
        Generate(node,node.r-1,node.c);
    }
    else 
    {flag[0]=false,flag[1]=false;}
    if (Reach(node.r,node.c+1))//右 
    {
        Generate(node,node.r,node.c+1);
    }
    else 
    {flag[1]=false,flag[2]=false;}
    if (Reach(node.r+1,node.c))// 下 
    {
        Generate(node,node.r+1,node.c);
    }
    else 
    {flag[2]=false,flag[3]=false;}
    if (Reach(node.r,node.c-1))// 左  
    {
        Generate(node,node.r,node.c-1);
    }
    else 
    {flag[3]=false,flag[0]=false;}
    if (!eight)
    {
        return;
    }
    if (!turnCorner)
    {
        for(int i=0;i<4;i++)
            flag[i]=true;
    }
    if (flag[0]&&Reach(node.r-1,node.c-1))// 左上  
    {
        Generate(node,node.r-1,node.c-1);
    }
    if (flag[1]&&Reach(node.r-1,node.c+1))//右上 
    {
        Generate(node,node.r-1,node.c+1);
    }
    if (flag[2]&&Reach(node.r+1,node.c+1))// 右下 
    {
        Generate(node,node.r+1,node.c+1);
    }
    if (flag[3]&&Reach(node.r+1,node.c-1))// 左下  
    {
        Generate(node,node.r+1,node.c-1);
    }
}
void FindPath(int sr,int sc,int dr,int dc)
{
    Node temp,best;
    temp.r=sr;
    temp.c=sc;
    temp.g=0;
    temp.h=ABS(sr,dr)+ABS(sc,dc);
    temp.f=temp.g+temp.h;
    temp.parent=-1;
    Init(temp);
    while (!openList.empty())
    {
        closeList.push_back(openList.back());
        openList.pop_back();
        best=closeList.back();
        if (best.r==dr&&best.c==dc)//找到 
        {
            BestPath(best);
            cout<<best.f<<endl;
            return;
        }
        kuozhan(best);
    }
    //没路径 
    cout<<"NO WAY!/n";
}
void ShowPath()
{
    int i,j;
    for (i=bestList.size();i>0;i--)
    {
        map[bestList.back().r][bestList.back().c]=8;
        bestList.pop_back();
    }
    for (i=0;i<R;i++)
    {
        for (j=0;j<C;j++)
        {
            cout<<map[i][j]<<" ";
        }
        cout<<endl;
    }
}
int main()
{
    freopen("in.txt","r",stdin);
    int i,j;
    cin>>R>>C;
    for (i=0;i<R;i++)
    {
        for (j=0;j<C;j++)
            cin>>map[i][j];
    }
    cin>>sr>>sc>>dr>>dc;
    FindPath(sr,sc,dr,dc);
    ShowPath();
    return 0;
}
输入:
10 10
1 1 1 1 1 0 0 0 0 0
1 2 0 0 1 1 1 1 1 0
1 1 1 0 1 0 0 0 1 1
0 0 1 0 1 1 1 0 1 1
1 1 1 0 0 0 0 1 1 1
1 1 0 0 1 1 1 1 1 1
1 1 1 0 1 1 1 0 0 0
0 0 1 0 0 0 2 0 1 1
1 1 1 1 1 0 0 0 1 1
1 1 1 1 1 1 1 1 1 1
1 1 7 6
输出:
17
1 8 8 8 8 0 0 0 0 0
1 2 0 0 8 8 8 8 8 0
1 1 1 0 1 0 0 0 8 1
0 0 1 0 1 1 1 0 8 1
1 1 1 0 0 0 0 8 8 1
1 1 0 0 1 1 8 8 1 1
1 1 1 0 1 1 8 0 0 0
0 0 1 0 0 0 8 0 1 1
1 1 1 1 1 0 0 0 1 1
1 1 1 1 1 1 1 1 1 1