astar.cpp

在LINUX下实现路径规划的程序中的主菜单界面的编辑程序。

#include "AStar.h"
#include <math.h>
//#include <iostream.h>
CAStar::CAStar()
{
	pASNode=NULL;
}

bool CAStar::FindPath(int FromID,int ToID,CWayPoint *pWayPoint)
{
	OpenList.clear();
	PathList.clear();
	int i=0;
	int FromIndex;
	int ToIndex;
	FromIndex=pWayPoint->GetIndexFromID(FromID);
    ToIndex=pWayPoint->GetIndexFromID(ToID);
	if(FromIndex<0||ToIndex<0)
		return false;
	//打开起始点
	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)
	{
		//如果所有打开的点都已被关闭,则退出,不可达
		if(OpenList.size()==0)
			return false;
		//从打开列表中找到一个F值最小的点
		int MinFIndex=OpenList[0];
		float MinF=pASNode[MinFIndex].G+pASNode[MinFIndex].H;
	    if(MinFIndex==ToIndex)
			break;
		//从打开列表中找到一个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 if(pTempChild->bIsClose)
			{
                if(pTempChild->G>pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j])
				{
					pTempChild->G=pASNode[MinFIndex].G+pASNode[MinFIndex].NeighborDistance[j];
					pTempChild->ParentIndex=MinFIndex;
				    pTempChild->bIsClose=false;
					pTempChild->bIsOpen=true;
					OpenList.push_back(pASNode[MinFIndex].Neighbor[j]);//加入打开列表
				}
			}
			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].bIsOpen=false;
		pASNode[MinFIndex].bIsClose=true;
		OpenList.erase(OpenList.begin());
        //堆排序，找出最小堆
		for (i=OpenList.size()/2-1;i>=0;i--)
		{
			int child=2*i+1;
			int tempI=OpenList[i];
			float tempF=pASNode[tempI].G+pASNode[tempI].H;
			while(child<OpenList.size())
			{
				float childF=pASNode[OpenList[child]].G+pASNode[OpenList[child]].H;
				if(child<(OpenList.size()-1))
				{
                    float childnextF=pASNode[OpenList[child+1]].G+pASNode[OpenList[child+1]].H;
				    if(childF>childnextF)
					{
					child++;
                    childF=childnextF;
					}
				}
				if(tempF<=childF)
					break;
				OpenList[i]=OpenList[child];
				i=child;
				child=2*i+1;
				OpenList[i]=tempI;
			}
		}

		//继续循环
	}
	//如果运行到此处,则说明已经到达了目标,则从目标逆推得到路径
	int tempIndex=ToIndex;
	SWPNode* PathNode=pWayPoint->GetWPNode(tempIndex);
        PathList.push_back(PathNode->ID);
	while(tempIndex!=FromIndex)
	{
		tempIndex=pASNode[tempIndex].ParentIndex;
                SWPNode* PathNode=pWayPoint->GetWPNode(tempIndex);
		PathList.push_back(PathNode->ID);
	}
        PathPointCount=PathList.size();
	//路径已经存储完毕
	return true;
}

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;
}