本人用A*实现的最优路径算法源代码

楼主^#

更多发布于：2005-05-31 11:35

简介：在GIS网络中，指定两地图上任意的两点，求最优路径。我在全上海大数据量下在普通PC下测试计算时间小于1秒。 
#ifndef _CGIS_NET_PATH_FINDER_H_ 
#define _CGIS_NET_PATH_FINDER_H_ 
 
#include "map.h" 
#include "vector.h" 
 
using namespace __STD; 
 
/******************************************************* 
**purpose： net path finder 
**author： Kemp(MeiPing Ke, in Shanghai) 
**time： 2005/05/29 
********************************************************/ 
 
class CGIS_NetPathFinder 
{ 
public: 
 CGIS_NetPathFinder(); 
 virtual ~CGIS_NetPathFinder(); 
 
public: 
 //set layer 
 void SetNetLayer(CGIS_Layer* pNetLayer){m_pNetLayer = pNetLayer;} 
 
 //get layer 
 CGIS_Layer* GetNetLayer(){return m_pNetLayer;} 
 
 //set start position 
 void SetStartPos(CGIS_Point* pPoint){m_StartPos = *pPoint;} 
 
 //get start position 
 CGIS_Point* GetStartPos(){return ;m_StartPos;} 
 
 //set end position 
 void SetEndPos(CGIS_Point* pPoint){m_EndPos = *pPoint;} 
 
 //get end position 
 CGIS_Point* GetEndPos(){return ;m_EndPos;} 
 
public: 
 //find path 
 bool FindPath(vector<int>; vResNode, vector<int>; vResArc); 
 
 //reset 
 void Reset(); 
public: 
 //node info 
 struct NODE 
 { 
 //G 
 double g; 
 
 //F 
 double f; 
 
 //FID 
 int fid; 
 
 //Arc 
 int iArc; 
 
 //parent node 
 NODE *pParent; 
 }; 
 
 //virtual node info 
 struct V_NODE 
 { 
 //FID 
 int iNodeID; 
 
 //Arc 
 int iArcID; 
 
 //start arc node 
 int iStartNode; 
 
 //start weight 
 double dStartWeight; 
 
 //end arc node 
 int iEndNode; 
 
 //end weight 
 double dEndWeight; 
 }; 
protected: 
 //init virtual nodes 
 bool InitVirtualNode(CGIS_Point; Pos, V_NODE; vNod, int iNodID); 
 
 //calculate weight 
 virtual inline double CalWeight(CGIS_Point* pFromPoint, CGIS_Point* pToPoint); 
 virtual inline double CalWeight(int x, int y, int x2, int y2); 
 virtual inline double CalWeight(int iArcFid); 
 
 //get the result path 
 void GetResultPath(NODE* pNode,
vector<int>; vResNod, vector<int>; vResArc); 
 
 //get near arc 
 int GetNearSetion (int x, int y, int; iDis, CGIS_Feature** ppFeature); 
protected: 
 //net layer 
 CGIS_Layer* m_pNetLayer; 
 
 //start point 
 CGIS_Point m_StartPos; 
 
 //end point 
 CGIS_Point m_EndPos; 
 
 //open list 
 map<double, NODE*> m_mOpenList; 
 
 //close list 
 map<int, NODE*> m_mCloseList; 
 
 //start virtual node 
 V_NODE m_StartVNode; 
 
 //end virtual node 
 V_NODE m_EndVNode; 
}; 
 
#endif 
 
 
#include "stdafx.h" 
#include "CGeoMath.h" 
#include "CGIS_CircleShape.h" 
#include "public_share.h" 
#include "CGIS_NetPathFinder.h" 
 
