astar.cpp

A*算法

// AStar.cpp: implementation of the CAStar class.
//
//////////////////////////////////////////////////////////////////////

#include "AStar.h"
#include <math.h>
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


CAStar::CAStar()
{
	pASNode=NULL;
}

int CAStar::GetNodeIndex(int pathIndex)
{
	return PathList[PathList.size()-pathIndex-1];
}

bool CAStar::FindPath(int FromIndex,int ToIndex)
{
	OpenList.clear();
	PathList.clear();
	int i=0;
	//打开起始点
	pASNode[FromIndex].bIsOpen=true;
	pASNode[FromIndex].G=0;
	OpenList.push_back(FromIndex);
	//计算所有的H值
	for(i=0;i<NodeCount;i++)
	{
		pASNode[i].H=sqrt(pow(pASNode[i].x-pASNode[ToIndex].x,2)+
					      pow(pASNode[i].y-pASNode[ToIndex].y,2));
	}
	while(1)
	{
		bool bFoundPath=false;
		int NotClosedCount=0;
		for(i=0;i<OpenList.size();i++)
		{
			//遍历所有打开并且没有关闭的点,
			if(!pASNode[OpenList[i]].bIsClose)
			{
				//如果打开的点是目标点,则退出循环,到达
				if(OpenList[i]==ToIndex)
					bFoundPath=true;//找到目标点
				NotClosedCount++;
			}
		}

		//如果所有打开的点都已被关闭,则退出,不可达
		if(NotClosedCount==0)
			return false;
		//从打开列表中找到一个F值最小的点
		int MinFIndex=-1;
		int MinF=99999999;
		for(i=0;i<OpenList.size();i++)
		{
			//遍历所有打开并且没有关闭的点,寻找最小的F值
			int tempI=OpenList[i];
			if(!pASNode[tempI].bIsClose)
			{
				int tempF=pASNode[tempI].G+pASNode[ToIndex].H;
				if(tempF<MinF)
				{
					MinF=tempF;
					MinFIndex=tempI;
				}
			}
		}
		//从打开列表中找到一个F值最小的点,把他的邻居全打开,把自己关闭,设置parent
		//并查看邻居,看自己做邻居的父结点是否能够获得更小的G值		
		for(int j=0;j<pASNode[MinFIndex].NeighborCount;j++)
		{
			SASNode *pTempChild=&pASNode[pASNode[MinFIndex].Neighbor[j]];
			if(pTempChild->bIsOpen)//如果是打开的邻居,看自己是否可以做它的父结点
			{
				if(pTempChild->G>pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j])
				{
					pTempChild->G=pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j];
					pTempChild->ParentIndex=MinFIndex;
				}
			}
			else
			{
				pTempChild->bIsOpen=true;
				OpenList.push_back(pASNode[MinFIndex].Neighbor[j]);//加入打开列表
				pTempChild->ParentIndex=MinFIndex;
				pTempChild->G=pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j];
			}
		}
		pASNode[MinFIndex].bIsClose=true;
		if(bFoundPath)
			break;
		//继续循环
	}
	//如果运行到此处,则说明已经到达了目标,则从目标逆推得到路径
	int tempIndex=ToIndex;
	PathList.push_back(tempIndex);
	while(tempIndex!=FromIndex)
	{
		tempIndex=pASNode[tempIndex].ParentIndex;
		PathList.push_back(tempIndex);
	}
	//路径已经存储完毕
	PathPointCount=PathList.size();
	return true;
}

int CAStar::GetNodeCount()
{
	return PathPointCount;
}

void CAStar::InitNodeByWayPoint(CWayPoint *pWayPoint)
{
	if(pASNode)
	{
		delete[] pASNode;
		pASNode=NULL;
	}
	NodeCount=pWayPoint->GetNodeCount();
	pASNode=new SASNode[NodeCount];
	for(int i=0;i<NodeCount;i++)
	{
		SWPNode* wpNode=pWayPoint->GetWPNode(i);
		pASNode[i].x=wpNode->x;
		pASNode[i].y=wpNode->y;
		pASNode[i].bIsClose=false;
		pASNode[i].bIsOpen=false;
		pASNode[i].F=0;
		pASNode[i].G=0;
		pASNode[i].ParentIndex=-1;
		pASNode[i].NeighborCount=0;
		for(int j=0;j<wpNode->NeighborCount;j++)
		{
			pASNode[i].NeighborCount=wpNode->NeighborCount;
			pASNode[i].Neighbor[j]=pWayPoint->GetIndexFromID(wpNode->Neighbor[j]);
			pASNode[i].NeighborDistance[j]=wpNode->NeighborDistance[j];
		}
	}
}

CAStar::~CAStar()
{
	delete[] pASNode;
	pASNode=NULL;
}