a星算法

/*
 * =====================================================================================
 *
 *       Filename:  astar.h
 *
 *    Description:  A*寻路算法
 *
 *        Version:  1.0
 *        Created:  08/19/2013 06:08:20 AM
 *       Compiler:  g++
 *
 *         Author:  xch
 *   Organization:  lucky
 *
 * =====================================================================================
 */

#include<vector>
#include <math.h>
#include <cstddef>
#include <stdlib.h>
#include <strings.h>
#include <stdio.h>

typedef long long           int64;
typedef int                 int32;
typedef short               int16;
typedef char                int8;
typedef unsigned long long  uint64;
typedef unsigned int        uint32;
typedef unsigned short      uint16;
typedef unsigned char       uint8;

/**
 * \brief 默认寻路半径
 */
#define DEFAULT_MAX_RADIUS (20)
/**
 * \brief 开启路径重用
 */
#define ENABLE_PATH_REUSE (1)

struct nPos
{
 
public:
 nPos()
 {
  x = 0;
  y = 0;
 }
 
 nPos(const uint16 x, const uint16 y)
 {
  this->x = 0;
  this->y = 0;
 }

      nPos & operator= (const nPos &pos)
        {
  x += pos.x;
  y += pos.y;
  return *this;
 }

        inline const nPos& operator- (const nPos &pos)
 {
  x -= pos.x;
  y -= pos.y;
  return *this;
        }

 
 inline const nPos& operator+ (const nPos &pos)
 {
  x += pos.x;
  y += pos.y;
  return *this;
 }

 inline const bool operator== (const nPos &pos) const
 {
  return pos.x == this->x && pos.y == this->y;
 }
 

 inline const bool operator!= (const nPos &pos) const
 {
  return (this->x != pos.x || this->y != pos.y);
 }
 
public:
 uint16 x;
 uint16 y;
};

const nPos  nInvalidPos((uint16)-1, (uint16)-1);

 

struct nPathPoint
{
 nPos pos;
 unsigned int f;
 unsigned int g;
 nPathPoint *father;
};

/
typedef enum { ASTAR_SUCCESS = 0, ASTAR_UNREACHABLE, ASTAR_TOO_FAR } astar_res_type;
enum AStarResult {  ASTAR_NONE_OPT, ASTAR_SUCCESS_OPT, ASTAR_UNREACHABLE_OPT, ASTAR_TOO_FAR_OPT, ASTAR_MAX_OPT };

/**
 * \brief 用于偏移计算的坐标值
 */
struct nAdjust
{
 int x;  /**< 横坐标*/
 int y;  /**< 纵坐标*/
};

const nPos NullPos((uint16)-1,(uint16)-1);

 

/**
 * \brief A* 寻路接口
 *
 */
class AStar
{
 public:
  AStar();
  virtual ~AStar();

 public:
   uint8 goToFindPath(const nPos &srcPos, const nPos &destPos);
  // bool goToFindDirect(const nPos &srcPos, const nPos &destPos, unsigned char stepValue=1);
   void setRefind();
   int setMaxRadius(int newRadius);
   virtual bool moveable(const nPos &referPos, bool dynamic = false) = 0;
   virtual bool move(const nPos &newPos) = 0;
   virtual bool move(const int &direct, const int &step) = 0;
   virtual bool getNotBlockSurroundPos(const nPos &dstPos, nPos &result) const = 0;
   virtual bool isFullBlock(const nPos &p) = 0;
 private:
   AStarResult findPath( const nPos &fromPos, const nPos &toPos);
   inline uint32 calH(const nPos &fromPos, const nPos &toPos);
 private:
                typedef std::vector<nPos> Path;
  Path m_path;
         bool m_bRefind;
  uint32 m_maxRadius;
  uint32 max_radius;
 public:
  bool findReachablePos(nPos &pos, const unsigned int stepValue, bool dynamic);
  typedef  std::vector<unsigned int > dismap_type;
  typedef  std::vector<nPathPoint > stack_type;
  typedef  std::vector<nPathPoint* > stack_heap_type;
  void dumpHeap(stack_heap_type &stack_heap);
  void intoHeap(stack_heap_type &stack_heap, nPathPoint *newPoint);
  nPathPoint *exitHeap(stack_heap_type &stack_heap);

};

