A星算法，找寻最短路径

#include <iostream>
#include <cstdio>
#include <queue>
#include <algorithm>
#include <cmath>
#define N 1000
#define inf 1<<30;
using namespace std;
/*
	a星算法，找寻最短路径
	算法核心：有两个表open表和close表
		将方块添加到open列表中，该列表有最小的和值。且将这个方块称为S吧。
		将S从open列表移除，然后添加S到closed列表中。
		对于与S相邻的每一块可通行的方块T：
		如果T在closed列表中：不管它。
		如果T不在open列表中：添加它然后计算出它的和值。
		如果T已经在open列表中：当我们使用当前生成的路径到达那里时，检查F（指的是和值）是否更小。如果是，更新它的和值和它的前继。

		F = G + H        (G指的是从起点到当前点的距离，而H指的是从当前点到目的点的距离（移动量估算值采用曼哈顿距离方法估算）
*/
int map[6][7];     //0表示是路，1表示有阻碍物
int xstart, ystart, xend, yend;       //(x1,y1)起点，（x2, y2)目的点
int close[6][7];  //0表示不在，1表示在
int n;
void astar(int , int );
bool check(int x, int y);
int panyical(int x, int y);
void print2();

struct point{
	int x, y;
	int f, g, h;
	int prex, prey;   //上一个点的x,y
	point(int x0, int y0, int g0, int h0)
	{
		x = x0;
		y = y0;
		g = g0;
		h = h0;
		f = g + h;
	}
	point(){
	}
}open[N];
void print1(point);        //逆向反推

void init()
{
	xstart = 3, ystart = 1;   //起点
	xend = 4, yend = 5;   //目的点
	map[4][1] = map[1][3] = map[2][3] = map[3][3] = map[4][3] = 1;   //设置阻隔物
	astar(xstart,ystart);
}

point minfpoint()
{
	int flag;
	int minf = inf;
	for(int t=0; t<n; ++t)
	{
		if(close[open[t].x][open[t].y] == 0 && open[t].f <= minf)
		{
			flag = t;
			minf = open[t].f;
		}
	}
	return open[flag];
}
void update(int x, int y, point &s)
{
	for(int t=0; t<n; ++t)
	{
		if(open[t].x == x && open[t].y == y)         //如果该点已经在open表中存在的话，则更新值
		{
			int k = s.g+1+panyical(x, y);
			if(open[t].f > k)
			{
				open[t].f = k;
				open[t].prex = s.x;
				open[t].prey = s.y;
			}
			return ;
		}
	}
	open[n] = point(x, y, s.g+1, panyical(x,y));
	open[n].prex = s.x;
	open[n].prey = s.y;
	n++;
}
void astar(int x, int y)
{
	point s = point(x, y, 0, 0);
	n = 0;
	while(1)
	{
		close[s.x][s.y] = 1;
		if(s.x == xend && s.y == yend)
		{
			break;
		}
		if(check(s.x+1, s.y))
		{
			update(s.x+1, s.y, s);
		}
		if(check(s.x-1, s.y))
		{
			update(s.x-1, s.y, s);
		}
		if(check(s.x, s.y+1))
		{
			update(s.x, s.y+1, s);
		}
		if(check(s.x, s.y-1))
		{
			update(s.x, s.y-1, s);
		}
		s = minfpoint();
	}
	printf("min:%d g:%d h:%d\n", s.f, s.g, s.h);
	//print1(s);
	print2();
}

void print1(point p)
{
	point s  = p;
	while(1)
	{
		printf("(%d,%d  g:%d, h:%d f:%d)->", s.x, s.y, s.g, s.h, s.f);
		int prex = s.prex;
		int prey = s.prey;
		if(prex == xstart && prey == ystart)
		{
			break;
		}
		for(int t=0; t<n; ++t)
		{
			if(open[t].x == prex && open[t].y == prey)
			{
				s = open[t];
				break;
			}
		}

	}
}
void print2()
{
	for(int t=0; t<n; ++t)
	{
		point s = open[t];
		printf("(%d,%d  g:%d, h:%d f:%d)\n", s.x, s.y, s.g, s.h, s.f);

	}
}
int panyical(int x, int y)
{
	return abs(xend-x) + abs(yend-y);
}

bool check(int x, int y){
	if(x<0 || x>5 || y<0 || y>6 || map[x][y]==1 || close[x][y]==1)
	{
		return false;
	}
	return true;
}
int main()
{
	init();
	return 0;
}

运行结果:

版权声明：本文为博主原创文章，未经博主允许不得转载。