A star 尋路算法實現（C++版本）

rpg遊戲中的尋路算法，寫出來測試了一下，A*算法的理論我就不講了

下面直接是代碼，在centos 6.2 上編譯測試，其他平臺應該也沒有問題。

---------------------------------------------Astar.h-----------------------------------------------------------

#include <stdlib.h>
#include <list>

//移動座標
class MoveCoord
{
public:
    MoveCoord(int grid_x=-1, int grid_y=-1);
    MoveCoord& operator+(const MoveCoord& coord);
    MoveCoord& operator+=(const MoveCoord& coord);
    bool operator==(const MoveCoord coord);
    bool operator!=(const MoveCoord& coord);
    MoveCoord& operator=(const MoveCoord& coord);

public:
    int grid_x;//格子座標x
    int grid_y;//格子座標y
};


//尋路座標
class PathCoord
{
public:
    PathCoord(void);
    PathCoord(MoveCoord parent,MoveCoord cur);
    virtual ~PathCoord();

    int get_f(void) const;

    MoveCoord parent;//父座標
    MoveCoord cur;//當前座標
    int g_value;//起點到該點的G值
    int h_value;//該點到目標點都f值
};

typedef std::list<MoveCoord> CoordList;
typedef bool (*IsMovableCoord)(int scene_id,int coord_x,int coord_y);

class AStar
{
public:
    AStar(void){};
    virtual ~AStar(void){};
    
    /* 在場景scene_id中尋找從src 到 dst的路徑，找到之後保存再coord_list中,返回true。否則返回false
     * 參數 IsMovableCoord：判斷座標是否可走的函數，可走返回true，否則返回false
     */
    static bool FindPath(const int scene_id,MoveCoord src,MoveCoord dst,IsMovableCoord is_movable,CoordList& coord_list);
};

#endif

---------------------------------------------Astar.cpp-----------------------------------------------------------

#include <queue>
#include <map>
#include "AStar.h"

MoveCoord::MoveCoord(int grid_x, int grid_y)
{
    this->grid_x = grid_x;
    this->grid_y = grid_y;
}
MoveCoord& MoveCoord::operator+(const MoveCoord& coord)
{
    this->grid_x +=coord.grid_x;
    this->grid_y +=coord.grid_y;
    return *this;
}
MoveCoord& MoveCoord::operator+=(const MoveCoord& coord)
{
    *this = *this + coord;
    return *this;
}
bool MoveCoord::operator==(const MoveCoord coord)
{
    return (this->grid_x==coord.grid_x
        && this->grid_y==coord.grid_y);
}
bool MoveCoord::operator!=(const MoveCoord& coord)
{
    return !(*this==coord);
}
MoveCoord& MoveCoord::operator=(const MoveCoord& coord)
{
    this->grid_x =coord.grid_x;
    this->grid_y =coord.grid_y;
    return *this;
}

PathCoord::PathCoord(void)
{
}

PathCoord::~PathCoord()
{
}

PathCoord::PathCoord(MoveCoord parent,MoveCoord cur)
{
    this->parent = parent;
    this->cur = cur;
    this->g_value = 0;
    this->h_value = 0;
}

int PathCoord::get_f(void) const
{
    return this->g_value + this->h_value;
}


//src ,dst 必須是相鄰的格子
int cal_g(const MoveCoord src,const MoveCoord dst)
{
    int cur = 14;
    if(src.grid_x== dst.grid_x
            || src.grid_y == dst.grid_y)
    {
        cur = 10;
    }
    return cur;
}

/*manhattan 計算從當前方格橫向或縱向移動到達目標所經過的方格數
忽略對角移動，然後把總數乘以 10*/
int cal_h(const MoveCoord src,const MoveCoord dst)
{
    int dx = abs(src.grid_x-dst.grid_x);
    int dy = abs(src.grid_y-dst.grid_y);
    return (dx+dy)*10;
}

// std::map 會根據key排序，因此定義自己的排序函數
bool operator<(const MoveCoord& lhs,const MoveCoord& rhs)
{
    if(lhs.grid_x < rhs.grid_x)
    {
        return true;
    }
    else if(lhs.grid_x == rhs.grid_x)
    {
        if(lhs.grid_y < rhs.grid_y)
        {
            return true;
        }
    }
    return false;
}

//定義 typedef std::map<MoveCoord,PathCoord,less> CoordMap;key爲MoveCoord，自定義key的比較函數
struct less
{
    bool operator()(const MoveCoord& lhs,const MoveCoord& rhs) const
    {
        if(lhs.grid_x < rhs.grid_x)
        {
            return true;
        }
        else if(lhs.grid_x == rhs.grid_x)
        {
            if(lhs.grid_y < rhs.grid_y)
            {
                return true;
            }
        }
        return false;
    }
};


// 最小堆排序（比較F值）
bool operator<(const PathCoord& lhs,const PathCoord& rhs)
{
    return lhs.get_f() > rhs.get_f();
}


typedef std::map<MoveCoord,PathCoord,less> CoordMap;
typedef std::priority_queue<PathCoord> CoordHeap;