/******************************************************* 
**purpose： net path finder 
**author： Kemp(MeiPing Ke, in Shanghai) 
**time： 2005/05/29 
********************************************************/ 
 
 
CGIS_NetPathFinder::CGIS_NetPathFinder():m_StartPos(0, 0), m_EndPos(0, 0) 
{ 
 //net layer 
 m_pNetLayer = NULL; 
 
 //start virtual node 
 memset(;m_StartVNode, 0, sizeof(V_NODE)); 
 
 //end virtual nod 
 memset(;m_EndVNode, 0, sizeof(V_NODE)); 
} 
 
CGIS_NetPathFinder::~CGIS_NetPathFinder() 
{ 
 Reset(); 
} 
 
//reset 
void CGIS_NetPathFinder::Reset() 
{ 
 //clean open list 
 map<double, NODE*>::iterator it = m_mOpenList.begin(); 
 for (; it != m_mOpenList.end(); it++) 
 { 
 delete it->second; 
 } 
 m_mOpenList.clear(); 
 
 //clean close list 
 map<int, NODE*>::iterator itClose = m_mCloseList.begin(); 
 for (; itClose != m_mCloseList.end(); itClose++) 
 { 
 delete itClose->second; 
 } 
 m_mCloseList.clear(); 
 
 //reset virtual node 
 memset(;m_StartVNode, 0, sizeof(V_NODE)); 
 memset(;m_EndVNode, 0, sizeof(V_NODE)); 
} 
 
