waypoint.cpp

A*算法

// WayPoint.cpp: implementation of the CWayPoint class.
//
//////////////////////////////////////////////////////////////////////

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

CWayPoint::CWayPoint()
{

}

void CWayPoint::ClearNode()
{
	std::vector<SWPNode*>::iterator it=pNodeVector.begin();
	std::vector<SWPNode*>::iterator itEnd=pNodeVector.end();
	for(;it!=itEnd;++it)
	{
		delete((SWPNode*)(*it));
		*it=NULL;
	}
	pNodeVector.clear();
}

bool CWayPoint::ReadWayPointFile(char *fileName)
{
	int i=0;
	int NodeCount=0;
	FILE *fp=NULL;
	fp=fopen(fileName,"r");
	if(fp==NULL)
		return false;
	ClearNode();
	char s[1024];
	fgets(s,1024,fp);
	sscanf(s,"关键点数量=%d",&NodeCount);
	for(i=0;i<NodeCount;i++)
	{
		int x,y;
		int ID;
		fgets(s,1024,fp);
		sscanf(s,"%d x=%d;y=%d",&ID,&x,&y);
		AddNode(x,y,ID);
		int NeighborCount;
		fgets(s,1024,fp);
		sscanf(s,"  由此出发共可到达%d个点",&NeighborCount);
		for(int j=0;j<NeighborCount;j++)
		{
			fgets(s,1024,fp);
			int ChildID,dist;
			sscanf(s,"  (%d)=%d",&ChildID,&dist);
			AddNeighbor(i,ChildID,dist);
		}
	}
	fclose(fp);
	return true;
}

int CWayPoint::AddNode(int x,int y,int ID)
{
	SWPNode *pTempNode=new SWPNode;
	pTempNode->x=x;
	pTempNode->y=y;
	pTempNode->ID=ID;
	pTempNode->NeighborCount=0;
	pNodeVector.push_back(pTempNode);
	return pNodeVector.size()-1;
}

int CWayPoint::GetNewID()
{
	int maxID=0;
	std::vector<SWPNode*>::iterator it=pNodeVector.begin();
	std::vector<SWPNode*>::iterator itEnd=pNodeVector.end();
	for(;it!=itEnd;++it)
	{
		if(((SWPNode*)(*it))->ID>maxID)
			maxID=((SWPNode*)(*it))->ID;
	}
	return maxID+1;	
}

bool CWayPoint::RemoveNode(int Index)
{
	//先移除他与邻居的连接
	for(int i=0;i<GetNodeCount();i++)
	{
		if(i!=Index)
		{
			RemoveNeighbor(Index,GetWPNode(i)->ID);
			RemoveNeighbor(i,GetWPNode(Index)->ID);
		}
	}
	pNodeVector.erase(pNodeVector.begin()+Index);
	return true;
}

int CWayPoint::AddNeighbor(int ParentIndex,int ChildID,int dist)
{
	SWPNode *pTempNode=pNodeVector[ParentIndex];
	//如果已经添加过这个项，就不再添加
	for(int i=0;i<pTempNode->NeighborCount;i++)
	{
		if(pTempNode->Neighbor[i]==ChildID)
			return -1;
	}
	//添加一个邻居
	if(pTempNode->NeighborCount<10)
	{
		pTempNode->Neighbor[pTempNode->NeighborCount]=ChildID;
		pTempNode->NeighborDistance[pTempNode->NeighborCount]=dist;
		pTempNode->NeighborCount++;
		return pTempNode->NeighborCount-1;
	}
	else
	{
		return -1;
	}
}

bool CWayPoint::RemoveNeighbor(int ParentIndex,int ChildID)
{
	SWPNode *pTempNode=pNodeVector[ParentIndex];
	//移除一个邻居
	for(int i=0;i<pTempNode->NeighborCount;i++)
	{
		if(pTempNode->Neighbor[i]==ChildID)	
		{
			for(int j=1;j<pTempNode->NeighborCount;j++)
			{
				pTempNode->Neighbor[i+j-1]=pTempNode->Neighbor[i+j];
				pTempNode->NeighborDistance[i+j-1]=pTempNode->NeighborDistance[i+j];
			}
			pTempNode->NeighborCount--;	
			i--;
		}

	}

	return true;
}

int CWayPoint::GetNodeCount()
{
	return pNodeVector.size();
}

SWPNode *CWayPoint::GetWPNode(int NodeIndex)
{
	return pNodeVector[NodeIndex];
}

SWPNode *CWayPoint::GetWPNodeByID(int NodeID)
{
	std::vector<SWPNode*>::iterator it=pNodeVector.begin();
	std::vector<SWPNode*>::iterator itEnd=pNodeVector.end();
	for(;it!=itEnd;++it)
	{
		if(((SWPNode*)(*it))->ID==NodeID)
			return (SWPNode*)(*it);
	}
	return NULL;
}
int CWayPoint::GetIndexFromID(int NodeID)
{
	for(int i=0;i<pNodeVector.size();i++)
	{
		if(pNodeVector[i]->ID==NodeID)
			return i;
	}
	return -1;
}

int CWayPoint::GetNearestIndex(int x,int y)
{
	int minDist=9999999;
	int minI=0;
	for(int i=0;i<pNodeVector.size();i++)
	{
		int tempDis=sqrt(pow((x-pNodeVector[i]->x),2)+pow(y-pNodeVector[i]->y,2));
		if(tempDis<minDist)
		{
			minI=i;
			minDist=tempDis;
		}
	}
	return minI;	
}

bool CWayPoint::SaveWayPointToFile(char *fileName)
{
	int i=0;
	int NodeCount=GetNodeCount();
	if(NodeCount==0)
		return false;
	FILE *fp=NULL;
	fp=fopen(fileName,"w");
	char s[1024];
	sprintf(s,"关键点数量=%d\n",NodeCount);
	fputs(s,fp);
	for(i=0;i<NodeCount;i++)
	{
		int x,y;
		x=GetWPNode(i)->x;
		y=GetWPNode(i)->y;
		sprintf(s,"%d x=%d;y=%d\n",GetWPNode(i)->ID,x,y);
		fputs(s,fp);
		int NeighborCount=GetWPNode(i)->NeighborCount;
		sprintf(s,"  由此出发共可到达%d个点\n",NeighborCount);
		fputs(s,fp);
		for(int j=0;j<NeighborCount;j++)
		{
			int ChildIndex,dist;
			ChildIndex=GetWPNode(i)->Neighbor[j];
			dist=GetWPNode(i)->NeighborDistance[j];
			sprintf(s,"  (%d)=%d\n",ChildIndex,dist);
			fputs(s,fp);
		}
	}
	fclose(fp);
	return true;
}

CWayPoint::~CWayPoint()
{

}