【转贴】A*算法的泛型抽象实现

【转贴】A*算法的泛型抽象实现

#ifndef __ASTAR_SERACH_ALGO_H__
#define  __ASTAR_SERACH_ALGO_H__

#include  < Algorithm >
    
namespace  fredazsq
{    
namespace  linghuye
{    
    
/**/ /**/ /**/ /* ************************************************
** 寻路节点
************************************************ */
template < typename TNodeLocation >
struct  TAXNode
{    
     float        G, F;
    TNodeLocation  pos;
    TAXNode *    pParentNode;
     bool        bClosed;         //  使用状态表明节点处于开放或关闭列表.
    
     bool   operator < ( const  TAXNode &  node)  const
     {    
         return  pos  <  node.pos;
    }     
} ;    
    
/**/ /**/ /**/ /* *****************************************************************************
**  封装A*寻路算法
*******************************************************************************
**  作为棋盘信息提供类TField必须实现下列函数:
**  1. bool IsFieldThrough(TNodeLocation pos) const;                                    在 pos 位置是否为空,可通行.
**  2. float EvaluateStepCost(TNodeLocation posFrom, TNodeLocation posTo, TNodeLocation posLastStep) const;        计算pos1,pos2到单步代价.
**  3. float EvaluateProbableCost(TNodeLocation posFrom, TNodeLocation posTo) const;    计算pos1,pos2到估算代价.
**  4. TNodeLocation* QueryNeiborLocations(TNodeLocation pos, int& nNodeCount); const    询问pos的相邻可通节点集
*******************************************************************************
**  TNodeLocation,节点位置
**  1.支持<,==,=操作符,如int.
*******************************************************************************
**  0. 初始化时传入棋盘信息提供类的实例引用.
**  1. 使用主函数SearchPath进行搜索,使用QueryResultPath取得结果.
***************************************************************************** */
template < class  TField,  class  TNodeLocation >
struct  CAStar2DAlgo
{    
public :
    typedef std::vector < TNodeLocation >     CResultPath;
    typedef TAXNode < TNodeLocation >         CAXNode;
    typedef std:: set < CAXNode >             CAXNodeSet;
    
     struct  CAXNodePriorityQueue             //  优先队列 
     {    
        std::vector < CAXNode *>  m_heap;
        
         static   bool  AXNodeCostSortPred(CAXNode *  n1, CAXNode *  n2)
         {    
             return  (n1 -> G  +  n1 -> F)  >  (n2 -> G  +  n2 -> F);
        }
        
         void  push(CAXNode *  pNode)
         {
            m_heap.push_back(pNode);
            std::push_heap(m_heap.begin(), m_heap.end(),  & CAXNodePriorityQueue::AXNodeCostSortPred);
        }
        
        CAXNode *  pop()
         {    
            CAXNode *  pNode  =  m_heap.front();
            std::pop_heap(m_heap.begin(), m_heap.end(),  & CAXNodePriorityQueue::AXNodeCostSortPred);
            m_heap.pop_back();
             return  pNode;
        }
        
         bool  empty()
         {
             return  m_heap.empty();
        }
        
         void  resort(CAXNode *  pNode)
         {    
            std::vector < CAXNode *> ::iterator it  =  std::find(m_heap.begin(), m_heap.end(), pNode);
            std::push_heap(m_heap.begin(), it,  & CAXNodePriorityQueue::AXNodeCostSortPred);
        }
        
         void  clear()
         {
            m_heap.clear();
        }
    } ;    
        
public :    
     const  TField &  m_field;
    TNodeLocation  m_posTarget;
    CAXNode *  m_ptrTargetNode;                     //  Result Node
        
    CAXNodeSet m_setNodes;                         //  All nodes in use 
    CAXNodePriorityQueue m_queOpenNodes;         //  Cost sorted open list
        
public :    
    CAStar2DAlgo( const  TField &  field) : m_field(field), m_ptrTargetNode(NULL)
     {        
    }     
        
     ~ CAStar2DAlgo()
     {    
    }     
        
