astarpathfinder.cpp

用VC6++对A*寻路算法的简单实现,在界面上点击鼠标就行.

// AstarPathFinder.cpp: implementation of the CAstarPathFinder class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "AstarPathFinder.h"
#include <malloc.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CAstarPathFinder::CAstarPathFinder()
{
	Stack = (STACK*)calloc(1,sizeof(STACK));
	isPath = false;
	OPEN = NULL;
	CLOSED = NULL;
	PATH = NULL;
	m_nMap = new int[ROWCOUNT*COLCOUNT];
	for(int i=0;i<ROWCOUNT*COLCOUNT;i++)
	{
		m_nMap[i] = 0;
	}
}

CAstarPathFinder::~CAstarPathFinder()
{
	FreeNodes();
	if(Stack)
		free(Stack);
	//*
	if(m_nMap)
		delete[] m_nMap;//*/

}

BOOL CAstarPathFinder::FindPath(int sx, int sy, int dx, int dy)
{
	NODE *pNode,*pBestNode;
	int TileNumDest = GetMapIndex(sx,sy);
	//生成Open和Closed表
	OPEN = (NODE*) calloc(1,sizeof(NODE));
	CLOSED = (NODE*) calloc(1,sizeof(NODE));
	//生成起始节点，并放入Open表中
	pNode = (NODE*) calloc(1,sizeof(NODE));
	pNode->g = 0;
	//计算h值
	pNode->h = (dx-sx)*(dx-sx) + (dy-sy)*(dy-sy);
	//计算f值，即估价值
	pNode->f = pNode->g + pNode->h;
	//计算该点在表中的位置
	pNode->NodeNum = GetMapIndex(dx,dy);
	//记录该点的坐标
	pNode->x = dx;
	pNode->y = dy;

	OPEN->pNextNode = pNode;
	for(;;)
	{
		//从Open表中取得一个估价值最好的节点
		pBestNode = ReturnBestNode();
		//路径不存在，返回
		if(pBestNode == NULL)
			return FALSE;
		//如果该节点是目标节点就退出
		if(pBestNode->NodeNum == TileNumDest)
			break;
		//否则生成子节点
 		GenerateChildren(pBestNode,sx,sy);
	}
	//保存PATH路径
	PATH = pBestNode;

	return TRUE;
}
//判断是否在CLOSED表中
NODE *CAstarPathFinder::CheckCLOSED(int tileNum)
{	
	NODE *pTmp;

	pTmp = OPEN->pNextNode;
	while( pTmp != NULL )
	{
		if(pTmp->NodeNum == tileNum)
			return pTmp;
		else
			pTmp = pTmp->pNextNode;
	}

	return NULL;
}
//判断是否在OPEN表中
NODE *CAstarPathFinder::CheckOPEN(int tileNum)
{
	NODE *pTmp;
	pTmp = CLOSED->pNextNode;

	while( pTmp !=	NULL )
	{
		if(pTmp->NodeNum == tileNum)
			return pTmp;
		else
			pTmp = pTmp->pNextNode;
	}

	return NULL;
}
//选择一个最佳的点
NODE *CAstarPathFinder::ReturnBestNode()
{
	NODE *pNode;

	if(OPEN->pNextNode == NULL)
	{
		AfxMessageBox("路径不存在!");
		return NULL;
	}
	
	pNode = OPEN->pNextNode;
	OPEN->pNextNode = pNode->pNextNode;

	pNode->pNextNode = CLOSED->pNextNode;
	CLOSED->pNextNode = pNode;

	return pNode;
}

void CAstarPathFinder::FreeNodes()
{
	NODE *Node, *OldNode;
	//删除OPEN表中的数据
	if ( OPEN != NULL )
	{
		Node = OPEN->pNextNode;
		while ( Node != NULL )
		{
			OldNode = Node;
			Node = Node->pNextNode;
			free(OldNode);
		}
	}
	//删除CLOSED表中的数据
	if ( CLOSED != NULL )
	{
		Node = CLOSED->pNextNode;
		while ( Node != NULL )
		{
			OldNode = Node;
			Node = Node->pNextNode;
			free(OldNode);
		}
	}
}

