astarpathfind.cpp

演示一个寻找最短路径的算法。

#include "StdAfx.h"
#include "AStarPathFind.h"

CAStarPathFind::CAStarPathFind(int xStart,int yStart,int xEnd,int yEnd,int xSpeed,int ySpeed)
:m_xStart(xStart),m_yStart(yStart),m_xEnd(xEnd),m_yEnd(yEnd),m_xSpeed(xSpeed),m_ySpeed(ySpeed)
{
//	m_OPEN	=	NULL;
//	m_CLOSED=	NULL;
	m_Map	=	NULL;
	m_ptPath=	NULL;
}

CAStarPathFind::CAStarPathFind(POINT ptStart,POINT ptEnd,int xSpeed,int ySpeed)
:m_xStart(ptStart.x),m_yStart(ptStart.y),m_xEnd(ptEnd.x),m_yEnd(ptEnd.y),m_xSpeed(xSpeed),m_ySpeed(ySpeed)
{
//	m_OPEN	=	NULL;
//	m_CLOSED=	NULL;
	m_Map	=	NULL;
}

CAStarPathFind::~CAStarPathFind()
{
//	if	(m_OPEN != NULL)
//		FreeLink(m_OPEN);

//	if	(m_CLOSED != NULL)
//		FreeLink(m_CLOSED);

	//m_Map不应由这个类来释放，由主程序来释放
	//m_ptPath也由主程序来释放
}

//评估函数
inline int CAStarPathFind::H(int x, int y)
{
	return (m_xEnd-x)*(m_xEnd-x)+(m_yEnd-y)*(m_yEnd-y);
}

//计算八个方向的坐标点
inline void CAStarPathFind::ExpandPoint(POINT *pt, int x, int y)
{
	assert(pt != NULL);

	pt[0].x = x - m_xSpeed;	pt[0].y = y - m_ySpeed;
	pt[1].x = x;			pt[1].y = y - m_ySpeed;
	pt[2].x = x + m_xSpeed;	pt[2].y = y - m_ySpeed;
	pt[3].x = x - m_xSpeed;	pt[3].y = y;
	pt[4].x = x + m_xSpeed;	pt[4].y = y;
	pt[5].x = x - m_xSpeed;	pt[5].y = y + m_ySpeed;
	pt[6].x = x;			pt[6].y = y + m_ySpeed;
	pt[7].x = x + m_xSpeed;	pt[7].y = y + m_ySpeed;
}

//计算最佳路径，放到pt数组中
void CAStarPathFind::MakePath(NODE *pHead)
{
	assert(pHead != NULL);

	NODE *pNext=pHead;

	m_ptPath	=	(POINT*)malloc(sizeof(POINT)*pHead->g);
	POINT	*pt=m_ptPath;
	while (pHead != NULL)
	{
		(*pt).x = pHead->x;
		(*pt).y = pHead->y;
		pt++;
		pHead = pHead->pParent;
	}
}

//释放链表
void CAStarPathFind::FreeLink(NODE *pHead)
{
	NODE	*pNode;

	while (pHead != NULL)
	{
		pNode	=	pHead;
		pHead	=	pHead->pNext;
		free(pNode);
	}
}

