【转】A*寻路算法 C++实现

头文件：AStarPathFinding

 #ifndef ASTARPATHFINDING_H
 #define ASTARPATHFINDING_H

 #include <queue>//为了使用优先级队列priority_queue
 #include <stack>
 #include <vector>

 //迷宫地图中节点类型标记
 enum{
  NODE_EMPTY,//可以通过的节点
  NODE_OBSTACLE,//障碍物，不可通过
  NODE_PATH//路径上的点
 };

 //记录路径上的点的坐标
 typedef struct tagpathNode{
  int x,y;
 }PathNode;

 //节点数据结构定义
 typedef struct tagNode{
  int x,y;//当前点在迷宫中的位置坐标
  int g;//起始点到当前点实际代价
  int h;//当前节点到目标节点最佳路径的估计代价
  int f;//估计函数：f = g + h。
  struct tagNode *father;//指向其父节点的指针
 }Node;

 //定义STL优先队列的排序方式
 class HeapCompare_f{
 public:
  bool operator()(Node* x,Node* y) const
  {
   return x->f > y->f;//依据估价函数进行排序：升序排列
  }
 };

 //迷宫寻路：A*算法
 class AStarPathFinding{
 public:

 private:
  char *m_mapFileName;//存储地图信息的文件名
  int m_rows,m_cols;//迷宫的高度和宽度
  int **m_maze;//迷宫布局
  int m_startX,m_startY;//起始点坐标
  int m_endX,m_endY;//目标点坐标
  ],dy[];//8个子节点移动方向：上、下、左、右、左上、右上、右下、左下

  Node *startNode,*endNode;//起始节点和目标节点
  int **m_path;//记录路径信息
  int m_steps;//搜索所花费的总步数

  //OPEN表：采用C++ STL中vector实现优先级队列功能
  //注意：存储的是Node*指针
  std::vector<Node*> OPENTable;
  //CLOSED表：存储的也是Node*指针
  std::vector<Node*> CLOSEDTable;

 public:
  //构造函数
  AStarPathFinding(char* mapFileName);
  ~AStarPathFinding();//析构函数
  void init();//初始化
  //读取地图信息
  bool readMap();
  //寻路主函数
  bool pathFinding();
  //产生路径信息
  void generatePath();
  //打印路径信息
  void printPath();
  //估计当前点到目标点的距离：曼哈顿距离
  int judge(int x,int y);
  //判断某一节点是否合法
  bool isIllegle(int x,int y);
 };

 #endif