/* \brief A* 地图拓展节点
 *
 */
struct PathPoint {
 PathPoint() : pos(0,0), father(nInvalidPos), g(0), h(0), state(NS_NONE) {};
 PathPoint( const nPos &_pos ) : pos(_pos), father(nInvalidPos), g(0), h(0), state(NS_NONE) {};
 // 注意下面这两个重载运算符比较的东西不一样
 bool operator < ( const PathPoint &point ) { return g+h < point.g+point.h; }
 bool operator == ( const PathPoint &point ) { return pos == point.pos; }

 nPos pos;
 nPos father;
 uint32 g;
 uint32 h;

 enum NodeState { NS_NONE, NS_OPENLIST, NS_CLOSELIST, NS_BLOCKED, NS_MAX };
 uint32 state;
};
const PathPoint NullPathPoint(nInvalidPos);
//===================================================================================

/**
 * \brief 节点地图
 *
 * 用节点坐标x,y直接索引到PathPoint存储位置。
 * 比较占地方，但是免去搜索操作。
 * 需要预设存储大小
 *
 */
class NodeMap {
 public:
  NodeMap( const nPos &source, uint32 radius = DEFAULT_MAX_RADIUS );
  inline void reset( const nPos &source, uint32 radius );
  inline PathPoint* checkAndGet( const nPos &pos );
  inline PathPoint* checkAndGet( const uint32 &x, const uint32 &y);

 private:
  inline uint32 calIndex(const uint32 &x, const uint32 &y) const;
  inline void resize( uint32 newSize );
  inline uint32 getSize() const;
  inline bool checkInRangeX(const uint32 &x) const;
  inline bool checkInRangeY(const uint32 &y) const;

 private:
  typedef std::vector<PathPoint> NodeMapType;
  NodeMapType m_nodeMap;

  uint32 m_radius;
  nPos m_sourcePos;
};
//===================================================================================

/**
 * \brief 二叉堆
 * T 需要支持 operator < 操作
 *
 */
template < typename T >
class BinaryHeap{
 public:
  BinaryHeap(){};
  inline bool putNodeIn( T *node );
  inline T* popRootNodeOut ();
  inline void clear();

 private:
  inline const uint32 getFatherIndex( const uint32 nodeIndex ) const;
  inline const uint32 get1stChildIndex( const uint32 nodeIndex ) const;
  inline const uint32 get2ndChildIndex( const uint32 nodeIndex ) const;

 protected:
  std::vector<T*> m_vec;
};
//===================================================================================

/**
 * \brief OpenList
 * 二叉堆实现最小F值节点查找
 *
 */
class OpenList {
 public:
  OpenList( NodeMap *pNodeMap );
  inline bool add( const PathPoint &point );
  inline bool checkAndUpdate( const PathPoint &point );
  inline PathPoint popMinF();
  inline void clear();
 private:
  NodeMap *m_pNodeMap;
  BinaryHeap<PathPoint> m_binaryHeap;
};
//===================================================================================

/**
 * \brief CloseList
 *
 */
class CloseList {
 public:
  CloseList( NodeMap *pNodeMap );
  inline bool add( const PathPoint &point );
  inline bool checkAndUpdate( const PathPoint &point );
  inline void clear();
 private:
  NodeMap *m_pNodeMap;
};

 

/*
 * =====================================================================================
 *
 *       Filename:  astar.cpp
 *
 *    Description:  A*寻路算法实现
 *
 *        Version:  1.0
 *        Created:  08/19/2013 06:23:17 AM
 *       Compiler:  g++
 *
 *         Author:  xch
 *   Organization:  lucky
 *
 * =====================================================================================
 */

#include"astar.h"

/**
 * \brief A*算法最大拓展节点
 * 在无可达路径时候，A*会拓展地图内的所有点，消耗过多资源。
 * 经过统计，一般可以寻到路径的平均值不会超过50，超过100的值很少来；
 * 失败情况下平均都会在1000以上（生存副本中大寻路半径情况统计）。
 * 可以根据使用情况调整。
 *
 */

