MicroPather实现A*算法

#include "micropather.h"

#include <ctype.h>  
#include <stdio.h>  
#include <memory.h>  
#include <math.h>  

#include <vector>  
#include <iostream>  

#include "micropather.h"  

using namespace micropather;  

//地图的长宽;  
//x为列号,y为行号;
const int MAPX = 10;  
const int MAPY = 10;  
const int gMap[MAPX * MAPY + 1] =
{   
    0, 0, 0, 1, 1, 1, 0, 0, 0, 1,
    1, 0, 1, 0, 1, 1, 0, 0, 1, 1,
    0, 0, 0, 1, 1, 1, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 1, 0, 1, 0,
    0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
    1, 0, 0, 0, 1, 0, 1, 0, 1, 0,
    0, 0, 0, 0, 0, 1, 1, 1, 0, 0,
    1, 0, 0, 0, 1, 1, 1, 0, 0, 0,
    1, 1, 0, 0, 0, 1, 0, 0, 0, 1,
    0, 0, 0, 1, 0, 0, 0, 1, 0, 0  
};  


class AStar : public Graph  
{  
private:  
    AStar(const AStar& );  
    void operator = (const AStar& );  

    int playerX, playerY;  
    std::vector<void*> path;  

    MicroPather* pather;  

public:  
    AStar() : pather ( 0 )  
    {  
        pather = new MicroPather(this);  
    }  

    virtual ~AStar()  
    {  
        delete pather;  
    }  

    int X() { return playerX; }  
    int Y() { return playerY; }  

    unsigned Checksum()  
    {  
        pather->Checksum();  
    }  

    void ClearPath()  
    {  
        path.resize( 0 );  
    }  

    int Passable(int nx, int ny)  
    {  
        if( nx >= 0 && nx <MAPX && ny >= 0 && ny <MAPY)  
        {  
            int index = ny * MAPX + nx;  
            int c = gMap[index];  
            if ( c == 0)  
                return 1;      //可通行;  
        }  
        return 0;             //不可通行;  
    }  

    void SetStartPos(int x, int y)
    {
        playerX = x;
        playerY = y;
    }

    int SetEndPos(int nx, int ny)  
    {  
        int result = 0;  
        if(Passable(nx, ny) == 1)  
        {  
            float totalCost;  
            result = pather->Solve(XYToNode(playerX, playerY), XYToNode(nx, ny), &path, &totalCost);  
            if(result == MicroPather::SOLVED)  
            {  
                playerX = nx;  
                playerY = ny;  
            }  
        }  
        return result;  
    }  

    //把索引号转化为XY坐标;  
    void NodeToXY(void *node, int *x, int *y)  
    {  
        int index = (int)node;  
        *y = index / MAPX;  
        *x = index - *y * MAPX;  
    }  

    //把XY坐标转化为索引号;  
    void* XYToNode(int x, int y)  
    {  
        return (void*)(y * MAPX + x);  
    }  

    //最小代价估计函数，求A*算法中的h值;  
    virtual float LeastCostEstimate(void *nodeStart, void *nodeEnd)  
    {  
        int xStart, yStart, xEnd, yEnd;  
        NodeToXY(nodeStart, &xStart, &yStart);  
        NodeToXY(nodeEnd, &xEnd, &yEnd);  

        int dx = xStart - xEnd;  
        int dy = yStart - yEnd;  
        return (float)sqrt((double)(dx * dx) + (double)(dy * dy));  
    }  

    virtual void AdjacentCost(void *node, std::vector<StateCost> *neighbors)  
    {  
        int x, y;  
        //八个方向  
        //const int dx[8] = { 1, 1, 0, -1, -1, -1, 0, 1 };  
        //const int dy[8] = { 0, 1, 1, 1, 0, -1, -1, -1 };  
        //const float cost[8] = { 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f };  

        //上下左右四个方向;  
        const int dx[4] = {1, 0, -1, 0};  //4个方向上的x的变化;
        const int dy[4] = {0, -1, 0, 1};  //4个方向上的y的变化;
        const float cost[4] = {1.0f, 1.0f, 1.0f, 1.0f};  

        NodeToXY(node, &x, &y);  

        for(int i = 0; i < 4; ++i)  
        {  
            int nx = x + dx[i];  
            int ny = y + dy[i];  
            //是否可行;
            int pass = Passable(nx, ny);  
            if(pass == 1)  
            {  
                StateCost nodeCost = {XYToNode(nx, ny), cost[i]};  
                neighbors->push_back(nodeCost);
            }  
        }  
    }  

    virtual void PrintStateInfo(void *node)  
    {  
        int x, y;  
        NodeToXY(node, &x, &y);  
        printf("(%d, %d)\n", y, x);  
    }  

    void Print()  
    {  
        unsigned size = path.size();  
        for(unsigned k = 0; k < size; ++k)  
        {  
            PrintStateInfo(path[k]);  
        }  
    }  
};  

int main()  
{  
    AStar astar;  
    astar.SetStartPos(2, 2);
    astar.SetEndPos(4, 6);  
    astar.Print();  

    system("pause");

    return 0;  
}