源文件：

 #include "AStarPathFinding.h"

 #include <iostream>
 #include <cstdio>
 #include <cmath>
 #include <string>
 #include <fstream>

 using namespace std;

 ;

 AStarPathFinding::AStarPathFinding(char* mapFileName)
 :m_steps()
 {
  m_mapFileName = () * sizeof(char));
  strcpy(m_mapFileName,mapFileName);
 }

 AStarPathFinding::~AStarPathFinding()
 {
  free(m_mapFileName);

  //千万不能有这句代码，因为startNode已加入OPEN表，会在释放OPEN表
  //的时候释放，否则会造成重复释放，出现bug
  //delete startNode;
  delete endNode;

  ////释放迷宫布局数组：注意多维数组空间释放
  ;i < m_rows;++i)
  {
   delete[] m_maze[i];
  }
  delete[] m_maze;

  ;i < m_rows;++i)
  {
   delete[] m_path[i];
  }
  delete[] m_path;

  //释放OPEN表以及CLOSED表内存空间
  vector<Node*>::iterator iter;
  for (iter = OPENTable.begin();iter != OPENTable.end();++iter)
  {
   delete (*iter);
  }
  OPENTable.clear();

  vector<Node*>::iterator iter2;
  for (iter2 = CLOSEDTable.begin();iter2 != CLOSEDTable.end();++iter2)
  {
   delete (*iter2);
  }
  CLOSEDTable.clear();
 }

 void AStarPathFinding::init()
 {
  dx[] =dx[] = dx[] = -;
  dx[] =dx[] = ;
  dx[] =dx[] = dx[] = ;

  dy[] = dy[] = dy[] = -;
  dy[] =dy[] = ;
  dy[] =dy[] = dy[] = ;

  readMap();

  //分配空间
  m_path = new int *[m_rows];
  ;i < m_rows;++i)
  {
   m_path[i] = new int[m_cols];
  }

  startNode = new Node;
  startNode->x = m_startX;
  startNode->y = m_startY;
  startNode->g = ;
  startNode->h = judge(startNode->x,startNode->y);
  startNode->f = startNode->g + startNode->h;
  startNode->father = NULL;

  endNode = new Node;
  endNode->x = m_endX;
  endNode->y = m_endY;
  endNode->father = NULL;
 }

 bool AStarPathFinding::pathFinding()
 {
  //判断起始点和目标点是否是同一点
  if (m_startX == m_endX && m_startY == m_endY)
  {
   cout << "WARNNING : The start point is the same as th destination " << endl;
   return true;
  }

  OPENTable.push_back(startNode);//起始点装入OPEN表

  //对vector中元素进行排序：将最后一个元素加入原本已序的heap内
  push_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());

  Node *tempNode = new Node;

  //开始遍历
  for (;;)
  {
   if (OPENTable.empty())//判断OPEN表是否为空
   {
    cout << "ERROR : unable to find the destination" << endl;
    return false;
   }

   tempNode = OPENTable.front();//注意：OPEN表为空会导致未定义行为
   ++m_steps;
   //将第一个元素移到最后，并将剩余区间重新排序，组成新的heap
   pop_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());
   OPENTable.pop_back();//删除最后一个元素

   //判断是否已经搜寻到目标节点
   if (tempNode->x == m_endX && tempNode->y == m_endY)
   {
    cout << "OK : success to find the destination" << endl;
    endNode->g = tempNode->g;
    endNode->h = tempNode->h;
    endNode->f = tempNode->f;
    endNode->father = tempNode->father;

    generatePath();

    return true;
   }

   ;i < ;++i)//针对每个子节点
   {
    int nextX = tempNode->x + dx[i];
    int nextY = tempNode->y + dy[i];
    if (isIllegle(nextX,nextY))
    {
     //注意：障碍物角落不能直接通过
      == *(*(m_maze + tempNode->x) + nextY) ||
       == *(*(m_maze + nextX) + tempNode->y))
     {
      continue;
     }
     //计算此子节点的g值
     int newGVal;
     if (!dx[i] && !dy[i])//位于对角线上
     {
      newGVal = tempNode->g + ;
     }
     else
      newGVal = tempNode->g + ;

     //搜索OPEN表，判断此点是否在OPEN表中
     vector<Node*>::iterator OPENTableResult;
     for (OPENTableResult = OPENTable.begin();
      OPENTableResult != OPENTable.end();++OPENTableResult)
     {
      if ((*OPENTableResult)->x == nextX &&
       (*OPENTableResult)->y == nextY)
      {
       break;
      }
     }

     //此子节点已经存在于OPEN表中
     if (OPENTableResult != OPENTable.end())
     {
      //OPEN表中节点的g值已经是最优的，则跳过此节点
      if ((*OPENTableResult)->g <= newGVal)
      {
       continue;
      }
     }

     //搜索CLOSED表，判断此节点是否已经存在于其中
     vector<Node*>::iterator CLOSEDTableResult;
     for (CLOSEDTableResult = CLOSEDTable.begin();
      CLOSEDTableResult != CLOSEDTable.end();++CLOSEDTableResult)
     {
      if ((*CLOSEDTableResult)->x == nextX &&
       (*CLOSEDTableResult)->y == nextY)
      {
       break;
      }
     }

     //此节点已经存在于CLOSED表中
     if (CLOSEDTableResult != CLOSEDTable.end())
     {
      //CLOSED表中的节点已经是最优的，则跳过
      if ((*CLOSEDTableResult)->g <= newGVal)
      {
       continue;
      }
     }

     //此节点是迄今为止的最优节点
     Node *bestNode = new Node;
     bestNode->x = nextX;
     bestNode->y = nextY;
     bestNode->father = tempNode;
     bestNode->g = newGVal;
     bestNode->h = judge(nextX,nextY);
     bestNode->f = bestNode->g + bestNode->h;

     //如果已经存在于CLOSED表中，将其移除
     if (CLOSEDTableResult != CLOSEDTable.end())
     {
      delete (*CLOSEDTableResult);
      CLOSEDTable.erase(CLOSEDTableResult);
     }

     //如果已经存在于OPEN表，更新
     if (OPENTableResult != OPENTable.end())
     {
      delete (*OPENTableResult);
      OPENTable.erase(OPENTableResult);

      //重新建堆，实现排序。注意不能用sort_heap，因为如果容器为空的话会出现bug
      make_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());
     }

     OPENTable.push_back(bestNode);//将最优节点放入OPEN表

     push_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());//重新排序
    }
   }

   CLOSEDTable.push_back(tempNode);
  }

  return false;
 }

 void AStarPathFinding::generatePath()
 {

  Node *nodeChild = endNode;
  Node *nodeParent = endNode->father;
  do
  {
   *(*(m_path + nodeChild->x) + nodeChild->y) = NODE_PATH;//标记为路径上的点
   nodeChild = nodeParent;
   nodeParent = nodeParent->father;
  } while (nodeChild != startNode);

  *(*(m_path + startNode->x) + startNode->y) = NODE_PATH;//标记为路径上的点
 }

 void AStarPathFinding::printPath()
 {
  cout << "The path is " << endl;
  ;i < m_rows;++i)
  {
   ;j < m_cols;++j)
   {
    if (NODE_PATH == *(*(m_path + i) + j))
    {
     cout << "# ";
    }
    else
     cout << *(*(m_maze + i) + j) << " ";
   }
   cout << endl;
  }

  cout << "搜索总步数："  << m_steps << endl;
 }

 bool AStarPathFinding::readMap()
 {
  //从文本文件读取迷宫布局信息
  ifstream mapFileStream(m_mapFileName);
  if (!mapFileStream)
  {
   cerr << "ERROR : unable to open map file" << endl;
   return false;
  }

  mapFileStream >> m_rows >> m_cols;

  //多维数组空间分配
  m_maze = new int *[m_rows];
  ;i < m_rows;++i)
  {
   m_maze[i] = new int[m_cols];
  }

  mapFileStream >> m_startX >> m_startY;
  mapFileStream >> m_endX >> m_endY;

  ;i < m_rows;++i)
  {
   ;j < m_cols;++j)
   {
    mapFileStream >> *(*(m_maze + i) + j);
   }
  }

  return true;
 }

 int AStarPathFinding::judge(int x, int y)
 {
   * (abs(m_endX - x) + abs(m_endY - y)));
 }

 bool AStarPathFinding::isIllegle(int x, int y)
 {
   && x < m_rows &&
   y >=  && y < m_cols &&
   *(*(m_maze + x) + y) == )
   return true;
  else
   return false;
 }

 int main()
 {
   AStarPathFinding astar("map.txt");
   astar.init();
   astar.pathFinding();
   astar.generatePath();
   astar.printPath();
   ;
 }

地图文件：

以上三个文件放在同一个目录下

打开终端：

g++ AStarPathFinding.cpp

./a.out