dijkstrasearch.cpp

Dijkstra算法及demo实现

#include "StdAfx.h"
#include "DijkstraSearch.h"

// 计算两点之间的距离(米)
#define DEG2RAD(X) (X / 57.2957795131)

double DegreesToMeters(double x1, double y1, double x2, double y2)
{
   if(x1==x2 && y1==y2) return 0.0;

   double a;//Use GRS 80 spheroid
   double f;//1 / 298.257
   a = 6378137.0;
   f = 0.003352813;

   double F1,G,l;
   F1 = (DEG2RAD(y1) + DEG2RAD(y2)) / 2.0;
   G = (DEG2RAD(y1) - DEG2RAD(y2)) / 2.0;
   l = (DEG2RAD(x1) - DEG2RAD(x2)) / 2.0;

   double sF2,cF2,sG2,cG2,sL2,cL2;
   sF2 = sin(F1) * sin(F1);
   cF2 = cos(F1) * cos(F1);
   sG2 = sin(G) * sin(G);
   cG2 = cos(G) * cos(G);
   sL2 = sin(l) * sin(l);
   cL2 = cos(l) * cos(l);

   double S,C;
   S = sG2 * cL2 + cF2 * sL2;
   C = cG2 * cL2 + sF2 * sL2;
	
   double omega,rho;
   omega = atan(sqrt(S / C));
   rho = sqrt(S * C) / omega;

   double d;
   d = 2 * a * omega;
	
   double H1,H2;
   H1 = (3 * rho - 1) / (2 * C);
   H2 = (3 * rho + 1) / (2 * S);

   double m;
   m = d * (1 + f * (H1 * sF2 * cG2 - H2 * cF2 * sG2));

   return (m * 0.0006214 * 1609);//乘1609将英里转换成米
}
/////////////////////////////////////////////////////////////////////////////////////////////

CDijkstraSearch::CDijkstraSearch(void)
{
	m_pMemObjectList=NULL;
	m_nObjectCount=0;
}

CDijkstraSearch::~CDijkstraSearch(void)
{
}

bool CDijkstraSearch::ConnectObject(MemSearchObject *pFirst,MemSearchObject *pSecond,bool bBoth)
{
	if(pFirst==NULL || pSecond==NULL) return false;
	pFirst->m_pNextMemObjectList.PushNode(pSecond);
	if(bBoth)
		pSecond->m_pNextMemObjectList.PushNode(pFirst);
	return true;
}

bool CDijkstraSearch::InitSearch(MemSearchObject *pMemObjectList,long nObjectCount)
{
	if(pMemObjectList==NULL || nObjectCount<=0) return false;
	m_pMemObjectList=pMemObjectList;
	m_nObjectCount=nObjectCount;
	return true;
}