//find path 
bool CGIS_NetPathFinder::FindPath(vector<int>; vResNode, vector<int>; vResArc) 
{ 
 ASSERT(m_pNetLayer != NULL); 
 ASSERT(m_StartPos != m_EndPos); 
 
 //init virtual nodes 
 int iVirtualNodeID = m_pNetLayer->GetMaxFeatureIDInMem() + 1; 
 if (!InitVirtualNode(m_StartPos, m_StartVNode, iVirtualNodeID++)) 
 return false; 
 if (!InitVirtualNode(m_EndPos, m_EndVNode, iVirtualNodeID++)) 
 return false; 
 
 //add the first nod to the open list 
 NODE* pNode = new NODE; 
 pNode->g = 0.; 
 pNode->f = pNode->g + CalWeight(;m_StartPos, ;m_EndPos); 
 pNode->pParent = NULL; 
 pNode->iArc = m_StartVNode.iArcID; 
 pNode->fid = m_StartVNode.iNodeID; 
 
 m_mOpenList.insert(map<double, NODE*>::value_type(pNode->f, pNode)); 
 
 //find 
 map<double, NODE*>::iterator itOpen; 
 map<int, int> mNodeArc; 
 map<int, int>::iterator itArc; 
 CGIS_GeoArc* pGeoArc = NULL; 
 CGIS_GeoPoint* pGeoPoint = NULL; 
 CGIS_Feature* pFeature = NULL; 
 NODE* pNewNode = NULL; 
 vector<int> vNearNode; 
 register int i = 0; 
 int iCount = 0, iArcID = 0, iNodeID = 0; 
 double h = 0.; 
 
 while (m_mOpenList.size() > 0) 
 { 
 //get min "f" 
 itOpen = m_mOpenList.begin(); 
 
 //found ? 
 pNode = itOpen->second; 
 if (pNode->fid == m_EndVNode.iNodeID) 
 { 
 GetResultPath(pNode, vResNode, vResArc); 
 return true; 
 } 
 else 
 { 
 //close it 
 m_mOpenList.erase(itOpen); 

m_mCloseList.insert(map<int, NODE*>::value_type(pNode->fid,
pNode)); 
 } 
 
 //get it's near nodes 
 vNearNode.clear(); 
 mNodeArc.clear(); 
 if (pNode->fid == m_StartVNode.iNodeID) 
 { 
 vNearNode.push_back(m_StartVNode.iStartNode); 
 vNearNode.push_back(m_StartVNode.iEndNode); 
 } 
 else if (pNode->fid == m_EndVNode.iNodeID) 
 { 
 vNearNode.push_back(m_EndVNode.iStartNode); 

vNearNode.push_back(m_EndVNode.iEndNode);
 
 } 
 else 
 { 
 pGeoPoint =
m_pNetLayer->GetAllFeaturesPtr()->GetPointFeatureSetPtr()->GetFeaturePtr(pNode->fid)->GetGeoPointPtr();
 
 iCount = pGeoPoint->GetArcCount(); 
 for ( i = 0; i < iCount; i++) 
 { 
 iArcID = pGeoPoint->GetArcID(i); 
 iArcID = abs(iArcID); 
 if (iArcID != m_StartVNode.iArcID) 
 { 

 pFeature =
m_pNetLayer->GetAllFeaturesPtr()->GetFeaturePtr(iArcID); 
 if (!pFeature) 
 { 
 ASSERT(false); 
 return false; 
 } 
 
 if (iArcID != m_EndVNode.iArcID) 

{ 

 iNodeID =
pFeature->GetGeoArcPtr()->GetStartNodeID(); 

 if (iNodeID !=
pNode->fid) 
 { 


 vNearNode.push_back(iNodeID); 


 mNodeArc.insert(map<int,
int>::value_type(iNodeID, iArcID)); 
 } 
 else 
 { 


 iNodeID =
pFeature->GetGeoArcPtr()->GetEndNodeID(); 


 vNearNode.push_back(iNodeID); 


 mNodeArc.insert(map<int,
int>::value_type(iNodeID, iArcID)); 
 } 
 } 
 else 
 { 


vNearNode.push_back(m_EndVNode.iNodeID); 
 } 
 } 
 } 
 } 
 
 //do near nodes 
 for (i = 0; i < vNearNode.size(); i++) 
 { 
 if (m_mCloseList.find(vNearNode) != m_mCloseList.end()) 
 continue; 
 
 //add the node to open list 
 if (m_mOpenList.find(vNearNode) != m_mOpenList.end()) 
 continue; 
 
 pNewNode = new NODE; 
 pNewNode->fid = vNearNode; 
 pNewNode->g = 0.; 
 h = 0.; 
 pNewNode->pParent = pNode; 
 pNewNode->iArc = 0; 
 
 if
(pNode->fid == m_StartVNode.iNodeID ;; pNewNode->fid ==
m_StartVNode.iStartNode) 
 { 
 pNewNode->iArc = m_StartVNode.iArcID; 

 pNewNode->g = pNewNode->pParent->g +
m_StartVNode.dStartWeight; 
 } 
 else if
(pNode->fid == m_StartVNode.iNodeID ;; pNewNode->fid ==
m_StartVNode.iEndNode) 
 { 
 pNewNode->iArc = m_StartVNode.iArcID; 

 pNewNode->g = pNewNode->pParent->g +
m_StartVNode.dEndWeight; 
 } 
 else if
((pNode->fid == m_EndVNode.iNodeID ;; pNewNode->fid ==
m_EndVNode.iEndNode) 

 || (pNode->fid == m_EndVNode.iEndNode ;;
pNewNode->fid == m_EndVNode.iNodeID)) 
 { 
 pNewNode->iArc = m_EndVNode.iArcID; 

 pNewNode->g = pNewNode->pParent->g +
m_EndVNode.dEndWeight; 
 } 
 else if
((pNode->fid == m_EndVNode.iNodeID ;; pNewNode->fid ==
m_EndVNode.iEndNode) 

 || (pNode->fid == m_EndVNode.iEndNode ;;
pNewNode->fid == m_EndVNode.iNodeID)) 
 { 
 pNewNode->iArc = m_EndVNode.iArcID; 

 pNewNode->g = pNewNode->pParent->g +
m_EndVNode.dEndWeight; 
 } 
 else if (pNewNode->fid != m_StartVNode.iNodeID 
 ;; pNewNode->fid != m_EndVNode.iNodeID 
 ;; pNode->fid != m_StartVNode.iNodeID 
 ;; pNode->fid != m_EndVNode.iNodeID) 
 { 

 if ((itArc = mNodeArc.find(pNewNode->fid)) !=
mNodeArc.end()) 
 { 

 pNewNode->iArc =
itArc->second; 

 pNewNode->g =
pNewNode->pParent->g + CalWeight(itArc->second); 
 } 
 } 
 
 if (pNewNode->fid == m_StartVNode.iNodeID) 
 { 
 h = CalWeight(;m_StartPos, ;m_EndPos); 
 } 
 else if (pNewNode->fid == m_EndVNode.iNodeID) 
 { 
 h = 0.; 
 } 
 else 
 { 

 pFeature =
m_pNetLayer->GetAllFeaturesPtr()->GetFeaturePtr(pNewNode->fid); 
 if (pFeature == NULL) 
 { 
 ASSERT(false); 
 return false; 
 } 
 

 h =
CalWeight(pFeature->GetGeoPointPtr()->GetPointPtr(),
;m_EndPos); 
 } 
 
 pNewNode->f = pNewNode->g + h; 

m_mOpenList.insert(map<double, NODE*>::value_type(pNewNode->f,
pNewNode)); 
 } 
 } 
 
 return false; 
} 
 