     void  Reset()
     {    
        m_setNodes.clear();
        m_queOpenNodes.clear();
        m_ptrTargetNode  =  NULL;
    }     
        
public :    
    inline CAXNode *  ClaimNode(TNodeLocation pos, CAXNode *  pParentNode  =  NULL)
     {    
        CAXNode node;
        node.pos  =  pos;
        node.F  =  node.G  =   0 ;
        node.bClosed  =   false ;     //  Default is open
        node.pParentNode  =  pParentNode;
         return   & ( * m_setNodes.insert(node).first);
    }     
        
     bool  SearchPath(TNodeLocation posStart, TNodeLocation posTarget)
     {    
        CAXNode *  pCurNode;
        m_posTarget  =  posTarget;
        
         //  Add start node into open list
        CAXNode *  pStartNode  =  ClaimNode(posStart);
        m_queOpenNodes.push(pStartNode);
        
         while ( ! m_queOpenNodes.empty())
         {    
             //  Pick lowest cost node
            pCurNode  =  m_queOpenNodes.pop();
            
             //  Check search target
             if (pCurNode -> pos  ==  m_posTarget)
             {
                m_ptrTargetNode  =  pCurNode;
                 return   true ;
            }
            
             //  Switch node from OpenList to CloseList
            pCurNode -> bClosed  =   true ;
            ProcessNeighborNodes(pCurNode);
        }
        
         return   false ;
    }
        
     void  ProcessNeighborNodes(CAXNode *  pCurNode)
     {    
        CAXNode tempNode;
         int  nNodeCount  =   0 ;
        TNodeLocation *  pLocs  =  m_field.QueryNeiborLocations(pCurNode -> pos, nNodeCount);
        TNodeLocation posLastStep  =  pCurNode -> pParentNode ?  pCurNode -> pParentNode -> pos : pCurNode -> pos;
        
         //  For each neibors 
         for ( int  i  =   0 ; i  <  nNodeCount;  ++ i)
         {    
            tempNode.pos  =  pLocs[i];
             if (m_field.IsFieldThrough(tempNode.pos))
             {    
                CAXNodeSet::iterator it  =  m_setNodes.find(tempNode);
                 if (it  ==  m_setNodes.end())
                 {    
                    CAXNode *  pNewNode  =  ClaimNode(tempNode.pos, pCurNode);
                    pNewNode -> G  =  pCurNode -> G  +  m_field.EvaluateStepCost(pCurNode -> pos, pNewNode -> pos, posLastStep);
                    pNewNode -> F  =  m_field.EvaluateProbableCost(pNewNode -> pos, m_posTarget);
                    m_queOpenNodes.push(pNewNode);
                }     
                 else   if ( ! it -> bClosed)
                 {    
                    CAXNode &  node  =   * it;
                     float  G  =  pCurNode -> G  +  m_field.EvaluateStepCost(pCurNode -> pos, node.pos, posLastStep);
                     if (G  <  node.G)
                     {    
                        node.G  =  G;
                        node.pParentNode  =  pCurNode;
                        m_queOpenNodes.resort( & node);
                    }
                }
            }
        }
    }
    
     /**/ /**/ /**/ /* *****************************************************************
    **  取回最终结果
    ***************************************************************** */
     bool  QueryResultPath(CResultPath &  path)
     {    
         if ( ! m_ptrTargetNode)  return   false ;
        
        CAXNode *  pNode  =  m_ptrTargetNode;
         while (pNode)
         {    
            path.push_back(pNode -> pos);
            pNode  =  pNode -> pParentNode;
        }
         return   true ;
    }
} ;

/**/ /**/ /**/ /* ******************************************************************************
**  TNodeLocation,节点位置
**  1.支持<,== 操作符号.
******************************************************************************* */
template < typename TNodeLocation >
struct  CShisenFieldImpl
{    
    TNodeLocation *  QueryNeiborLocations(TNodeLocation pos,  int &  nRetCount)  const
     {    
         static   const   int  dy[ 4 ]  =   {  1 ,  0 ,  0 ,  - 1  } ;
         static  TNodeLocation posNeibors[ 4 ];     //  Single thread
                
        nRetCount  =   4 ;
         for ( int  i  =   0 ; i  <   4 ; i ++ )
         {
            posNeibors[i].x  =  pos.x  +  dx[i];
            posNeibors[i].y  =  pos.y  +  dy[i];
        }
         return  posNeibors;
    }
    
