游戏开发A*寻路算法C++实现

#include <stdio.h>

#include <list>

#include <algorithm>

using std::list;

using std::abs;

const int Max_Row = 10;

const int Max_Col = 10;

struct node

{
int i; // 行坐标
int j; // 列坐标

node(): i(0), j(0) {}
node(int _i, int _j):i(_i), j(_j) {}
bool operator== (const node& rhs) const {return i == rhs.i && j == rhs.j;}
bool operator!= (const node& rhs) const {return !(*this == rhs);}
void swap(node& rhs)
{
int _i = i;
int _j = j;
i = rhs.i;
j = rhs.j;
rhs.i = _i;
rhs.j = _j;
}

};

// 寻路中间结点

struct path_node

{
path_node(): g(0), h(0) {}

// 预计总花费
int get_f() const {return g + h;}

// 交换
void swap(path_node& rhs)
{
int _g = g;
int _h = h;
g = rhs.g;
h = rhs.h;
rhs.g = g;
rhs.h = h;
pre_node.swap(rhs.pre_node);
this_node.swap(rhs.this_node);
}

int g;
// 起点到本结点预计花费
int h;
// 本结点到终点预计花费
node pre_node;
// 寻路中上一结点
node this_node; // 本结点

};

// 地图数据， 0障碍格， 1可行走格

int g_map[Max_Row + 1][Max_Col + 1] = 