bool CDijkstraSearch::StartSearch(MemSearchObject *pBeginObject,MemSearchObject *pEndObject,int nType,SearchObjectList *pSearchResult)
{
	if(pBeginObject==NULL || pEndObject==NULL || pSearchResult==NULL)
		return false;

	if(m_pMemObjectList==NULL || m_nObjectCount<=0) 
		return false;

	MemSearchResultList pMemResultList;//已搜索完成的路径列表
	long nFirstResultIndex=0;//已搜索结果pMemResultList中第一个有剩余邻结点的序号
	ValueType vNoUseValue,vMemValue; 
	
	//临时变量
	MemSearchObjectList *pMemSearchObjectList=NULL;
	MemSearchObject     *pMemSearchObject=NULL;
	register long i,j;
	long nCount=0;
	long jj,nCount2;

	//将所有的结点加入到没有参与搜索的结点
	for(i=0;i<m_nObjectCount;i++)
	{
		m_pMemObjectList[i].m_pLeftNextMemObjectList.RemoveAllNode();
		long n=m_pMemObjectList[i].m_pNextMemObjectList.GetNodeCount();
		for(long m=0;m<n;m++)
		{
			m_pMemObjectList[i].m_pLeftNextMemObjectList.PushNode(m_pMemObjectList[i].m_pNextMemObjectList.GetNode(m));
		}
	}

	//开始搜索
	while(1)
	{
		//从未使用的结点中，找出最短的 pNoUseLessObject
		MemSearchObject *pMemNoUseLessObject=NULL;
		vNoUseValue=OBJECT_MAXVALUE;
		if(pBeginObject->m_pLeftNextMemObjectList.GetNodeCount()>0)//如果首结点还有未使用的相邻结点
		{
			pMemSearchObjectList=&pBeginObject->m_pLeftNextMemObjectList;
			nCount=pMemSearchObjectList->GetNodeCount();
			for(j=0;j<nCount;j++)
			{
				pMemSearchObject=pMemSearchObjectList->GetNode(j);
				ValueType vTmp=pMemSearchObject->m_OwnerObject.GetValue(pBeginObject->m_OwnerObject,nType);
				if(vTmp<vNoUseValue)
				{	
					vNoUseValue=vTmp;
					pMemNoUseLessObject=pMemSearchObject;
				}
			}
		}

		//从已搜索完成的列表中，找出最短的 pMemLessObject
		MemSearchObject *pMemLessObject=NULL;
		vMemValue=OBJECT_MAXVALUE;
		
		//遍历pMemResultList
		MemSearchResult *pMemResult=NULL;
		MemSearchResult *pParentMemSearchObject=NULL;
		nCount=pMemResultList.GetNodeCount();

		//从有剩余邻结结点的结果查找，nFirstResultIndex表示可能有邻结结点的起始位置
		for(j=nFirstResultIndex;j<nCount;j++)
		{
			pMemResult=pMemResultList.GetNode(j);
			if(pMemResult->m_pMemObject->m_pLeftNextMemObjectList.GetNodeCount()<=0) 
			{
				if(j==nFirstResultIndex+1) nFirstResultIndex=j;
				continue;
			}

			//遍历每个最短路线，找下一结点最短路线
			pMemSearchObjectList=&pMemResult->m_pMemObject->m_pLeftNextMemObjectList;
			nCount2=pMemSearchObjectList->GetNodeCount();

			for(jj=0;jj<nCount2;jj++)
			{
				pMemSearchObject=pMemSearchObjectList->GetNode(jj);
				pMemResult->m_pMemObject->m_OwnerObject;
				ValueType vTmp=pMemSearchObject->m_OwnerObject.GetValue(pMemResult->m_pMemObject->m_OwnerObject,nType);
				vTmp+=pMemResult->m_vValue;

				if(vTmp<vMemValue)
				{
					vMemValue=vTmp;
					pParentMemSearchObject=pMemResult;
					pMemLessObject=pMemSearchObject;
				}
			}
		}

		//如果没有了可用的结点，就退出
		if(vNoUseValue==OBJECT_MAXVALUE && vMemValue==OBJECT_MAXVALUE)
			break;

		//加入到pMemResultList
		if(vNoUseValue<vMemValue)
		{
			//加了新的搜索结果
			MemSearchResult *pMemResult=new MemSearchResult;
			pMemResult->m_pParent=NULL;
			pMemResult->m_pMemObject=pMemNoUseLessObject;
			pMemResult->m_vValue=vNoUseValue;
			pMemResultList.PushNode(pMemResult);
			//移除一个已经使用的相邻结点,取消两相关的相邻关系
			pBeginObject->m_pLeftNextMemObjectList.RemoveNode(pMemNoUseLessObject);
			pMemNoUseLessObject->m_pLeftNextMemObjectList.RemoveNode(pBeginObject);

			if(pMemNoUseLessObject==pEndObject) break;//如果找到了就退出
		}
		else
		{
			//加了新的搜索结果
			MemSearchResult *pMemResult=new MemSearchResult;
			pMemResult->m_pParent=pParentMemSearchObject;
			pMemResult->m_vValue=vMemValue;
			pMemResult->m_pMemObject=pMemLessObject;
			pMemResultList.PushNode(pMemResult);
			//移除一个已经使用的相邻结点,取消两相关的相邻关系
			pParentMemSearchObject->m_pMemObject->m_pLeftNextMemObjectList.RemoveNode(pMemLessObject);
			pMemLessObject->m_pLeftNextMemObjectList.RemoveNode(pParentMemSearchObject->m_pMemObject);

			if(pMemLessObject==pEndObject) break;//如果找到了就退出
		}
	}

	//从pMemResultList查找到真正目标的线路,并保存到pSearchResult
	nCount=pMemResultList.GetNodeCount();
	MemSearchResult *pMemResult=NULL;

	for(j=0;j<nCount;j++)
	{
		pMemResult=pMemResultList.GetNode(j);
		//从pMemResult中查找pEndObject
		MemSearchResult *pMemSearchTmp=NULL;
		pMemSearchTmp=pMemResult;
		while(pMemSearchTmp!=NULL)
		{
			if(pMemSearchTmp->m_pMemObject->m_OwnerObject==pEndObject->m_OwnerObject)//找到了
			{
				//开始保存真正的结果到pSearchResult
				MemSearchResult *pMemSearchTmp2=pMemResult;
				while(pMemSearchTmp2!=NULL)
				{
					SearchObject *p=new SearchObject;
					*p=pMemSearchTmp2->m_pMemObject->m_OwnerObject;
					pSearchResult->PushNode(p);
					pMemSearchTmp2=pMemSearchTmp2->m_pParent;
				}
				//增加起始结点
				SearchObject *p=new SearchObject;
				*p=pBeginObject->m_OwnerObject;
				pSearchResult->PushNode(p);
				j=nCount;
				pSearchResult->ReserveNode();//将结点反序，使它从起点到终点顺序
				break;
			}
			
			pMemSearchTmp=pMemSearchTmp->m_pParent;
		}
	}

	//释放pMemResultList空间
	nCount=pMemResultList.GetNodeCount();
	for(j=0;j<nCount;j++)
	{
		pMemResult=pMemResultList.GetNode(j);
		delete pMemResult;
	}

	//释放临时的m_pLeftNextMemObjectList空间
	for(i=0;i<m_nObjectCount;i++)
	{
		m_pMemObjectList[i].m_pLeftNextMemObjectList.RemoveAllNode();
	}

	return (pSearchResult->GetNodeCount()>0)?true:false;
}