我的A*寻路算法

我的A*寻路算法

       关于A*算法，可以参考： http://data.gameres.com/message.asp?TopicID=25439
       以下是实现的代码。
AStar.cpp

#include  " StdAfx.h "
#include  " .\astar.h "



Node  CreateNode( int  _x, int  _y, int  _value = 0 ,Node  *  _parent = 0 )
{
    return  Node(_x,_y,_value,_parent);
}
// 重新排列開始隊列
bool sort_node(const Node  *   & node1,const Node  *   & node2)
{
    return node1->value < node2->value ;
}


AStar::AStar( void )
{
}

AStar:: ~ AStar( void )
{
    //釋放空間
    for(int i=0;i<vec_begin.size();++i)
        delete vec_begin[i];

    for(int k=0;k<vec_closed.size();++k);
        delete vec_closed[0];

}

// 轉入地圖文件初始化數組
void  AStar::Initalize( char   *  mapfile)
{
      ifstream fin(mapfile);
      fin.getline(map[0],MAX_COL);
      col=(int)strlen(map[0]);
      row=0;
      while(map[row][col]==NULL)
      {
         row++;
          fin.getline(map[row],MAX_COL);      
      }

      for(int i=0;i<row;i++)
      {
          for(int j=0;j<col;j++)
              cout<<map[i][j];
          cout<<endl;
      }
}
// 比較兩個節點位置是否相同
bool AStar::equal_node( Node  * node1, Node  * node2)
{
    if(node1->x==node2->x&&
        node1->y==node2->y)
        return true;
    return false;
}

// 開始搜索*
void  AStar::Search()
{
    vec_begin.push_back(&start);
    while(!vec_begin.empty())
    {
        if(vec_begin.size()==1&&equal_node(vec_begin[0],&end))
            break;
        //取出第一個不是目標位置
        Node *node;
        int pos=0;
        do
        {
            node=vec_begin[pos];
            pos++;
        }while(equal_node(node,&end));
        

        Node *temp_node;
        int m,n;
        //向右
        m=node->x;
        n=node->y+1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+10,node);
            vec_begin.push_back(temp_node);
        }
        //右下
        m=node->x+1;
        n=node->y+1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+14,node);
            vec_begin.push_back(temp_node);
        }
        //向下
        m=node->x+1;
        n=node->y;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+10,node);
            vec_begin.push_back(temp_node);
        }
        //左下
        m=node->x+1;
        n=node->y-1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+14,node);
            vec_begin.push_back(temp_node);
        }
        //向左
        m=node->x;
        n=node->y-1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+10,node);
            vec_begin.push_back(temp_node);
        }
        //左上
        m=node->x-1;
        n=node->y-1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+14,node);
            vec_begin.push_back(temp_node);
        }

        //向上
        m=node->x-1;
        n=node->y;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+10,node);
            vec_begin.push_back(temp_node);
        }
        //右上
        m=node->x-1;
        n=node->y+1;
        if(InMap(m,n)&&!InClosed(m,n))
        {
            temp_node=new Node(m,n,node->value+14,node);
            vec_begin.push_back(temp_node);
        }

        //將該位置加入closed
        vec_closed.push_back(node);
        vec_begin.erase(vec_begin.begin()+pos-1);

        //重新將begin隊列排序，重復取value小的
        Sort();

        cout<<"----------------------begin------------------------------"<<endl;
        put_vector(vec_begin);
        cout<<"----------------------closed-----------------------------"<<endl;
        put_vector(vec_closed);
        //system("pause");
    }
    if(vec_begin.size()==1)
    {
        cout<<"output the path:"<<endl;
        Node* temp=vec_begin[0];
        while(temp!=0)
        {
            cout<<temp->x<<"  "<<temp->y<<endl; 
            temp=temp->parent;
        }                      
    }
}
void  AStar::put_vector(vector < Node  *>   & vect)
{
    
    for(int i=0;i<vect.size();i++)
        cout<<vect[i]<<" :  "<<vect[i]->x<<"    "<<vect[i]->y<<"    "<<vect[i]->value<<"    parent:"<<vect[i]->parent<<endl;
}
// 在地圖上該位置是否合法(不可到達位置)
bool AStar::InMap( int  x, int  y)
{
    if(x>=row||x<0) return false;
    if(y>=col||y<0) return false;

    if(map[x][y]=='1') return false;

    return true;
}
// 是否在關閉隊列:已經搜索過的位置放入關閉隊列
bool AStar::InClosed( int  x, int  y)
{
    for(int i=0;i<vec_closed.size();i++)
    {
        if(vec_closed[i]->x==x&&vec_closed[i]->y==y)
            return true;
    }
    return false;
}

void  AStar::Sort()
{
    ::sort(vec_begin.begin(),vec_begin.end(),sort_node);
    nodeiter::iterator iter,iter2;;
    iter=vec_begin.begin()+1;
    
    while(iter!=vec_begin.end())
    {
        iter2=vec_begin.begin();
         while(iter2!=iter)
        {
            if((*iter)->x==(*iter2)->x&&(*iter)->y==(*iter2)->y)
            {
               //cout<<"                                                  ======0"<<endl;
               iter2=iter-1;
               delete *iter;
               vec_begin.erase(iter);
               iter=iter2;
               break;
            }
            iter2++;
        }
       
        iter++;
    }
}

   AStar.h

/**/ /*
** Auther:cjz
** Date: 2008/01/14
** Description:A*尋路算法
*/
#pragma once
#include  < algorithm >

#include  < vector >
#include  < fstream >
using namespace std;

#define MAX_COL  50
struct Node
{

    
    int x;
    int y;
    int value;
    Node * parent;
    
    Node(int _x=0,int _y=0,int _value=0,Node * _parent=0)
    {
        x=_x;
        y=_y;
        value=_value;
        parent=_parent;
    }
} ;


class AStar
{
public:
    AStar(void);
    ~AStar(void);

    void Initalize(char * mapfile);
    //開始尋路
    void Search();
    
private:
    bool equal_node( Node *node1, Node *node2);
    bool InMap(int x,int y );
    bool InClosed(int x,int y);
    void Sort();
    void put_vector(vector<Node*> &);

private:
    char map[MAX_COL][MAX_COL];
    int row,col;
    vector<Node *> vec_begin,vec_closed;
    typedef vector<Node *>  nodeiter;
public:
    
    Node start,end;
} ;

用法:
   

#include  " AStar.h "


int  _tmain( int  argc, _TCHAR *  argv[])
{
    

    
    AStar star;
    star.Initalize("c:\\a.txt");

    cout<<"input start pos(x,y):";
    int x,y;
    scanf("%d,%d",&x,&y);
    star.start=Node(x,y);
    cout<<"input end   pos(x,y):";
    scanf("%d,%d",&x,&y);
    star.end=Node(x,y);

    star.Search();



    system("pause");
    return 0;
}