黑马程序员技术交流社区

标题: A* 算法计算导航路径（最短路径） [打印本页]

---

作者: aisini    时间: 2014-7-29 19:50
标题: A* 算法计算导航路径（最短路径）
            导航路径的计算，一般用 Dijkstra 算法和 A* 算法。
            Dijkstra 算法是全局遍历，确保运算结果一定是最短路径。
            A* 算法是策略寻路，不保证一定是最短路径。
          Dijkstra 需要载入全部数据，遍历搜索。（也可以分层计算，分层载入）
          A* 算法可以根据需要，分部分块载入地图数据。    本代码是基于 A* 算法搜索最短路径的一个演示程序。
  附件源码中的道路属性类参考了高德导航地图的数据结构。

1. /// <summary>
   
2. /// 搜索最短路径
   
3. /// </summary>
   
4. /// <param name="starts">起始节点描述序列</param>
   
5. /// <param name="endPt">终止点坐标值</param>
   
6. /// <param name="endsID">终止节点ID序列</param>
   
7. /// <param name="avoidsID">避开节点ID序列</param>
   
8. /// <returns></returns>
   
9. public int[] CreateShortPath(ExtendLine startELine, ExtendLine endELine, int[] avoids)
   
10. {
    
11.     ExtendLine sl = startELine;
    
12.     ExtendLine el = endELine;
    
13.     // 检测入口参数的合法性
    
14.     // ...
    
15. 
    
16.     // 检查起始节点内是否包含终止节点，如果包含，直接返回此点
    
17.     Debug.Assert( sl.IsOut == false && el.IsOut == true );
    
18.     if (( sl.FLine != null && el.FLine != null && el.FLine.TNodeID == el.FLine.FNodeID ) ||
    
19.         ( sl.FLine != null && el.TLine != null && sl.FLine.TNodeID == el.TLine.FNodeID ))
    
20.         return new int[] { sl.FLine.TNodeID };
    
21. 
    
22.     if (( sl.TLine != null && el.FLine != null && sl.TLine.TNodeID == el.FLine.FNodeID ) ||
    
23.         ( sl.TLine != null && el.TLine != null && sl.TLine.TNodeID == el.TLine.FNodeID ))
    
24.         return new int[] { sl.TLine.TNodeID };
    
25. 
    
26.     // 初始化搜索队列
    
27.     AStarQueue queue = this.InitSearchQueue( sl, el, avoids );
    
28. 
    
29.     // 搜索最短路径
    
30.     int pos = this.ShortSearch( queue, el.VLine.Points[1] );
    
31. 
    
32.     // 返回路径信息
    
33.     return this.TracePath( queue, pos );
    
34. }
    
35. 
    
36. /// <summary>
    
37. /// 初始化搜索队列
    
38. /// </summary>
    
39. private AStarQueue InitSearchQueue(ExtendLine sl, ExtendLine el, int[] avoidsID)
    
40. {
    
41.     // 创建队列
    
42.     AStarQueue queue = new AStarQueue( _gTab );
    
43. 
    
44.     // 起始点加入开放列表
    
45.     Debug.Assert( sl.FLine != null || sl.TLine != null );
    
46.     if ( sl.FLine != null )
    
47.     {
    
48.         Debug.Assert( sl.FLine.TNodeID > 0 );
    
49.         GraphNode gn = GraphNode.SearchByID( _gTab, sl.FLine.TNodeID );
    
50.         float h = GeoMath.Distance( gn.Point, el.VLine.Points[1] );
    
51.         queue.SetOpen( gn.NodePos, -1, sl.FLine.Length, h );
    
52.     }
    
53.     if ( sl.TLine != null )
    
54.     {
    
55.         Debug.Assert( sl.TLine.TNodeID > 0 );
    
56.         GraphNode gn = GraphNode.SearchByID( _gTab, sl.TLine.TNodeID );
    
57.         float h = GeoMath.Distance( gn.Point, el.VLine.Points[1] );
    
58.         queue.SetOpen( gn.NodePos, -1, sl.TLine.Length, h );
    
59.     }
    
60. 
    
61.     // 终止点加入终止列表
    
62.     if ( el.FLine != null )
    
63.     {
    
64.         Debug.Assert( el.FLine.FNodeID > 0 );
    
65.         GraphNode gn = GraphNode.SearchByID( _gTab, el.FLine.FNodeID );
    
66.         Debug.Assert( ! queue.IsOpen( gn.NodePos ) );   // 起始点不能与终止点重合
    
67.         queue.SetEnd( gn.NodePos );
    
68.     }
    
69.     if ( el.TLine != null )
    
70.     {
    
71.         Debug.Assert( el.TLine.FNodeID > 0 );
    
72.         GraphNode gn = GraphNode.SearchByID( _gTab, el.TLine.FNodeID );
    
73.         Debug.Assert( ! queue.IsOpen( gn.NodePos ) );
    
74.         queue.SetEnd( gn.NodePos );
    
75.     }
    
76. 
    
77.     // 避开点加入封闭列表
    
78.     if ( avoidsID != null )
    
79.     {
    
80.         foreach ( int nid in avoidsID )
    
81.         {
    
82.             GraphNode gn = GraphNode.SearchByID( _gTab, nid );
    
83.             if ( queue.IsOpen( gn.NodePos ) )
    
84.                 continue;
    
85.             else if ( queue.IsEnd( gn.NodePos ) )
    
86.                 continue;
    
87.             else
    
88.                 queue.SetClose( gn.NodePos );
    
89.         }
    
90.     }
    
91. 
    
92.     // 返回队列
    
93.     return queue;
    
94. }
    