//init virtual nods 
bool CGIS_NetPathFinder::InitVirtualNode(CGIS_Point; Pos, V_NODE; vNode, int iNodeID) 
{ 
 CGIS_Feature* pNearArc = NULL; 
 int iDis = 0; 
 D_DOT pt, ptOut; 
 D_DOT* pLin = NULL; 
 double dDis = 0.; 
 int iSub = -1; 
 int iCount = 0; 
 
 try 
 { 
 GetNearSetion (Pos.x, Pos.y, iDis, ;pNearArc); 
 if (pNearArc == NULL) 
 { 
 AfxMessageBox("无法找到最近段！"); 
 return false; 
 } 
 
 pt.x = ((double)Pos.Getx ()) / 10000.; 
 pt.y = ((double)Pos.Gety ()) / 10000.; 
 
 iCount = pNearArc->GetGeoArcPtr()->GetPointCount(); 
 pLin = new D_DOT[iCount]; 
 for (int i = 0; i < iCount; i++) 
 { 
 pLin.x =
pNearArc->GetGeoArcPtr()->GetPointPtr(i)->Getx() / 10000.; 
 pLin.y =
pNearArc->GetGeoArcPtr()->GetPointPtr(i)->Gety() / 10000.; 
 } 
 
 iSub = GetNearSubInLin( pt, pLin, iCount); 
 if (iSub < 0) 
 throw ""; 
 if (0 == GetNearPtInLin(pt, pLin, iCount, ptOut, dDis)) 
 throw ""; 
 
 memset(;vNode, 0, sizeof(V_NODE)); 
 vNode.iArcID = pNearArc->GetFeatureID(); 
 vNode.iStartNode = pNearArc->GetGeoArcPtr()->GetStartNodeID(); 
 vNode.iEndNode = pNearArc->GetGeoArcPtr()->GetEndNodeID(); 
 
 CGIS_Feature* pF1 =
m_pNetLayer->GetAllFeaturesPtr()->GetFeaturePtr(vNode.iStartNode);
 
 if (pF1 == NULL) 
 throw ""; 
 double dWight1 =
CalWeight(pLin[0].x * 10000, pLin[0].y * 10000,
pF1->GetGeoPointPtr()->GetPointPtr()->Getx(),
pF1->GetGeoPointPtr()->GetPointPtr()->Gety()); 
 double dWight2 =
CalWeight(pLin[iCount - 1].x * 10000, pLin[iCount - 1].y * 10000,
pF1->GetGeoPointPtr()->GetPointPtr()->Getx(),
pF1->GetGeoPointPtr()->GetPointPtr()->Gety()); 
 if (dWight1 > dWight2) 
 { 
 int iTemp = vNode.iStartNode; 
 vNode.iStartNode = vNode.iEndNode; 
 vNode.iEndNode = iTemp; 
 } 
 
 vNode.iNodeID = iNodeID; 
 vNode.dStartWeight = 0.; 
 vNode.dEndWeight = 0.; 
 
 for (i = 0; i < iSub; i++) 

vNode.dStartWeight += CalWeight(pLin.x * 10000, pLin.y * 10000,
pLin[i + 1].x * 10000, pLin[i + 1].y * 10000); 
 vNode.dStartWeight +=
CalWeight(pLin[iSub].x * 10000, pLin[iSub].y * 10000, ptOut.x * 10000,
ptOut.y * 10000); 
 
 vNode.dEndWeight +=
CalWeight(ptOut.x * 10000, ptOut.y * 10000, pLin[iSub + 1].x * 10000,
pLin[iSub + 1].y * 10000); 
 for (i = iSub + 1; i < iCount - 1; i++) 

vNode.dEndWeight += CalWeight(pLin.x * 10000, pLin.y * 10000,
pLin[i + 1].x * 10000, pLin[i + 1].y * 10000); 
 } 
 catch (LPSTR) 
 { 
 delete [] pLin; 
 return false; 
 } 
 
 delete [] pLin; 
 
 return true; 
} 
 
