A*算法C实现

参考 http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html 实现了A*算法，模拟了一下，大多数场景还是可以应对的，以传统的场景为例 

 

以下，1为源，2为目的，-1为墙。

 

初始地图：

 0  0  0  0  0
 0  0 -1  0  0
 0  1 -1  0  2
 0  0 -1  0  0
 0  0  0  0  0

路径：

 0  0  0  0  0
 0  0 -1  0  0
 0  1 -1  0  2
 0  1 -1  1  0
 0  0  1  0  0

 

但对于下面的地址，处理的结果很差

 0  0  0  0  0
 0 -1  0 -1  0
 1 -1  0 -1  2
 0 -1  0 -1  0
 0  0  0  0  0

 

路径： 

 0  1  0  0  0
 1 -1  1 -1  0
 1 -1  1 -1  2
 0 -1  1 -1  1
 0  0  0  1  0

 

这种场景还要再研究下优化方法，第一版的草稿先粘一下，有兴趣的朋友可以参考。为了方便大家编译，把地图编辑器也去掉了，换成硬编码的二维数组。

 

/*
 * A* 算法模拟
 */

#include <stdio.h>
#include <stdlib.h>
#include <vector>

using std::vector;

typedef struct stNode
{
	stNode() : x(0), y(0), px(0), py(0), f(0), g(0), h(0)
	{}

	int x;
	int y;
	int px; /* 父结点横坐标 */
	int py; /* 父结点纵坐标 */
	int f;
	int g;
	int h;
}Node;

/*
 * 地图0为通路，1为起点，2为终点，-1为不可达
 */
#define ROW 5
#define COLUMN 5
int short_map[][COLUMN] = {
	{0, 0, 0, 0, 0},
	{0, -1, 0, -1, 0},
	{1, -1, 0, -1, 2},
	{0, -1, 0, -1, 0},
	{0, 0, 0, 0, 0}};

vector<Node> close_map;
vector<Node> open_map;

/*
 * 找到本次查找的边界
 */
void CalcBound(Node & cur, int &startx, int &starty, int &endx, int &endy)
{
	startx = (cur.x > 0) ? (cur.x - 1) : cur.x;
	starty = (cur.y > 0) ? (cur.y - 1) : cur.y;
	endx = (cur.x < COLUMN) ? (cur.x + 1) : cur.x;
	endy = (cur.y < COLUMN) ? (cur.y + 1) : cur.y;
}

/*
 * 计算G值，G=从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径 
 */
int CalcG(Node cur, int nextx, int nexty)
{
	if (cur.x == nextx || cur.y == nexty)
	{
		return 10;	
	}
	else
	{
		return 14;
	}
}

/*
 * 计算H值，H=从指定的方格移动到终点 B 的估算成本
 * 我们使用 Manhattan 方法，计算从当前方格横向或纵向移动到达目标所经过的方格数，忽略对角移动，然后把总数乘以 10 
 *
 */
int CalcH(Node dst, int nextx, int nexty)
{
	int stepnum = 0;

	stepnum += ((dst.x >= nextx) ? 1 : -1) * (dst.x - nextx);
	stepnum += ((dst.y >= nexty) ? 1 : -1) * (dst.y - nexty);

	return stepnum * 10;
}

Node * IsInCloseMap(int i, int j)
{
	int pos = 0;
	for (vector<Node>::const_iterator it = close_map.begin(); it != close_map.end(); it++, pos++)
	{
		if (it->x == i && it->y == j )
		{
			return &close_map[pos];
		}
	}

	return NULL;
}

Node * IsInOpenMap(int i, int j)
{
	int pos = 0;
	for (vector<Node>::const_iterator it = open_map.begin(); it != open_map.end(); it++, pos++)
	{
		if (it->x == i && it->y == j )
		{
			return &(open_map[pos]);
		}
	}

	return NULL;
}

void InsertCloseList(Node & node)
{
	close_map.push_back(node);
}

void InsertOpenList(Node & node)
{
	open_map.push_back(node);
}

/*
 * 计算F值，返回下一步坐票
 *
 * param int cur: 当前计算的坐标
 * param out next: 下一步的坐标即F值最小的一个
 */
int FindNext(Node cur, Node dst, Node & next)
{
	int startx = 0; /* 遍历的横向坐标起点 */
	int starty = 0; /* 遍历的纵向坐标起点 */
	int endx = 0;   /* 遍历的横向坐标终点 */
	int endy = 0;   /* 遍历的纵向坐标终点 */

	InsertCloseList(cur);

	/* 计算坐标 */
	CalcBound(cur, startx, starty, endx, endy);

	/* 遍历找到F值最小的坐标 */
	int min = 0;
	int minx = 0;
	int miny = 0;

	Node * pnext = NULL;
	for (int i = startx; i <= endx; i++)
	{
		for (int j = starty; j <= endy; j++)
		{
			if (IsInCloseMap(i, j))
			{/* 已存在于关闭列表 */
				continue;
			}

			if (-1 == short_map[i][j])
			{/* 不可达跳过 */
				continue;
			}

			/* 找到一个相邻结点，判断是否在open_list中 a*/
			Node * node = IsInOpenMap(i, j);
			if (NULL == node)
			{
				node = new Node();
				node->x = i;
				node->y = j;
				node->px = cur.x;
				node->py = cur.y;
				node->g = CalcG(cur, i, j);
				node->h = CalcH(dst, i, j);
				node->f = node->g + node->h;

				InsertOpenList(*node);
			}
			else
			{/* 在open_list中 */
				int g = CalcG(cur, i, j);
				if ((g + cur.g) < node->g)
				{
					node->px = cur.x;
					node->py = cur.y;
					node->g = g;
					node->f = node->g + node->h;
				}
			}

			int value = node->f;
			if (0 == min || min > value)
			{
				min = value;
				pnext = node;
			}
		}
	}

	next = * pnext;

	return 0;
}

void Draw()
{
	for (int i = 0; i < ROW; i++)
	{
		for (int j =0; j < COLUMN; j++)
		{
			printf("%2d ", short_map[i][j]);
		}
		printf("\n");
	}
	printf("\n");

}

int AStar()
{
	Node Src;
	Node Dst;
	Node Cur;
	Node Next;

	/* 查找源和目的结点 */
	for (int i = 0; i < ROW; i++)
	{
		for (int j =0; j < COLUMN; j++)
		{
			if (1 == short_map[i][j])
			{
				Src.x = i; 
				Src.y = j;
				Cur.x = i;
				Cur.y = j;
			}
			
			if (2 == short_map[i][j])
			{
				Dst.x = i;
				Dst.y = j;
			}

			printf("%2d ", short_map[i][j]);
		}

		printf("\n");
	}
	printf("\n");

	/* 寻路 */
	for (int i = 0; ; i++)
	{
		FindNext(Cur, Dst, Next);

		if (Next.x == Dst.x && Next.y == Dst.y)
		{
			InsertCloseList(Next);
			break;
		}

		Cur = Next;
	}

	/* 反向绘图 */
	Node * node;
	int x = Dst.x;
	int y = Dst.y;
	
	while(1)
	{
		node = IsInCloseMap(x, y);
		
		if (NULL == node)
		{
			printf("[%d][%d]\n", x, y);
			break;
		}

		if (node->x == Src.x && node->y == Src.y)
		{
			break;
		}
	
		short_map[node->px][node->py] = 1;
		Draw();
		x = node->px;
		y = node->py;
	}
}

int main(int argc, char ** argv)
{
	AStar();
	return 0;
}