A星算法的一个例子

最近看了A*算法的实现，转载的两篇文章写得很清楚了，大致为先找到到起点和终点，根据评估函数找到周围8个点中的最优点作为中间点，再以中间点为中心向外扩展，直到找到终点。
自己根据转载的文章写了个例子：

#include <math.h>
#include <vector>
#include <algorithm>

const int DIAG_COST = 14;
const int STRAIGHT_COST = 10;

typedef struct _MyCell 
{
    _MyCell():x(0), y(0), f(0), g(0), h(0), walkable(true), parent(NULL){};
    int x;
    int y;
    int f;
    int g;
    int h;
    bool walkable;
    _MyCell* parent;
}MyCell;

MyCell* start_node = 0;
MyCell* end_node = 0;
int row = 5;
int col = 8;
MyCell* all_nodes = NULL;

#define  Min(x, y) (x) < (y) ? (x) : (y)
#define  Max(x, y) (x) > (y) ? (x) : (y)

int Diagnoal(MyCell* node)
{
    int dx = abs(node->x - end_node->x);
    int dy = abs(node->y - end_node->y);
    int diag = Min(dx, dy);
    int straight = dx + dy;
    return DIAG_COST * diag + (straight - 2 * diag) * STRAIGHT_COST;
}

MyCell* getNode(int x, int y)
{
    return &all_nodes[x * col + y];
}

void InitAllCells()
{
    all_nodes = new MyCell[5 * 8];
    for (int i = 0; i < row; ++i)
    {
        for (int j = 0; j < col; ++j)
        {
            all_nodes[i * col + j].x = i;
            all_nodes[i * col + j].y = j;
        }
    }
    start_node = getNode(1, 1);
    end_node = getNode(3, 6);

    MyCell* tmp = getNode(0, 4);
    tmp->walkable = false;
    tmp = getNode(1,4);
    tmp->walkable = false;
    tmp = getNode(2,4);
    tmp->walkable = false;
    tmp = getNode(3,4);
    tmp->walkable = false;
}


bool CompareCell(MyCell* first, MyCell* second)
{
    return first->f < second->f;
}

bool IsIn(std::vector<MyCell*>& open_list, MyCell* test)
{
    std::vector<MyCell*>::iterator iter = open_list.begin();
    for (; iter != open_list.end(); ++iter)
    {
        if (*iter == test)
        {
            return true;
        }
    }
    return  false;
}



void main()
{
    InitAllCells();

    start_node->g = 0;
    start_node->h = Diagnoal(start_node);
    start_node->f = start_node->g + start_node->h;

    std::vector<MyCell*> open_list;

    std::vector<MyCell*> close_list;

    MyCell* node = start_node;
    while (node != end_node)
    {
        int st_x = Max(0, node->x - 1);
        int end_x = Min(row - 1, node->x + 1);
        int st_y = Max(0, node->y - 1);
        int end_y = Min(col - 1, node->y + 1);

        for (int i = st_x; i <= end_x; ++i)
        {
            for (int j = st_y; j <= end_y; ++j)
            {
                MyCell* test = getNode(i, j);
                if (test == node || test->walkable == false)
                {
                    continue;
                }

                int cost = DIAG_COST;
                if (node->x == test->x || node->y == test->y)
                {
                    cost = STRAIGHT_COST;
                }
                int g = node->g + cost;
                int h = Diagnoal(test);
                int f = g + h;

                if (IsIn(open_list, test) || IsIn(close_list, test))
                {
                    if (f < test->f)
                    {
                        test->f = f;
                        test->g = g;
                        test->h = h;
                        test->parent = node;
                    }
                }
                else
                {
                    test->f = f;
                    test->g = g;
                    test->h = h;
                    test->parent = node;
                    open_list.push_back(test);
                }
            }
        }
        close_list.push_back(node);
        if (open_list.empty())
        {
            break;
        }

        sort(open_list.begin(), open_list.end(), CompareCell);
        node = open_list[0];
        open_list.erase(open_list.begin());
    }

    std::vector<MyCell*> ret;
    ret.push_back(end_node);
    node = end_node;
    while (node != start_node)
    {
        node = node->parent;
        ret.push_back(node);
    }

    std::vector<MyCell*>::iterator iter = ret.begin();
    for (; iter != ret.end(); ++iter)
    {
        printf("x = %d, y = %d\n", (*iter)->x, (*iter)->y);
    }
}