#define MAX_EXTENDED_NODE_NUM (150)

#define JUDGE(startpos, endpos) (std::max(abs(startpos.x - endpos.x), abs(startpos.y - endpos.y)))

extern nPos MAP_MAX_POS;

//----------------------------
// AStar2 实现
//----------------------------
AStar::AStar() : m_bRefind(false), max_radius(DEFAULT_MAX_RADIUS)
{
}

AStar::~AStar()
{
}

 

 

uint8 AStar::goToFindPath(const nPos &srcPos, const nPos &destPos)
{
 if(!m_path.empty())
 {
  if(m_path[0] != destPos || m_path.size() == 1)
  {
   m_path.clear();
  }
  else
  {
   nPos nextPos = m_path.back();
   m_path.pop_back();
 //  printf("==========AI======[%u]======\r\n",(uint16)m_path.size());
   if(move(nextPos))
   {
    return ASTAR_SUCCESS;
   }
  }
 }else
 {
  findPath(srcPos, destPos);
 }
 return false;
}

 

void AStar::setRefind()
{
 m_bRefind = true;
}

int AStar::setMaxRadius(int newRadius)
{
 return (m_maxRadius = newRadius);
}

uint32 AStar::calH(const nPos &fromPos, const nPos &toPos)
{
 return abs(fromPos.x-toPos.x) + abs(fromPos.y-toPos.y);
}

AStarResult AStar::findPath(const nPos &fromPos, const nPos &toPos)
{

 m_path.clear();

 if ( fromPos == toPos )
 {
  return ASTAR_NONE_OPT;
 }

 // 检查完全阻塞，是否扩展周围节点作为目标
// if ( isFullBlock(toPos) )
// {
//  return ASTAR_UNREACHABLE;
// }

 // 距离、步长检查
 unsigned int dx = abs(fromPos.x - toPos.x);
 unsigned int dy = abs(fromPos.y - toPos.y);
 if ( dx > DEFAULT_MAX_RADIUS || dy > DEFAULT_MAX_RADIUS )
 {
  return ASTAR_TOO_FAR_OPT;
 }

 // A* 算法开始
 static const int offset[8][3] =
 {   // 偏移计算矩阵(x,y,weight)
  {-1,1,1}, {0,1,1}, {1,1,1},
  {-1,0,1},    {1,0,1},
  {-1,-1,1}, {0,-1,1}, {1,-1,1},
 };
 static NodeMap nodeMap(fromPos, DEFAULT_MAX_RADIUS);
 static OpenList openList(&nodeMap);   // OpenList
 static CloseList closeList(&nodeMap);  // CloseList
 m_maxRadius = DEFAULT_MAX_RADIUS;
 nodeMap.reset( fromPos, m_maxRadius );
 openList.clear();
 closeList.clear();

 PathPoint startPoint;
 startPoint.pos = fromPos;
 startPoint.father = NullPos;
 openList.add(startPoint);

 AStarResult res = ASTAR_NONE_OPT;
 unsigned int loopCounter = 0;

 while ( loopCounter < MAX_EXTENDED_NODE_NUM )
 {
  // 弹出最小F节点
  PathPoint nowPoint = openList.popMinF();
  if ( nowPoint == NullPathPoint )
  {
   res = ASTAR_UNREACHABLE_OPT;
   break;
  }
  closeList.add(nowPoint);

  // 拓展周围8个节点
  for ( int i=0; i<8; i++)
  {
   PathPoint point;
   point.pos.x = nowPoint.pos.x + offset[i][0];
   point.pos.y = nowPoint.pos.y + offset[i][1];
   point.father = nowPoint.pos;
   point.g = nowPoint.g + offset[i][2];
   point.h = calH(point.pos, toPos);

   // 检查节点合法性
   PathPoint *p = nodeMap.checkAndGet(point.pos);
   if ( !p ) continue;

   // 是否到达目标点
   if ( abs((int) point.pos.x - (int)toPos.x ) < 1 &&
     abs( (int)point.pos.y - (int)toPos.y ) < 1 )
   {

    closeList.add(point);
    // 构造一个toPos的PathPoint节点
    point.g += 1;
    point.h = 0;
    point.father = point.pos;
    point.pos = toPos;
    closeList.add(point);

    res = ASTAR_SUCCESS_OPT;
    break;
   }

   // 是否阻挡
   if (moveable(point.pos, false) )
   {
    continue;
   }

   // 是否已经在OpenList
   if ( openList.checkAndUpdate(point) )
   {
    continue;
   }

   // 是否已经在CloseList
   if ( closeList.checkAndUpdate(point) )
   {
    continue;
   }

   // 扩展节点
   openList.add(point);
  }

  if ( res != ASTAR_NONE_OPT )
  {
   break;
  }

  loopCounter++;
 }

 // 回退生成路径
 if (res == ASTAR_SUCCESS_OPT)
 {
  PathPoint *pPoint = nodeMap.checkAndGet(toPos);
  while( pPoint && pPoint->pos != fromPos )
  {
   m_path.push_back( pPoint->pos );
   pPoint = nodeMap.checkAndGet( pPoint->father );
  }
 }

 return res;
}

