waypoint.cpp

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

#include "WayPoint.h"
#include <math.h>

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();
	fscanf(fp,"关键点数量=%d\n",&NodeCount);
	for(i=0;i<NodeCount;i++)
	{
	    int x,y;
		int ID;
		fscanf(fp,"%d x=%d;y=%d\n",&ID,&x,&y);
		AddNode(x,y,ID);
		int NeighborCount;
		fscanf(fp,"  由此出发共可到达%d个点\n",&NeighborCount);
		for(int j=0;j<NeighborCount;j++)
		{
			int ChildID;
			float dist;
			fscanf(fp,"  (%d)=%f\n",&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::AddNeighbor(int ParentIndex,int ChildID,float 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;
	}
}

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

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

int CWayPoint::GetIndexFromID(int NodeID)
{
	for(int i=0;i<pNodeVector.size();i++)
	{
		if(pNodeVector[i]->ID==NodeID)
			return i;
	}
	return -1;
}

CWayPoint::~CWayPoint()
{

}