//calculate weight 
inline double CGIS_NetPathFinder::CalWeight(CGIS_Point* pFromPoint, CGIS_Point* pToPoint) 
{ 
 double dx = (pFromPoint->x - pToPoint->x)/10000.; 
 double dy = (pFromPoint->y - pToPoint->y)/10000.; 
 
 return sqrt(dx * dx + dy * dy); 
} 
 
inline double CGIS_NetPathFinder::CalWeight(int x, int y, int x2, int y2) 
{ 
 double dx = (x - x2)/10000.; 
 double dy = (y - y2)/10000.; 
 
 return sqrt(dx * dx + dy * dy); 
} 
 
inline double CGIS_NetPathFinder::CalWeight(int iArcFid) 
{ 
 ASSERT(m_pNetLayer != NULL); 
 
 CGIS_GeoArc* pGeoArc = m_pNetLayer->GetAllFeaturesPtr()->GetFeaturePtr(iArcFid)->GetGeoArcPtr(); 
 double dWeight = 0.; 
 int iCount = pGeoArc->GetPointCount(); 
 for (int i = 0; i < iCount - 1; i++) 
 { 
 dWeight += CalWeight(pGeoArc->GetPointPtr(i), pGeoArc->GetPointPtr(i + 1)); 
 } 
 
 return dWeight; 
} 
 
//get the result path 
void CGIS_NetPathFinder::GetResultPath(NODE* pNode,
vector<int>; vResNode, vector<int>; vResArc) 
{ 
 ASSERT(pNode != NULL ;; pNode->iArc != 0); 
 
 NODE* pParentNode = pNode; 
 while (pParentNode != NULL) 
 { 
 if (pParentNode->fid != m_StartVNode.iNodeID 
 ;; pParentNode->fid != m_EndVNode.iNodeID) 
 { 
 vResNode.push_back(pParentNode->fid); 
 } 
 
 vResArc.push_back(pParentNode->iArc); 
 
 pParentNode = pParentNode->pParent; 
 } 
} 
 