//===================================================================================

//----------------------------
// NodeMap 实现
//----------------------------
NodeMap::NodeMap(const nPos &source, uint32 radius) : m_radius(radius), m_sourcePos(source)
{
 m_nodeMap.clear();
 m_nodeMap.resize(getSize());
}

void NodeMap::reset(const nPos &source, uint32 radius)
{
 m_sourcePos = source;
 m_radius = radius;
 m_nodeMap.resize(getSize());
 //这两行完成的功能就是把PathPoint的值恢复成默认值.PathPoint::father没有设置成nNullPos,但是这样不会影响计算过程.
 bzero(&(*m_nodeMap.begin()), sizeof(PathPoint) * m_nodeMap.size());
}

PathPoint* NodeMap::checkAndGet(const nPos &pos)
{
 return checkAndGet(pos.x, pos.y);
}

PathPoint* NodeMap::checkAndGet(const uint32 &x, const uint32 &y)
{
 if (nInvalidPos == m_sourcePos || 0 == m_radius) {
  return NULL;
 }

 if (!checkInRangeX(x))
 {
  return NULL;
 }
 if (!checkInRangeY(y))
 {
  return NULL;
 }
 return &(m_nodeMap[calIndex(x,y)]);
}

bool NodeMap::checkInRangeX(const uint32 &x) const
{
 uint32 maxX = m_sourcePos.x + m_radius;
 uint32 minX = m_sourcePos.x > m_radius ? m_sourcePos.x-m_radius : 0;

 if (x > maxX || x < minX) {
  return false;
 }
 else{
  return true;
 }
}

bool NodeMap::checkInRangeY(const uint32 &y) const
{
 uint32 maxY = m_sourcePos.y + m_radius;
 uint32 minY = m_sourcePos.y > m_radius ? m_sourcePos.y-m_radius : 0;

 if (y > maxY || y < minY) {
  return false;
 }
 else {
  return true;
 }
}

uint32 NodeMap::calIndex(const uint32 &x, const uint32 &y) const
{
 uint32 dx = x - (m_sourcePos.x - m_radius);
 uint32 dy = y - (m_sourcePos.y - m_radius);

 return dx + dy * 2 * m_radius;
}

uint32 NodeMap::getSize() const
{
 //现在m_radius是没有变化的,所以size没有变化
 return (2*m_radius+1) * (2*m_radius+1);
}

//----------------------------
// 二叉堆 实现
//----------------------------

// putNodeIn ＆ popRootNodeOut 保证在比较相同的情况下，新进入的节点在堆的更上层
 template < typename T >
bool BinaryHeap<T>::putNodeIn(T *node)
{
 m_vec.push_back(node);
 if (m_vec.size() <= 1) {
  return true;
 }

 uint32 nowIndex = m_vec.size()-1;
 while (nowIndex != 0) {
  uint32 fatherIndex = getFatherIndex(nowIndex);

  if (!(*m_vec[fatherIndex] < *m_vec[nowIndex])) {
   T* tmp = m_vec[nowIndex];
   m_vec[nowIndex] = m_vec[fatherIndex];
   m_vec[fatherIndex] = tmp;

   nowIndex = fatherIndex; 
  }
  else {
   break;
  }
 }
 return true;
}

 template < typename T >