95. 
    
96. /// <summary>
    
97. /// 在设置好的队列内搜索最短路径
    
98. /// </summary>
    
99. /// <param name="queue">已设置好起止节点的队列</param>
    
100. /// <param name="endPoint">终止点坐标</param>
     
101. /// <returns>终止节点的位置(Pos)</returns>
     
102. private int ShortSearch(AStarQueue queue, FPoint endPt)
     
103. {
     
104.     // 开始搜索
     
105.     while ( queue.Min >= 0 )
     
106.     {
     
107.         // 当前点加入封闭列表
     
108.         int curPos = queue.Min;
     
109.         queue.SetClose( curPos );
     
110. 
     
111.         GraphNode gnCur = this._gTab[curPos] as GraphNode;
     
112.         foreach ( GraphLink glnk in gnCur.Links )
     
113.         {
     
114.             // 获取连接点
     
115.             int nextPos = glnk.NodePos;
     
116.             GraphNode gnNext = this._gTab[nextPos] as GraphNode;
     
117.             float g = queue.GValue( curPos ) + glnk.Length;
     
118.             float h = GeoMath.Distance( gnNext.Point, endPt );
     
119.          
     
120.             if ( ! queue.IsEnd( gnNext.NodePos ) )  // 未达终点
     
121.             {
     
122.                 // 不在封闭列表
     
123.                 if ( ! queue.IsClose( nextPos ) )
     
124.                 {
     
125.                     // 开放列表内
     
126.                     if ( queue.IsOpen( nextPos ) )
     
127.                     {
     
128.                         if ( g < queue.GValue( nextPos ) )
     
129.                         {
     
130.                             queue.SetOpen( nextPos, curPos, g, h );
     
131.                         }
     
132.                     }
     
133.                     else
     
134.                     {
     
135.                         queue.SetOpen( nextPos, curPos, g, h );
     
136.                     }
     
137.                 }
     
138.             }
     
139.             else    // 抵达终点
     
140.             {
     
141.                 queue.SetOpen( nextPos, curPos, g, h );
     
142.                 queue.SetClose( nextPos );
     
143.                 return nextPos;
     
144.             }
     
145.         }
     
146.     }
     
147.     return -1;  // 不可到达
     
148. }
     
149. 
     
150. /// <summary>
     
151. /// 获取路径数据
     
152. /// </summary>
     
153. /// <param name="queue">已搜出路径的队列</param>
     
154. /// <param name="pos">回溯操作的起始点</param>
     
155. /// <returns>NodeID, RoadID, NodeID, RoadID, ... , NodeID</returns>
     
156. private int[] TracePath(AStarQueue queue, int endPos)
     
157. {
     
158.     // 获取路径
     
159.     ArrayList idls = new ArrayList();
     
160.     for ( int pos = endPos; pos >= 0; pos = queue[pos].ParentPos )
     
161.     {
     
162.         GraphNode gn = _gTab[pos] as GraphNode;
     
163.         idls.Add( gn.NodeID );
     
164. 
     
165.         int parentPos = queue[pos].ParentPos;
     
166.         if ( parentPos >= 0 )
     
167.         {
     
168.             GraphNode pn = _gTab[parentPos] as GraphNode;
     
169.             foreach ( GraphLink glnk in pn.Links )
     
170.             {
     
171.                 if ( glnk.NodePos == pos )
     
172.                 {
     
173.                     idls.Add( glnk.RoadID );
     
174.                     break;
     
175.                 }
     
176.             }
     
177.         }
     
178.     }
     
179. 
     
180.     idls.Reverse();
     
181.     int[] ids = new int[idls.Count];
     
182.     idls.CopyTo( ids );
     
183. 
     
184.     Debug.Assert( this.ValidResult( ids ) );
     
185.     return ids;
     
186. }

复制代码

---

作者: rende1991    时间: 2014-7-30 00:39
学习了  努力再努力

---

欢迎光临 黑马程序员技术交流社区 (http://bbs.itheima.com/)

黑马程序员IT技术论坛 X3.2