A*寻路

#include <algorithm>
#include <iostream>
#include <vector>
#include <math.h>

int W;
int H;
int S;

struct Position
{
	Position(int _x = 0,int _y = 0)
	{
		x = _x;
		y = _y;
	}
	int x;
	int y;
};

Position start(0,0);
Position end(19,19);


struct Node
{
	Node(Node *_pFather,Position _pos)
	{
		pFather = _pFather;
		pos = _pos;
	
		JiSuanFGH();
	}
	Node *pFather;
	Position pos;
	int f;
	int g;
	int h;
	void JiSuanFGH()
	{
		h = abs(pos.x - end.x) + abs(pos.y - end.y);
		h *= 10;
		//当前消耗
		if (pFather == NULL)
		{
			g = 0;
		}
		else
		{
			if((pos.x - pFather->pos.x) * (pos.y - pFather->pos.y) == 0)
			{
				g = pFather->g + 10;
			}
			else
			{
				g = pFather->g + 14;
			}
		}
		f = g + h;		
	}
};


bool mybigger(const Node*  a, const Node* b)
{
	return a->f < b->f;
}

void main()
{

	W = 20;
	H = 20;
	S = W * H;
	int *pMap;

	pMap = new int[S];
	memset(pMap,0,S * sizeof(int));

	int Map[400] = 
	{
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
		1,1,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
		0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,
		0,0,0,0,1,0,0,0,0,1,0,0,1,0,0,0,0,0,0,0,
		1,1,1,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,
	};

	for(int i = 0;i < S; ++i)
	{
		pMap[i] = Map[i];
	}



	std::vector<Node *> open;
	std::vector<Node *> close;

	//            上  下  左  右   左上   右上   右下   左下
	int dirx[] = {0,  0,  -1,  1,  -1,    1,    1,     -1  };
	int diry[] = {-1, 1,   0,  0,  -1,    -1,   1,     1};
	int dirnumber = sizeof(dirx)/sizeof(int);
	bool myexit = false;

	open.push_back(new Node(NULL,start));

	Node * pFind ;
	while (1)
	{
		//std::sort(open.begin(),open.end(),mybigger);
		while(open.size() > 0)
		{
			//f值最小的
			//Node * pSelect = open[open.size() - 1];
			Node * pSelect ;
			int min = 0;
			pSelect = open[min];
			
			for(int i = 0;i < open.size(); ++i)
			{
				if(pSelect->f > open[i]->f)
				{
					pSelect = open[i];
					min = i;
					
				}
			}
			open.erase(open.begin() + min);
			//open.pop_back();
			close.push_back(pSelect);
			//八个方向扩展
			for(int i = 0;i < dirnumber;++i)
			{
				int x = pSelect->pos.x + dirx[i];
				int y = pSelect->pos.y + diry[i];
				//越界不处理
				if (x < 0 || x > W - 1 || y < 0 || y > H - 1)
				{
					continue;
				}
				//障碍不处理,规定1是障碍物
				if(pMap[x + y * W] == 1)
				{
					continue;
				}
				//在关闭列表中不处理
				bool bin = false;
				for(int i = 0;i < close.size(); ++i)
				{
					if(close[i]->pos.x== x && close[i]->pos.y == y)
					{
						bin = true;
						break;
					}
				}
				if(bin)
					continue;

				
				bin = false;
				
				pFind = NULL;
				for(int i = 0;i < open.size(); ++i)
				{
					if(open[i]->pos.x== x && open[i]->pos.y == y)
					{

						bin = true;
						pFind = open[i];
						break;
					}
				}
				//在开启列表中
				if(bin)
				{
					//优化路径
					//原有的f和通过当前方法进行移动的消耗G进行比较
					int Current = pSelect->g + 10;
					if((pSelect->pos.x - x) * (pSelect->pos.y - y) != 0 )
						Current += 4;

					//发现有更优的路径
					if(Current < pFind->g)
					{
						pFind->pFather = pSelect;
						pFind->g = Current;
					}

				}
				//不在开启列表中
				else
				{
					pFind = new Node(pSelect,Position(x,y));
					open.push_back(pFind);		
				}

				if(pFind->pos.x == end.x && pFind->pos.y == end.y )
				{
					myexit = true;
					break;		
				}
			}
			if(myexit == true)
			{
				break;
			}
		}
		if(myexit == true)
		{
			break;
		}

	}
	Node *pPath = pFind;
	while(pPath !=  NULL)
	{
		pMap[pPath->pos.x + pPath->pos.y * W] = 2;
		pPath = pPath->pFather;

 	}


	char * terrent[] = {"□","■","●"};

	for(int i = 0;i < S; ++i)
	{
		std::cout<<terrent[pMap[i]];
		if(i % W == W - 1)
			std::cout<<std::endl;
	}



	
	delete [] pMap;
	pMap = NULL;

	system("pause");


	
}