{
///////0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10
/*0*/ {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
/*1*/ {0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
/*2*/ {0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
/*3*/ {0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0},
/*4*/ {0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0},
/*5*/ {0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0},
/*6*/ {0, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0},
/*7*/ {0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0},
/*8*/ {0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 0},
/*9*/ {0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
/*10*/{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}

};

// 寻路中待检查结点队列

list<path_node> testing_nodes;

// 寻路中检查完毕结点队列

list<path_node> closed_nodes;

// 预测花费

void estimate_cost(node start, node end, node cur, path_node& path_node)

{
path_node.g = abs(cur.i - start.i) + abs(cur.j - start.j);
path_node.h = abs(end.i - cur.i) + abs(end.j - cur.j);

};

// 是否在待检测队列

path_node* find_in_testing_nodes(node cur)

{
for (list<path_node>::iterator iter = testing_nodes.begin();
iter != testing_nodes.end(); ++iter)
if (iter->this_node == cur)
return &*iter;
return NULL;

}

// 是否在关闭队列

path_node* find_in_closed_nodes(node cur)

{
for (list<path_node>::iterator iter = closed_nodes.begin();
iter != closed_nodes.end(); ++iter)
if (iter->this_node == cur)
return &*iter;
return NULL;

}

// 判断一个点是否可以加入可行走路径

bool walkable(node test_node)

{
if (g_map[test_node.i][test_node.j] == 0)
return false;
if (find_in_closed_nodes(test_node) != NULL)
return false;
return true;

}

// 获取一个点周围可行走路径，不是障碍格且没在关闭队列中

void query_walkable_node(node cur, list<node>& nodes)

{
nodes.clear();
// 只允许横、竖行走
// 左
node test_node1(cur);
--test_node1.i;
if (walkable(test_node1))
nodes.push_back(test_node1);

// 右
node test_node2(cur);
++test_node2.i;
if (walkable(test_node2))
nodes.push_back(test_node2);

// 上
node test_node3(cur);
--test_node3.j;
if (walkable(test_node3))
nodes.push_back(test_node3);

// 下
node test_node4(cur);
++test_node4.j;
if (walkable(test_node4))
nodes.push_back(test_node4);

}

// 更新顺序，插入排序（向后更新）

void update_testing_nodes(list<path_node>::iterator iter)

{
list<path_node>::iterator next_iter = iter;
++next_iter;
while (next_iter != testing_nodes.end() && iter->get_f() > next_iter->get_f())
{
iter->swap(*next_iter);
iter = next_iter;
++next_iter;
}

}

// 更新顺序，插入排序（向前更新）

void update_testing_nodes(path_node* node_ptr)

{
list<path_node>::iterator iter = testing_nodes.begin();
while (iter != testing_nodes.end())
{
if (&(*iter) == node_ptr)
break;
++iter;
}

if (iter != testing_nodes.end())
{
list<path_node>::iterator pre = iter;
--iter;
while (pre != testing_nodes.begin() && pre->get_f() > iter->get_f())
{
pre->swap(*iter);
iter = pre;
--pre;
}

if (pre == testing_nodes.begin() && pre->get_f() > iter->get_f())
pre->swap(*iter);
}

}

// 结点进入待检测队列

void enter_testing_nodes(node start, node end, node cur_node, node pre_node)

{
// 每次占用第一个结点
testing_nodes.push_front(path_node());
list<path_node>::iterator iter = testing_nodes.begin();
iter->this_node = cur_node;
iter->pre_node = pre_node;
estimate_cost(start, end, cur_node, *iter);

// 更新顺序
update_testing_nodes(iter);

}

// 结点进入关闭队列

void enter_closed_nodes(path_node node)

{
closed_nodes.push_back(node);

}

// 寻路开始

bool find_path(node start, node end, list<node>& path_nodes)

{
if (start == end)
{
printf("same node!\n");
return true;
}

// 初始化
testing_nodes.clear();
closed_nodes.clear();
list<node> nodes;

enter_testing_nodes(start, end, start, start);

// 开始计算
while (!testing_nodes.empty())
{
// 获取具有最小估测值的结点
path_node min_node = testing_nodes.front();
testing_nodes.pop_front();
query_walkable_node(min_node.this_node, nodes);

// 进入关闭队列
enter_closed_nodes(min_node);

// 处理寻找到的可行走结点
for (list<node>::const_iterator iter = nodes.begin();
iter != nodes.end(); ++iter)
{
// 若为终点，寻路结束，处理结果
if (*iter == end)
{
path_nodes.push_front(end);
path_node* node_ptr = find_in_closed_nodes(min_node.this_node);
node cur_node = node_ptr->this_node;
while (cur_node != start)
{
path_nodes.push_front(cur_node);
node_ptr = find_in_closed_nodes(node_ptr->pre_node);
cur_node = node_ptr->this_node;
};
path_nodes.push_front(start);
return true;
}

// 若在当前检测队列中，更新
path_node* node_ptr = find_in_testing_nodes(*iter);
if (node_ptr != NULL)
{
// 若走新路径更小花费，更新
int g = min_node.g + 1;
if (g < node_ptr->g)
{
node_ptr->pre_node = *iter;
node_ptr->g = g;
update_testing_nodes(node_ptr);
}
continue;
}

enter_testing_nodes(start, end, *iter, min_node.this_node);
}
}

return false;

}

void print_map(const list<node>& nodes, node start, node end)

{
for (int i = 0; i < Max_Row + 1; ++i)
{
for (int j = 0; j < Max_Col + 1; ++j)
{
if (g_map[i][j] == 0)
{
printf("■");
}
else
{
bool find_flag = 0;
for (list<node>::const_iterator iter = nodes.begin();
iter != nodes.end(); ++iter)
{
if (iter->i == i && iter->j == j)
find_flag = true;
}
if (find_flag)
{
if (start == node(i, j))
printf("▲");
else if(end == node(i, j))
printf("▼");
else
printf("●");
}
else
printf("○");
}
}
printf("\n");
}

}

int main()

{
list<node> nodes;
node start(4, 3);
node end(2, 2);
if (find_path(start, end, nodes))
{
printf("find path!\n");
for (list<node>::const_iterator iter = nodes.begin();
iter != nodes.end(); ++iter)
printf("(%d,%d)\n", iter->i, iter->j);
}
else
{
printf("no path!\n");
}
print_map(nodes, start, end);

getchar();
return 0;

}