T* BinaryHeap<T>::popRootNodeOut()
{
 if (m_vec.empty()) {
  return NULL;
 }

 T* ret = m_vec[0];

 m_vec[0] = m_vec.back();
 m_vec.pop_back();

 if (m_vec.size() <= 1) {
  return ret;
 }

 uint32 nowIndex = 0;
 uint32 maxIndex = m_vec.size() - 1;
 while (true) {
  uint32 firstChildIndex = get1stChildIndex(nowIndex);
  uint32 secondChildIndex = get2ndChildIndex(nowIndex);
  uint32 minChildIndex = nowIndex;

  if (secondChildIndex > maxIndex) {
   if (firstChildIndex > maxIndex) {
    break;
   }
   else {
    minChildIndex = firstChildIndex;
   }
  }
  else {
   minChildIndex = *m_vec[firstChildIndex] < *m_vec[secondChildIndex] ?
    firstChildIndex : secondChildIndex;
  }

  if (*m_vec[minChildIndex] < *m_vec[nowIndex])
  {
   T* tmp = m_vec[nowIndex];
   m_vec[nowIndex] = m_vec[minChildIndex];
   m_vec[minChildIndex] = tmp;

   nowIndex = minChildIndex;
  }
  else {
   break;
  }
 }

 return ret;
}

template < typename T >
const uint32 BinaryHeap<T>::getFatherIndex(const uint32 nodeIndex) const
{
 if (0 == nodeIndex)
  return 0;

 return (nodeIndex - 1) / 2;
}

template < typename T >
const uint32 BinaryHeap<T>::get1stChildIndex(const uint32 nodeIndex) const
{
 return nodeIndex * 2 + 1;
}

template < typename T >
const uint32 BinaryHeap<T>::get2ndChildIndex(const uint32 nodeIndex) const
{
 return nodeIndex * 2 + 2;
}

 template < typename T >
void BinaryHeap<T>::clear()
{
 m_vec.clear();
}

//----------------------------
// OpenList 实现
//----------------------------
OpenList::OpenList(NodeMap *pNodeMap) : m_pNodeMap(pNodeMap)
{
}

bool OpenList::add(const PathPoint &point)
{
 PathPoint *pPoint = m_pNodeMap->checkAndGet(point.pos);
 if (!pPoint) {
  return false;
 }
 if (PathPoint::NS_NONE != pPoint->state) {
  return false;
 }

 pPoint->pos = point.pos;
 pPoint->father = point.father;
 pPoint->g = point.g;
 pPoint->h = point.h;
 pPoint->state = PathPoint::NS_OPENLIST;

 m_binaryHeap.putNodeIn(pPoint);

 return true;
}

bool OpenList::checkAndUpdate(const PathPoint &point)
{
 PathPoint *pPoint = m_pNodeMap->checkAndGet(point.pos);
 if (!pPoint) {
  return false;
 }
 if (PathPoint::NS_OPENLIST != pPoint->state) {
  return false;
 }

 if (pPoint->g + pPoint->h > point.g + point.h) {
  pPoint->g = point.g;
  pPoint->h = point.h;
  pPoint->father = point.father;

  m_binaryHeap.putNodeIn(pPoint);
 }

 return true;
}

PathPoint OpenList::popMinF()
{
 PathPoint *pPoint = NULL;
 // 因为add会加入指向相同位置的指针到二叉堆里面，
 // 在pop掉一个后逻辑上就不应该在OpenList中了，
 // 所以在二叉堆里面指向没有标记OpenList对象的指针需要全部忽略掉。
 do {
  pPoint = m_binaryHeap.popRootNodeOut();
  if (!pPoint)
  {
   return NullPathPoint;
  }
 }
 while (PathPoint::NS_OPENLIST != pPoint->state);

 // 出去的时候一定要标记
 pPoint->state = PathPoint::NS_NONE;

 return *pPoint;
}

void OpenList::clear() {
 m_binaryHeap.clear();
}

