A星算法的一个例子

来源：互联网发布：扩音器软件下载编辑：程序博客网时间：2024/05/14 11:33
最近看了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);    }}
0 0