void CAstarPathFinder::GenerateChildren(NODE *pBestNode, int dx, int dy)
{
	int x=0,y=0;
	//左上,判断左上子节点是否有障碍物,并且判断左上节点是否不在范围内
	if(FreeTile(x=pBestNode->x-TITLE, y=pBestNode->y-TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,14);
	//上,判断上子节点是否有障碍物,并且判断上节点是否不在范围内
   if(FreeTile(x=pBestNode->x, y=pBestNode->y-TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,10);
	//右上,判断右上子节点是否有障碍物,并且判断右上节点是否不在范围内
   if(FreeTile(x=pBestNode->x+TITLE, y=pBestNode->y-TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,14);
	//右,判断右子节点是否有障碍物,并且判断右节点是否不在范围内
   if(FreeTile(x=pBestNode->x+TITLE, y=pBestNode->y)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,10);
	//右下,判断右下子节点是否有障碍物,并且判断右下节点是否不在范围内
   if(FreeTile(x=pBestNode->x+TITLE, y=pBestNode->y+TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,14);
	//下,判断下子节点是否有障碍物,并且判断下节点是否不在范围内
   if(FreeTile(x=pBestNode->x, y=pBestNode->y+TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,10);
	//左下,判断左下子节点是否有障碍物,并且判断左下节点是否不在范围内
   if(FreeTile(x=pBestNode->x-TITLE, y=pBestNode->y+TITLE)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,14);
	//左,判断左子节点是否有障碍物,并且判断左节点是否不在范围内
   if(FreeTile(x=pBestNode->x-TITLE, y=pBestNode->y)&&x>=0&&y>=0&&x<ROWCOUNT&&y<COLCOUNT)
      GenerateChild(pBestNode,x,y,dx,dy,10);
}

void CAstarPathFinder::GenerateChild(NODE *pBestNode,int x,int y,int dx,int dy,int step)
{
	NODE *pOld,*pNew;
	int g, TileNumS;
	//计算子节点的 g 值
	g = pBestNode->g+step;
	TileNumS = GetMapIndex(x,y);
	//判断子节点是否在Open表中
	if((pOld = CheckOPEN(TileNumS)) != NULL)
	{
		for(int i=0; i<8; i++)
		{
			if(pBestNode->Child[i] == NULL)
				break;
			pBestNode->Child[i] = pOld;
		}
		//比较Open表中的估价值和当前的估价值
		if( g < pOld->g )
		{
			//当前的估价值小就更新Open表中的估价值
			pOld->pParent = pBestNode;
			pOld->g = g;
	   		pOld->f = g + pOld->h;
		}
	}
	else if((pOld=CheckCLOSED(TileNumS)) != NULL)//判断子节点是否在CLOSED表中
	{
		for(int i=0; i<8; i++)
		{
			if(pBestNode->Child[i] == NULL)
				break;
			pBestNode->Child[i] = pOld;
		}

		if( g < pOld->g )
		{
			pOld->pParent = pBestNode;
			pOld->g = g;
			pOld->f = g + pOld->h;
			//依次更新pOld的所有子节点的估价值
			PropagateDown(pOld,step);
		}
	}
	else  //不在Open表中也不在Close表中									
	{
		//生成新的节点
		pNew = (NODE*)calloc(1,sizeof(NODE));
		pNew->pParent = pBestNode;
		pNew->g = g;
		pNew->h = abs(x-dx) + abs(y-dy);
		pNew->f = pNew->g + pNew->h;
		pNew->x = x;
		pNew->y = y;
		pNew->NodeNum = TileNumS;
		//再插入Open表中，同时排序
		Insert(pNew);
		for(int i=0; i<8; i++)
		{
			if(pBestNode->Child[i] == NULL)
				break;
			pBestNode->Child[i] = pNew;
		}
	}
}
//更新所有子节点的估价值
void CAstarPathFinder::PropagateDown(NODE *pOld,int step)
{
	int g;
	NODE *pChild, *pFather;

	g = pOld->g;
	for(int i=0; i<8; i++)
	{
		if((pChild = pOld->Child[i]) == NULL)
			break;
		if(g+step < pChild->g)
		{
			pChild->g = g+step;
			pChild->f = pChild->g + pChild->h;
			pChild->pParent = pOld;
			Push(pChild);
		}
	}

	while(Stack->NextStackPtr != NULL)
	{
		pFather = Pop();
		for(i=0; i<8; i++)
		{
			if((pChild = pFather->Child[i]) == NULL)
				break;
			if(pFather->g+step < pChild->g)
			{
				pChild->g = pFather->g;
				pChild->f = pChild->g + pChild->h;
				pChild->pParent = pFather;
				Push(pChild);
			}
		}
	}
}

void CAstarPathFinder::Push(NODE *pNode)
{
	STACK *tmp;

    tmp =( STACK* )calloc(1,sizeof( STACK ));
    tmp->NodePtr = pNode;
    tmp->NextStackPtr = Stack->NextStackPtr;
    Stack->NextStackPtr = tmp;
}

NODE *CAstarPathFinder::Pop()
{
	NODE *tmp;
    STACK *tmpSTK;

    tmpSTK = Stack->NextStackPtr;
    tmp = tmpSTK->NodePtr;

    Stack->NextStackPtr = tmpSTK->NextStackPtr;

    free(tmpSTK);

    return(tmp);
}
//插入新节点，并排序
void CAstarPathFinder::Insert(NODE *pNew)
{
	NODE *tmp1, *tmp2;
	int f;

	if ( OPEN->pNextNode == NULL )
	{
		OPEN->pNextNode = pNew;
		return;
	}

	f = pNew->f;
	tmp1 = OPEN;
	tmp2 = OPEN->pNextNode;

	while ( (tmp2 != NULL) && (tmp2->f < f) )
	{
		tmp1 = tmp2;
		tmp2 = tmp2->pNextNode;
	}

	pNew->pNextNode = tmp2;
	tmp1->pNextNode = pNew;
}
//获取该点在地图中的位置
int CAstarPathFinder::GetMapIndex(int x, int y)
{	
	return (y*ROWCOUNT+x);
}
//判断该点是否有障碍物
int CAstarPathFinder::FreeTile(int x,int y)
{
	return(!m_nMap[y*ROWCOUNT+x]);
}
//创建一条路径
BOOL CAstarPathFinder::NewPath(int sx,int sy,int dx,int dy)
{
	if ( FreeTile(dx,dy)&&FreeTile(sx,sy) && (GetMapIndex(sx,sy)!=GetMapIndex(dx,dy)) )
    {
		FreeNodes();
		if(FindPath(sx,sy,dx,dy))
			return (isPath=TRUE);
		else
			return (isPath=FALSE);
    }
    else
  		return (isPath=FALSE);
}
//初始化地图
void CAstarPathFinder::InitMap(const int *pMap)
{
	for(int i=0;i<ROWCOUNT*COLCOUNT;i++)
	{
		m_nMap[i] = pMap[i];
	}
}