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;
}

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

发表于

我来说两句

0 条评论
登录 后参与评论

相关文章

来自专栏ml

nyoj-----幸运三角形

幸运三角形 时间限制:1000 ms  |  内存限制:65535 KB 难度:3 描述         话说有这么一个图形,只有两种符号组成(‘+’或者‘-’...

30110
来自专栏marsggbo

Udacity并行计算课程笔记- Fundamental GPU Algorithms (Reduce, Scan, Histogram)

如下图示,第一种情况只有一个工人挖洞,他需要8小时才能完成,所以工作总量(Work)是8小时。第二种情况是有4个工人,它们2个小时就能完成挖洞任务,此时工作总量...

1231
来自专栏数据结构与算法

Day4晚笔记

数据结构 并查集:捆绑两个点的信息,判断对错 倍增:LCA, 字符串 hash,模拟, 最小表示法 给定一个环状字符串,切开,使得字符串的字典序最小 图和树 割...

2614
来自专栏蜉蝣禅修之道

网络流算法Dinic的Python实现

1504
来自专栏蜉蝣禅修之道

优化后的Levensthein distance算法实现

3695
来自专栏瓜大三哥

视频压缩编码技术(H.264) 之算术编码

早在1948年,香农就提出将信源符号依其出现的概率降序排序,用符号序列累计概率的二进值作为对芯源的编码,并从理论上论证了它的优越性。1960年, Peter E...

1163
来自专栏深度学习之tensorflow实战篇

tensorflow载入数据的三种方式 之 TF生成数据的方法

Tensorflow数据读取有三种方式: Preloaded data: 预加载数据 Feeding: Python产生数据,再把数据喂给后端。 Reading...

4164
来自专栏数据科学与人工智能

Python语言做数据探索教程

本文总结Python语言做数据探索的知识。 类似R语言做数据探索,利用Python语言做数据探索。 1 数据导入 2 数据类型变换 3 数据集变换 4 数据排序...

3775
来自专栏大数据挖掘DT机器学习

python数据分析师面试题选

python数据分析部分 1. 如何利用SciKit包训练一个简单的线性回归模型 利用linear_model.LinearRegression()函数 #...

5666
来自专栏数据结构与算法

1038 一元三次方程求解

1038 一元三次方程求解 2001年NOIP全国联赛提高组 时间限制: 1 s 空间限制: 128000 KB 题目等级 : 白银 Silver ...

2848

扫码关注云+社区

领取腾讯云代金券