/* 在場景scene_id中尋找從src 到 dst的路徑,找到之後保存再coord_list中
 * 返回true。否則返回false
*/
bool AStar::FindPath(const int scene_id,MoveCoord src,MoveCoord dst,
                     IsMovableCoord is_movable,CoordList& coord_list)
{
    if( src == dst)
    {
        return false;
    }
    
    coord_list.clear();

    //九宮格數組
    int coord_arr[9][2]={
        {0,0},
        {-1,1},{0,1},{1,1},
        {-1,0},{1,0},
        {-1,-1},{0,-1},{1,-1},
    };

    CoordHeap uncheck_coord_heap;
    CoordMap checked_map;
    CoordMap uncheck_map;

    PathCoord cur_path_coord(MoveCoord(-1,-1),src);
    uncheck_coord_heap.push(cur_path_coord);

    while(uncheck_coord_heap.empty()==false)
    {
        PathCoord cur_path_coord = uncheck_coord_heap.top();
        uncheck_coord_heap.pop();
        uncheck_map.erase(cur_path_coord.cur);
        checked_map[cur_path_coord.cur]= cur_path_coord;

        if(cur_path_coord.cur==dst)//到達目標點
        {
            while(cur_path_coord.parent!=MoveCoord(-1,-1))
            {
                coord_list.push_front(cur_path_coord.cur);
                cur_path_coord = checked_map[cur_path_coord.parent];
            }
            return true;
        }

        for (int i = 1; i <=8; i++)
        {
            MoveCoord cur_check = cur_path_coord.cur;
            cur_check += MoveCoord(coord_arr[i][0],coord_arr[i][1]);
            if(cur_check.grid_x<0 || cur_check.grid_y<0)
            {
                continue;
            }
            if(is_movable(scene_id,cur_check.grid_x,cur_check.grid_y)==false)
            {
                continue;
            }
            
            CoordMap::iterator iter1 = checked_map.find(cur_check);
            if (iter1 != checked_map.end())
            {
                continue;
            }

            CoordMap::iterator iter2 = uncheck_map.find(cur_check);
            int tmp_g = cal_g(cur_path_coord.cur,cur_check)+cur_path_coord.g_value;
            if(iter2==uncheck_map.end())
            {
                PathCoord path_coord(cur_path_coord.cur,cur_check);
                path_coord.g_value = tmp_g;
                path_coord.h_value = cal_h(cur_check,dst);
                uncheck_coord_heap.push(path_coord);
                uncheck_map[cur_check]=path_coord;
            }
            else
            {
                PathCoord &path = iter2->second;
                if (path.g_value > tmp_g)
                {
                    path.parent = cur_path_coord.cur;
                    path.g_value = tmp_g;
                    uncheck_coord_heap.push(path);
                }
            }
        }
    }
    return false;
}

----------------------------------------------main.cpp------------------------------------------------------------------------------

#include "AStar.h"
#include <tchar.h>
#include <iostream>


//假設下面是一張小地圖
/*
 y ----------------------------------------------------------------------------------
 * |(0,9)  | (1,9)  | (2,9) | (3,9) | (4,9) | (5,9)  | (6,9) | (7,9) | (8,9) | (9,9) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,8)  | (1,8)  | (2,8) | (3,8) | (4,8) | (5,8)  | (6,8) | (7,8) | (8,8) | (9,8) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,7)  | (1,7)  | (2,7) | (3,7) | (4,7) | (5,7)  | (6,7) | (7,7) | (8,7) | (9,7) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,6)  | (1,6)  | (2,6) | (3,6) | (4,6) | (5,6)  | (6,6) | (7,6) | (8,6) | (9,6) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,5)  | (1,5)  | (2,5) | (3,5) | (4,5) | (5,5)  | (6,5) | (7,5) | (8,5) | (9,5) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,4)  | (1,4)  | (2,4) | (3,4) | (4,4) | (5,4)  | (6,4) | (7,4) | (8,4) | (9,4) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,3)  | (1,3)  | (2,3) | (3,3) | (4,3) | (5,3)  | (6,3) | (7,3) | (8,3) | (9,3) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,2)  | (1,2)  | (2,2) | (3,2) | (4,2) | (5,2)  | (6,2) | (7,2) | (8,2) | (9,2) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,1)  | (1,1)  | (2,1) | (3,1) | (4,1) | (5,1)  | (6,1) | (7,1) | (8,1) | (9,1) |
 * |-------|--------|-------|-------|-------|--------|-------|-------|-------|-------|
 * |(0,0)  | (1,0)  | (2,0) | (3,0) | (4,0) | (5,0)  | (6,0) | (7,0) | (8,0) | (9,0) |
 0 ----------------------------------------------------------------------------------> x
 */

bool MovableCoord(int scene_id,int x,int y)
{
    MoveCoord coord(x,y);
    //指定不可走點
    if(coord == MoveCoord(-1,-1)
        || coord == MoveCoord(5,4)
        || coord == MoveCoord(0,2)
        || coord == MoveCoord(1,2)
        || coord == MoveCoord(2,2)
        || coord == MoveCoord(3,2)
        || coord == MoveCoord(4,2)
        || coord == MoveCoord(5,2)
        || coord == MoveCoord(6,2)
        || coord == MoveCoord(7,2)
        || coord == MoveCoord(8,2))
        //|| coord == MoveCoord(1,8)
        //|| coord == MoveCoord(2,8)
        //|| coord == MoveCoord(3,8)
        //|| coord == MoveCoord(4,8))
    {
        return false;
    }
    return true;
}

int main(int argc, char* argv[])
{
    MoveCoord src(5,1);
    MoveCoord dst(5,5);
    
    CoordList path_list;
    if(AStar::FindPath(0,src,dst,MovableCoord,path_list))
    {
        CoordList::iterator iter = path_list.begin();
        for(;iter!= path_list.end();iter++)
        {
            std::cout<<"coord: " << iter->grid_x << " " << iter->grid_y << std::endl;
        }
    }
    return 0;
}