//主函数，最佳路径保存在数组ptPath中，并返回数组大小，即结点数
int  CAStarPathFind::PathFind(POINT **pptPath)
{
//	assert(m_OPEN	== NULL);
//	assert(m_CLOSED	== NULL);
	assert(m_Map	!= NULL);

	NODE *pNode, *pPre, *pNext, *pOpenHead, *pClosedHead;

	//初始化OPEN，CLOSED表，并把起始点放入OPEN表中
	pOpenHead = (NODE*)malloc(sizeof(NODE));
	pNext	=	(NODE*)malloc(sizeof(NODE));
	pNext->x = m_xStart;
	pNext->y = m_yStart;
	pNext->g = 1;
	pNext->h = H(m_xStart, m_yStart);
	pNext->pParent	=	NULL;
	pNext->pNext	=	NULL;
	pOpenHead->pNext=	pNext;
	pOpenHead->pParent	=	NULL;

	pClosedHead = (NODE*)malloc(sizeof(NODE));
	pClosedHead->pParent	=	NULL;
	pClosedHead->pNext	=	NULL;

	while	(pOpenHead->pNext != NULL)
	{
		//取出OPEN表中第一个结点，并判断是否是终点，是则结束，否则继续
		pNode	=	pOpenHead->pNext;
		if	(pNode->x == m_xEnd && pNode->y == m_yEnd)
		{
			int	nNum;
			nNum = pNode->g;
			MakePath(pNode);
			*pptPath	=	m_ptPath;
			FreeLink(pOpenHead);
			pOpenHead	=	NULL;
			FreeLink(pClosedHead);
			pClosedHead	=	NULL;

			return	nNum;
		}

		bool	bFlag;
		POINT	pt[8];
		ExpandPoint(pt, pNode->x, pNode->y);	//计算八个方向的坐标点
		for	(int i=0; i<8; i++)
		{
			int	x	=	TileX(pt[i].x);
			int	y	=	TileY(pt[i].y);

			//没有检查从两个障碍块中穿过去的情况
			if	(x < 0 || y < 0 || x >= m_nMapWidth || y >= m_nMapHeight ||	//超出地图范围
				 m_Map[y*m_nMapWidth + x] == 1)	//地图上不可通过，与地图数据结构有关
				continue;

			int g=pNode->g+1;
			int h=H(x, y);	//计算评估值

			bFlag=FALSE;
			pPre = pNode;		//遍历OPEN表，看是否有相同的
			pNext= pNode->pNext;
			while (pNext != NULL)
			{
				if	(pNext->x == x && pNext->y == y)
				{
					if	(pNext->g <= g)
					{
						bFlag = TRUE;
						break;
					}
					else
					{
						pPre->pNext = pNext->pNext;
						free(pNext);
						break;
					}
				}
				pPre = pNext;
				pNext= pNext->pNext;
			}

			if	(!bFlag)	//如果OPEN表没有重复的，则查看CLOSED表看有无重复的
			{
				pPre = pClosedHead;
				pNext= pPre->pNext;
				while (pNext != NULL)
				{
					if	(pNext->x == x && pNext->y == y)
					{
						if	(pNext->g <= g)
						{
							bFlag = TRUE;
							break;
						}
						else
						{
							pPre->pNext = pNext->pNext;
							free(pNext);
							break;
						}
					}
					pPre = pNext;
					pNext= pNext->pNext;
				}
			}

			if	(!bFlag)	//如果两个表都没有重复的
			{
				NODE	*pPoint = (NODE*)malloc(sizeof(NODE));
				pPoint->x	=	x;
				pPoint->y	=	y;
				pPoint->g	=	g;
				pPoint->h	=	h;
				pPoint->pParent	=	pNode;

				pPre = pNode;
				pNext= pNode->pNext;
				while (pNext != NULL)
				{
					if	((pNext->g + pNext->h) > (g+h))
					{
						pPre->pNext		=	pPoint;
						pPoint->pNext	=	pNext;
						break;
					}
					pPre	=	pNext;
					pNext	=	pNext->pNext;
				}

				if	(pNext == NULL)
				{
					pPre->pNext	=	pPoint;
					pPoint->pNext	=	NULL;
				}
			}
		}

		//把这个最佳点从OPEN表中取出，放入CLOSED表中
		pOpenHead->pNext	=	pNode->pNext;
		pNode->pNext		=	pClosedHead->pNext;
		pClosedHead->pNext	=	pNode;
	}

	return	0;
}

/*以下与地图坐标计算有关，以后再修改，改为CMap *map*/
void CAStarPathFind::SetMapData(int *Map,int nMapWidth,int nMapHeight,int nTileWidth,int nTileHeight)
{
	assert(Map != NULL);

	if	(m_Map != NULL)
		free(m_Map);

	m_Map			=	Map;
	m_nMapWidth		=	nMapWidth;
	m_nMapHeight	=	nMapHeight;
	m_nTileWidth	=	nTileWidth;
	m_nTileHeight	=	nTileHeight;
}

inline int  CAStarPathFind::TileX(int x)
{
	return	x/m_nTileWidth;
}

inline int  CAStarPathFind::TileY(int y)
{
	return	y/m_nTileHeight;
}
/*************************************************/