     float  EvaluateStepCost(TNodeLocation posFrom, TNodeLocation posTo, TNodeLocation posLast)  const
     {    
         if (((posFrom.x - posTo.x)   &&   (posLast.y - posFrom.y))   ||   
           ((posFrom.y - posTo.y)   &&   (posLast.x - posFrom.x)))
         {    
             return   100 ;
        }
         else   return   1 ;
    }
    
     float  EvaluateProbableCost(TNodeLocation pos1, TNodeLocation pos2)  const
     {
         return   0 ;
    }
    
/**/ /**/ /**/ /*     平滑优化
    float EvaluateStepCost(MY_POSITION posFrom, MY_POSITION posTo, MY_POSITION posLast) const
    {    
        int cx1 = posTo.x - posFrom.x;
        int cy1 = posTo.y - posFrom.y;
        int cx2 = posFrom.x - posLast.x;
        int cy2 = posFrom.y - posLast.y;
        return ((cx1 == cx2  &&  cy1 == cy2)? 0 : 10) + ((cx1  &&  cy1)? 14 : 10);
    }
*/     
} ;    
    
/**/ /**/ /**/ /* ******************************************************************************
**  最小路径
****************************************************************************** */
template < typename TNodeLocation >
struct  CShortestPathFieldImpl
{    
    TNodeLocation *  QueryNeiborLocations(TNodeLocation pos,  int &  nRetCount)  const
     {    
         static   const   int  dx[ 4 ]  =   {  0 ,  - 1 ,  1 ,   0  } ;
         static   const   int  dy[ 4 ]  =   {  1 ,   0 ,  0 ,  - 1  } ;
         static  TNodeLocation posNeibors[ 4 ];     //  Single thread
                
        nRetCount  =   4 ;
         for ( int  i  =   0 ; i  <   4 ; i ++ )
         {
            posNeibors[i].x  =  pos.x  +  dx[i];
            posNeibors[i].y  =  pos.y  +  dy[i];
        }
         return  posNeibors;
    }
    
     float  EvaluateStepCost(TNodeLocation posFrom, TNodeLocation posTo, TNodeLocation posLast)  const
     {    
         return   1 ;
    }
    
     float  EvaluateProbableCost(TNodeLocation pos1, TNodeLocation pos2)  const
     {
         return   0 ;
    }
} ;

/**/ /**/ /**/ /* ******************************************************************************
**  最小路径
****************************************************************************** */
template < typename TNodeLocation >
struct  CGamePathFieldImpl
{    
    TNodeLocation *  QueryNeiborLocations(TNodeLocation pos,  int &  nRetCount)  const
     {    
         static   const   int  dx[ 8 ]  =   {  - 1 ,  0 ,  1 ,  - 1 ,  1 ,  - 1 ,   0 ,   1  } ;
         static   const   int  dy[ 8 ]  =   {   1 ,  1 ,  1 ,   0 ,  0 ,  - 1 ,  - 1 ,  - 1  } ;
         static  MY_POSITION posNeibors[ 8 ];
        
        nRetCount  =   8 ;
         for ( int  i  =   0 ; i  <   8 ; i ++ )
         {
            posNeibors[i].x  =  pos.x  +  dx[i];
            posNeibors[i].y  =  pos.y  +  dy[i];
        }
         return  posNeibors;
    }
    
     float  EvaluateStepCost(TNodeLocation posFrom, TNodeLocation posTo, TNodeLocation posLast)  const
     {    
         if (posFrom.x - posTo.x   &&   posFrom.y - posTo.y)
         {
             return   14 ;
        }
         else   return   10 ;
    }
    
     float  EvaluateProbableCost(TNodeLocation posFrom, TNodeLocation posTo)  const
     {
         return  (abs(posFrom.x - posTo.x)  +  abs(posFrom.y - posTo.y))  *   10 ;
    }
} ;

} //  namespace linghuye
} //  namespace fredazsq

#endif // __FIELD_SERACH_ALGO_H__