之前很早就想准备关于寻路分析的博文，一方面没有时间和精力去做这件事情，平时也只是找些相关的资料看看，知道个什么事，并没有从代码层面去分析怎么个寻路，所以算不懂。从参与的几款游戏开发经历来说，是没有机会手写这方面的实现，一般都有开源的实现直接用，所以现在下决心要搞懂这块，不光代码，也要看些论文和分析，真正的收获。
可能每个游戏项目使用到的实现不一样，但总体来说，都相似，有迪杰斯特拉算法，B星等，关于理论方面的就不再说明，可能在这方面的理解短时间还达不到，源代码在此处github，这里只分析A星。
在FSA头文件中，有关于些模板类的使用说明，分析固定大小的内存分配器，直接分配一定的内存，避免每次malloc/new/free/delete等操作，类似的实现思路可以参考leveldb，由于代码量不多，直接贴上代码并给注释。
51 template <class USER_TYPE> class FixedSizeAllocator
52 {
54 public:
56 enum
57 {
58 FSA_DEFAULT_SIZE = 100
59 };
64 struct FSA_ELEMENT //节点信息
65 {
66 USER_TYPE UserType;
68 FSA_ELEMENT *pPrev;
69 FSA_ELEMENT *pNext;
70 };
243 private:
245 FSA_ELEMENT *m_pFirstFree;//指向可用内存
246 FSA_ELEMENT *m_pFirstUsed;
247 unsigned int m_MaxElements;
248 FSA_ELEMENT *m_pMemory;//指向内存开始处
250 };
以上是数据成员及结构类型，下面是构造函数和析构函数：
72 public:
73 FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) :
74 m_pFirstUsed( NULL ),
75 m_MaxElements( MaxElements )
76 {
79 char *pMem = new char[ m_MaxElements * sizeof(FSA_ELEMENT) ];
81 m_pMemory = (FSA_ELEMENT *) pMem;
84 m_pFirstFree = m_pMemory;
85
87 memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements );
88
90 FSA_ELEMENT *pElement = m_pFirstFree;
93 for( unsigned int i=0; i<m_MaxElements; i++ )
94 {
95 pElement->pPrev = pElement-1;
96 pElement->pNext = pElement+1;
98 pElement++;
99 }
100
102 m_pFirstFree->pPrev = NULL;
104 (pElement-1)->pNext = NULL;
106 }
109 ~FixedSizeAllocator()
110 {
112 delete [] (char *) m_pMemory;
113 }
如注释，析构函数即释放m_pMemory，构造函数简单的分配固定个数和大小的内存节点，并设置相关的指针变量，初始化每个节点的pPrev和pNext，即双向链表。
下面的实现是分配和回收节点内存的实现：
116 USER_TYPE *alloc()
117 {
119 FSA_ELEMENT *pNewNode = NULL;
121 if( !m_pFirstFree )
122 {
123 return NULL;//无节点可用
124 }
125 else
126 {
127 pNewNode = m_pFirstFree;
128 m_pFirstFree = pNewNode->pNext;//指向下一个可用节点
132 if( pNewNode->pNext )
133 {
134 pNewNode->pNext->pPrev = NULL;//从双向链表中断开
135 }
136
137 // node is now on the used list
139 pNewNode->pPrev = NULL;
141 if( m_pFirstUsed == NULL )
142 {
143 pNewNode->pNext = NULL;
144 }
145 else
146 {
147 m_pFirstUsed->pPrev = pNewNode;
148 pNewNode->pNext = m_pFirstUsed;
149 }
151 m_pFirstUsed = pNewNode;
152 }
154 return reinterpret_cast<USER_TYPE*>(pNewNode);
155 }
以上分配节点从m_pFirstFree获取，并把自己通过头插法插入到m_pFirstUsed中。
162 void free( USER_TYPE *user_data )
163 {
164 FSA_ELEMENT *pNode = reinterpret_cast<FSA_ELEMENT*>(user_data);
167 if( pNode->pPrev )
168 {
169 pNode->pPrev->pNext = pNode->pNext;
170 }
171 else
172 {
174 m_pFirstUsed = pNode->pNext;
175 }
176
177 if( pNode->pNext )
178 {
179 pNode->pNext->pPrev = pNode->pPrev;
180 }
181
183 if( m_pFirstFree == NULL )
184 {
186 m_pFirstFree = pNode;
187 pNode->pPrev = NULL;
188 pNode->pNext = NULL;
189 }
190 else
191 {
193 m_pFirstFree->pPrev = pNode;
194 pNode->pNext = m_pFirstFree;
195 m_pFirstFree = pNode;
196 }
198 }
从m_pFirstUsed回收节点内存至m_pFirstFree，这里并没有检查待回收节点的内存是否是从这里分配出去的。
下面是寻路算法的实现引用：
F(n)= G(n) + H(n)
G(n)是开始节点到当前节点实际的移动代价；
H(n)是当前节点到目标节点的预估移动代价；
F(n)是G(n)和H(n)代价的总和；
算法描述：
首先有两个集合1.优先队列openList【后面搜索过程中将要被关注的集合元素】，2.closeList后面不需要关注的集合
1.将开始节点放进优先队列
2.如果优先队列中还有元素
3.从优先队列中取出一个元素（F(n)值最小的元素）
4.遍历该元素的相邻元素（上下左右四个方向，条件是该节点没有被遍历过，即不是openList或closeList中的数据），计算其fn,gn,hn的值。并将其加入优先队列。
5.如果该元素是目标节点，即结束算法，找出最短路径（输出）
6.如果该元素不是目标节点，将此元素加入closed【此后搜索过程中不需要被关注的集合元素】列表，然后继续执行过程2。
63 enum
64 {
65 SEARCH_STATE_NOT_INITIALISED,
66 SEARCH_STATE_SEARCHING,
67 SEARCH_STATE_SUCCEEDED,
68 SEARCH_STATE_FAILED,
69 SEARCH_STATE_OUT_OF_MEMORY,
70 SEARCH_STATE_INVALID
71 };
79 class Node
80 {
81 public:
83 Node *parent; // used during the search to record the parent of successor nodes
84 Node *child; // used after the search for the application to view the search in reverse
85
86 float g; // cost of this node + it's predecessors
87 float h; // heuristic estimate of distance to goal
88 float f; // sum of cumulative cost of predecessors and self and heuristic
89
90 Node() :
91 parent( 0 ),
92 child( 0 ),
93 g( 0.0f ),
94 h( 0.0f ),
95 f( 0.0f )
96 {
97 }
99 UserState m_UserState;
100 };
106 class HeapCompare_f
107 {
108 public:
110 bool operator() ( const Node *x, const Node *y ) const
111 {
112 return x->f > y->f;
113 }
114 };
以上是关于寻路过程中的状态值，和抽象化成的节点信息，其中UserState是用户自定义的类型，需要实现如下接口，用于在处理过程中的判断，和堆排序的仿函数：
GoalDistanceEstimate/IsSameState/IsGoal/GetSuccessors/GetCost/IsSameState
58 template <class UserState> class AStarSearch
59 {
117 public:
132 AStarSearch( int MaxNodes ) :
133 m_State( SEARCH_STATE_NOT_INITIALISED ),
134 m_CurrentSolutionNode( NULL ),
135 #if USE_FSA_MEMORY
136 m_FixedSizeAllocator( MaxNodes ),
137 #endif
140 {
141 }
458 UserState *GetSolutionStart()
459 {
460 m_CurrentSolutionNode = m_Start;
461 if( m_Start )
462 {
463 return &m_Start->m_UserState;
464 }
465 else
466 {
467 return NULL;
468 }
469 }
472 UserState *GetSolutionNext()
473 {
474 if( m_CurrentSolutionNode )
475 {
476 if( m_CurrentSolutionNode->child )
477 {
479 Node *child = m_CurrentSolutionNode->child;
481 m_CurrentSolutionNode = m_CurrentSolutionNode->child;
483 return &child->m_UserState;
484 }
485 }
487 return NULL;
488 }
491 UserState *GetSolutionEnd()
492 {
493 m_CurrentSolutionNode = m_Goal;
494 if( m_Goal )
495 {
496 return &m_Goal->m_UserState;
497 }
498 else
499 {
500 return NULL;
501 }
502 }
525 float GetSolutionCost()
526 {
527 if( m_Goal && m_State == SEARCH_STATE_SUCCEEDED )
528 {
529 return m_Goal->g;
530 }
531 else
532 {
533 return FLT_MAX;
534 }
535 }
632 }
746 private:
749 vector< Node * > m_OpenList;
752 vector< Node * > m_ClosedList;
756 vector< Node * > m_Successors;
757
759 unsigned int m_State;
762 int m_Steps;
765 Node *m_Start;
766 Node *m_Goal;
768 Node *m_CurrentSolutionNode;
769
770 #if USE_FSA_MEMORY
772 FixedSizeAllocator<Node> m_FixedSizeAllocator;
773 #endif
785 };
以上几个接口实现是在寻路成功后，获取下一个节点的接口。
其他几个private函数是跟资源的分析和释放有关，和debug信息相关的，就不再占用过多的篇幅去分析：
FreeAllNodes/FreeUnusedNodes/AllocateNode/FreeNode
下面列出关键的实现：
150 void SetStartAndGoalStates( UserState &Start, UserState &Goal )
151 {
154 m_Start = AllocateNode();
155 m_Goal = AllocateNode();
159 assert((m_Start != NULL && m_Goal != NULL));
160 m_Start->m_UserState = Start;
161 m_Goal->m_UserState = Goal;
162
163 m_State = SEARCH_STATE_SEARCHING;
164
168 m_Start->g = 0;
169 m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
170 m_Start->f = m_Start->g + m_Start->h;
171 m_Start->parent = 0;
175 m_OpenList.push_back( m_Start );
178 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
179
181 m_Steps = 0;
182 }
在即将开始寻路时，是需要给算法设置起始和目标点，并设置状态SEARCH_STATE_SEARCHING，初始化g/h/f，这里使用欧几里得距离公式计算，为了减少开根的耗时(参考中有说明这种操作带来衡量单位的问题)，设GoalDistanceEstimate的伪代码实现如下：
local function GoalDistanceEstimate(goal_node)
local xdiff = (goal_node.x - current_node.x)
local ydiff = (goal_node.y - current_node.y)
return xdiff * xdiff + ydiff * ydiff
end
起点的parent无，并把它加入到m_OpenList中，一个节点其实是有序的，这里可以省略。
185 unsigned int SearchStep()
186 {
188 assert((m_State > SEARCH_STATE_NOT_INITIALISED) && (m_State < SEARCH_STATE_INVALID));
192 if((m_State == SEARCH_STATE_SUCCEEDED) || (m_State == SEARCH_STATE_FAILED))
195 {
196 return m_State;//状态判断
197 }
198
202 if( m_OpenList.empty())
203 {
204 FreeAllNodes();//释放内存节点
205 m_State = SEARCH_STATE_FAILED;//搜索失败
206 return m_State;
207 }
213 Node *n = m_OpenList.front();
214 pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
215 m_OpenList.pop_back();
以上执行每一步，都会判断当前处于状态，要是m_OpenList已没有可搜索节点则进行失败处理，否则取f最小的节点并重新调整m_OpenList堆结构；
这先解释下设置m_State状态分别在什么情况下：
当内存不足时，会设置m_State=SEARCH_STATE_OUT_OF_MEMORY，在状态不对时(即只会在没有调用SetStartAndGoalStates时进行m_State = SEARCH_STATE_SEARCHING)，会m_State=SEARCH_STATE_INVALID，当找到目标点时，会m_State = SEARCH_STATE_SUCCEEDED，这里不考虑内存不足的情况和其他不合法的情况。
这里IsGoal判断pop出来的最小值节点是不是目标节点，因为对于寻路来说，细粒度是goal_node.x == current_node.x && goal_node.y == current_node.y这样不太好判断，可以抽像化起始点目标点各自所在的坐标区域为比较对象，比如地图大小为10米，然后分割成10份，那么只要比较所在哪一段即可。
这里分两种情况分析，情况二当pop出的节点不是goal_node时，最开始m_OpenList中就只有m_Start：
218 if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
219 {
220 //情况一，稍后分析
250 m_State = SEARCH_STATE_SUCCEEDED;
252 return m_State;
253 }
254 else //情况二
255 {
261 m_Successors.clear(); // empty vector of successor nodes
265 bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
266
267 if( !ret )
268 {//处理失败的情况
270 typename vector< Node * >::iterator successor;
273 for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
274 {
275 FreeNode( (*successor) );
276 }
278 m_Successors.clear();
281 FreeAllNodes();
283 m_State = SEARCH_STATE_OUT_OF_MEMORY;
284 return m_State;
285 }
288 for( typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
289 {
292 float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState );
293
300 typename vector< Node * >::iterator openlist_result;
302 for( openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result ++ )
303 {
304 if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
305 {
306 break;
307 }
308 }
309
310 if( openlist_result != m_OpenList.end() )
311 {
315 if( (*openlist_result)->g <= newg )
316 {
317 FreeNode( (*successor) );
320 continue;
321 }
322 }
324 typename vector< Node * >::iterator closedlist_result;
325
326 for( closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result ++ )
327 {
328 if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) )
329 {
330 break;
331 }
332 }
333
334 if( closedlist_result != m_ClosedList.end() )
335 {
339 if( (*closedlist_result)->g <= newg )
340 {
342 FreeNode( (*successor) );
344 continue;
345 }
346 }
351 (*successor)->parent = n;
352 (*successor)->g = newg;
353 (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
354 (*successor)->f = (*successor)->g + (*successor)->h;
358 if( closedlist_result != m_ClosedList.end() )
359 {
361 FreeNode( (*closedlist_result) );
362 m_ClosedList.erase( closedlist_result );
370 }
371
373 if( openlist_result != m_OpenList.end() )
374 {
376 FreeNode( (*openlist_result) );
377 m_OpenList.erase( openlist_result );
383 make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );//堆结构破坏，重新调整堆
384
385 }
388 m_OpenList.push_back( (*successor) );
391 push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
393 }
397 m_ClosedList.push_back(n);
399 }
401 return m_State;
403 }
由于这段代码比较复杂，因为篇幅原因把源代码处的注释给删除了，可参考源码；每次搜索会根据一定的策略搜当前区域的后继区域，这里的策略一般是类似九宫格，根据当前区域搜周围八个方向区域，把检查合法的区域(不合法的区域如不能经过，该节点的parent节点不再检查)加入到当前区域的m_Successors：
407 bool AddSuccessor( UserState &State )
408 {
409 Node *node = AllocateNode();
411 if( node )
412 {
413 node->m_UserState = State;
415 m_Successors.push_back( node );
417 return true;
418 }
420 return false;
421 }
当执行完n->m_UserState.GetSuccessors后，即把n的八个可能的successors给找到；如果执行失败，则267~284行处理本次寻路失败，释放相关的内存并返回，一般来说不太可能出现这种情况，一般返回true为多，这里假设true的情况；
接着处理依次successors，根据公式F(n)= G(n) + H(n)开始评估当前节点到后继节点的代价：n->g + n->m_UserState.GetCost((*successor)->m_UserState)，这里假设GetCost返回为1。
遍历m_OpenList判断当前的successor是否在其中，若在则判断：if((*openlist_result)->g <= newg)成立则放弃该节点；接着同m_OpenList上面的逻辑判断m_ClosedList；若都不在其中或者g更小则更新successor的parent/g/h/f；若更小则删除m_OpenList或者m_ClosedList中的节点，并插入到m_OpenList，最后更新m_ClosedList，即把n放进去：
351 (*successor)->parent = n;
352 (*successor)->g = newg;
353 (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState );
354 (*successor)->f = (*successor)->g + (*successor)->h;
以上是在搜索节点时的过程，一个节点能不能被搜到是由用户提供的接口去判断，比如旁边有阻挡过不去，那就不应该成为后续的successor节点，之后再根据算法第四点判断g的值，能不能进入m_OpenList，最后调整m_OpenList小根堆结构。
对于搜索到目标节点的情况一：
218 if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
219 {
222 m_Goal->parent = n->parent;
223 m_Goal->g = n->g;
224
227 if(false == n->m_UserState.IsSameState(m_Start->m_UserState))
228 {
229 FreeNode(n);
232 Node *nodeChild = m_Goal;
233 Node *nodeParent = m_Goal->parent;
234
235 do
236 {
237 nodeParent->child = nodeChild;
239 nodeChild = nodeParent;
240 nodeParent = nodeParent->parent;
242 }
243 while( nodeChild != m_Start );
245 }
246
248 FreeUnusedNodes();//释放m_ClosedList和m_ClosedList
250 m_State = SEARCH_STATE_SUCCEEDED;
252 return m_State;
253 }
暂时不太明白这句if(false == n->m_UserState.IsSameState(m_Start->m_UserState))执行的意义，如注释说明A special case is that the goal was passed in as the start state so handle that here；如果从代码实现角度分析，当n与m_Goal相等的条件成立，m_Goal作为n->parent后继节点，但算法实现的其他接口中，只使用到child，类似单链表那种取next节点；而n->m_UserState.IsSameState(m_Start->m_UserState)语句成立的条件是起始点和目标点是同一个节点，否则只返回false；接着会根据parent对链表逆置成正序操作：
232 Node *nodeChild = m_Goal;
233 Node *nodeParent = m_Goal->parent;
235 do
236 {
237 nodeParent->child = nodeChild;
239 nodeChild = nodeParent;
240 nodeParent = nodeParent->parent;
242 }
243 while(nodeChild != m_Start);
以上是整个算法的实现，但还是要结合具体的业务来使用，实现相关的接口；一开始只有起始点在m_OpenList，初始化时它的g=0/h=distance(start, goal)/f=g+h，当开始SearchStep时，pop出m_OpenList，并找出自己的successors并依次处理等逻辑。
网上类似的原理分析资料挺多的，包括对于优化相关的，选择哪种评估函数，避免抖动情况和平滑处理等，不再说明。
参考资料：
浅谈路径规划算法
A星算法详解
recast源码解析
a * ( a-star ) 搜索算法实现原理
A*寻路 -- 更加真实 的路径（一）
寻路优化（一）——二维地图上A*启发函数的设计探索
网友评论