//----------------------------
// CloseList 实现
//----------------------------
CloseList::CloseList(NodeMap *pNodeMap) : m_pNodeMap(pNodeMap)
{
}

bool CloseList::add(const PathPoint &point)
{
 PathPoint *pPoint = m_pNodeMap->checkAndGet(point.pos);
 if (!pPoint) {
  return false;
 }
 if (PathPoint::NS_NONE != pPoint->state) {
  return false;
 }

 pPoint->pos = point.pos;
 pPoint->father = point.father;
 pPoint->g = point.g;
 pPoint->h = point.h;
 pPoint->state = PathPoint::NS_CLOSELIST;

 return true;
}

bool CloseList::checkAndUpdate(const PathPoint &point)
{
 PathPoint *pPoint = m_pNodeMap->checkAndGet(point.pos);
 if (!pPoint) {
  return false;
 }
 if (PathPoint::NS_CLOSELIST != pPoint->state) {
  return false;
 }

 if (pPoint->g + pPoint->h > point.g + point.h) {
  pPoint->g = point.g;
  pPoint->h = point.h;
  pPoint->father = point.father;
 }

 return true;
}

void CloseList::clear()
{
}

bool AStar::findReachablePos(nPos &pos, const unsigned int stepValue, bool dynamic)
{
 const nAdjust adjust[8] =
 {
  { 1, 0 },
  { 0, -1 },
  { 0, 1 },
  {  -1, 0 },
  { 1, -1 },
  {  -1, -1 },
  {  -1, 1 },
  { 1, 1 },
 };

 unsigned int step = 1;
 for(;step <= stepValue;step++)
 {
  for(int i = 0; i < 8; i++)
  {
   nPos tempPos = pos;
   tempPos.x += adjust[i].x * step;
   tempPos.y += adjust[i].y * step;

   if (!moveable(tempPos, dynamic))
   {
    continue;
   }

   pos = tempPos;
   return true;
  }
 }

 return false;

}

void AStar::dumpHeap(stack_heap_type &stack_heap)
{
 for (unsigned int index = 0; index < stack_heap.size(); index++)
 {
  printf("\t\tf:%u, pos:(%u,%u), g:%u\n", stack_heap[index]->f, stack_heap[index]->pos.x, stack_heap[index]->pos.y, stack_heap[index]->g);
 }
}

 

void AStar::intoHeap(stack_heap_type &stack_heap, nPathPoint *newPoint)
{
   stack_heap.push_back(newPoint);
   unsigned int endpoint = stack_heap.size() - 1;

   for(;endpoint != 0;)
   {
    nPathPoint *e = stack_heap[endpoint];
    nPathPoint *p = stack_heap[endpoint/2];
    if (e->f < p->f)
    {
     nPathPoint *temp = e;
     stack_heap[endpoint] = p;
     stack_heap[endpoint/2] = temp;
     endpoint /= 2;
    }
    else
    {
     break;
    }
  }
}

 

nPathPoint* AStar::exitHeap(stack_heap_type &stack_heap)
{
 if (stack_heap.empty())
 {
  return NULL;
 }
 nPathPoint *ret = stack_heap[0];
 stack_heap[0] = stack_heap[stack_heap.size()-1];
 stack_heap.pop_back();

 unsigned int endpoint = 0;
 for(;endpoint*2+2 < stack_heap.size();)
 {
  nPathPoint *e = stack_heap[endpoint];
  nPathPoint *pl = stack_heap[endpoint*2+1];
  nPathPoint *pr = stack_heap[endpoint*2+2];

  if (e->f > pl->f)
  {
   nPathPoint *temp = e;
   stack_heap[endpoint] = pl;
   stack_heap[endpoint*2+1] = temp;

   endpoint = endpoint * 2 + 1;
  }
  else if (e->f > pr->f)
  {
   nPathPoint *temp = e;
   stack_heap[endpoint] = pr;
   stack_heap[endpoint*2+2] = temp;
   endpoint = endpoint * 2 + 2;
  }
  else
  {
   break;
  }
 }
 return ret;
}

 

Makefile
 all:
 g++ -fpic -shared astar.cpp -o libastar.so -lm
clean:
 rm -f *.so