int CGIS_NetPathFinder::GetNearSetion (int x,int y,int; iDis,CGIS_Feature** ppFeature) 
{ 
 CGIS_Point CenterPos; 
 CenterPos.Setx (x); 
 CenterPos.Sety (y); 
 
 CGIS_FeatureSet SectionSet(m_pNetLayer, "temp query set"); 
 int iRadiu = 10 * 10000; 
 CGIS_CircleShape CircleShape; 
 while
(SectionSet.GetArcFeatureSetPtr()->GetFeatureCount() < 1
;; iRadiu < 500 * 100 * 10000) 
 { 
 CircleShape.SetCentrePoint(CenterPos); 
 CircleShape.SetRadius(iRadiu); 
 

pNetLayer->GetAllFeaturesPtr()->QueryByShape(;CircleShape,
GIS_SPATIAL_INTERSECT, ;SectionSet); 
 
 iRadiu += 10 * 10000; 
 } 
 
 D_DOT pt,ptOut,*pLin = NULL; 
 int iHaveLen = 0; 
 double dDist = 0,dMinDist = 99999999999999.;// a impossible max value 
 int iFindID = 0; 
 
 pt.x = x; 
 pt.y = y; 
 
 CGIS_Feature* pFeature =SectionSet.GetArcFeatureSetPtr () ->GetFirstFeaturePtr (); 
 while (pFeature) 
 { 
 if (iHaveLen < pFeature ->GetGeoArcPtr () ->GetPointCount ()) 
 { 
 if (pLin) 
 delete [] pLin; 
 pLin
= new D_DOT[pFeature ->GetGeoArcPtr () ->GetPointCount ()]; 
 if (!pLin) 
 return 0; 

iHaveLen = pFeature ->GetGeoArcPtr () ->GetPointCount (); 
 } 
 
 for (int i = 0; i < pFeature ->GetGeoArcPtr () ->GetPointCount (); i ++) 
 { 

pLin.x = pFeature ->GetGeoArcPtr () ->GetPointPtr (i)
->Getx (); 

pLin.y = pFeature ->GetGeoArcPtr () ->GetPointPtr (i)
->Gety (); 
 } 
 dDist = 0.; 

GetNearPtInLin(pt,pLin,pFeature ->GetGeoArcPtr () ->GetPointCount
(),ptOut,dDist); 
 if (dDist < dMinDist) 
 { 
 dMinDist = dDist; 
 iFindID = pFeature ->GetFeatureID (); 
 if (ppFeature) 
 *ppFeature = pFeature; 
 } 
 pFeature
=SectionSet.GetArcFeatureSetPtr () ->GetNextFeaturePtr
(); 
 } 
 if (pLin) 
 delete [] pLin; 
 
 iDis = dMinDist; 
 return iFindID; 
}

喜欢0 评分0

举报回复

whmwxhanshan123 路人甲注册日期2006-06-17 发帖数3108 QQ 铜币6445枚威望0点贡献值0点银元0个加关注写私信	1楼^# 发布于：2007-03-20 18:54 <P>关注!</P>
	举报回复(0) 喜欢(0) 评分

tianwailk 路人甲注册日期2006-10-20 发帖数1 QQ 铜币105枚威望0点贡献值0点银元0个加关注写私信	2楼^# 发布于：2007-03-19 17:38 回复：（johnxt）本人用A*实现的最优路径算法源代码 //add the node to open list<BR><BR> if (m_mOpenList.find(vNearNode) != m_mOpenList.end())<BR><BR> continue;<BR>这里是不是有问题。map数组的键值显然是double，而vNearNode返回为INT。显然，这一步判断是失败的。永远没法查证某个结点是不是在OPEN表。
	举报回复(0) 喜欢(0) 评分

苍青月光路人甲注册日期2005-08-23 发帖数2 QQ 铜币94枚威望0点贡献值0点银元0个加关注写私信	3楼^# 发布于：2005-08-23 17:02 <P>感谢楼主，提供了一个解决思路</P>
	举报回复(0) 喜欢(0) 评分

gis

管理员

注册日期2003-07-16
发帖数15951
QQ
铜币25345枚
威望15368点
贡献值0点
银元0个

加关注写私信

4楼^#

发布于：2005-05-31 11:40

支持楼主，不知道你用什么数据实现的，给个小例子给大家就方便了
<img src="images/post/smile/dvbbs/em01.gif" /><img src="images/post/smile/dvbbs/em02.gif" /><img src="images/post/smile/dvbbs/em04.gif" />

GIS麦田守望者，期待与您交流。

举报回复喜欢评分

您需要登录后才可以回帖，登录或者注册

本人用A*实现的最优路径算法源代码

最新喜欢：