A*算法

#include<stdio.h>
#include<iostream>
#include<vector>
#include<windows.h>
using namespace std;
#define ROWS 9
#define CLOS 11
#define ZXDJ 10
#define XXDJ 14
//点的类型
struct MyPoint {
  int row;
  int col;
  int f;
  int g;
  int h;
  void setF() {
    f = g + h;
  }
  bool operator==(const MyPoint& p1) {
    if (this->row == p1.row && this->col == p1.col) {
      return true;
    }else
    return false;
  }
};
//树的节点类型
struct TreeNode {
  MyPoint pos;  //当前点坐标
  vector<TreeNode*> childs;//存储当前点的孩子节点指针数组
  TreeNode* pParent;//存储当前点父节点数组
};
enum direct{p_up,p_down,p_left,p_right,p_lup,p_ldown,p_rup,p_rdown};
//检查是否需要统计
bool needAdd(MyPoint pos, int map[ROWS][CLOS], bool pathMap[ROWS][CLOS]) {
  if (pos.row >= ROWS || pos.row < 0|| pos.col >= CLOS || pos.col < 0) {
    return false;
  }
  if (map[pos.row][pos.col]||pathMap[pos.row][pos.col])
    return false;
  return true;
}
void printMap(int map[ROWS][CLOS],TreeNode* pCurrent,MyPoint endPos) {
  for (int i = 0; i < ROWS; i++) {
    for (int j = 0; j < CLOS; j++) {
      if (i == pCurrent->pos.row && j == pCurrent->pos.col) {
        cout << "人";
      }
      else if (i == endPos.row && j == endPos.col) {
        cout << "终";
      }
      else if (map[i][j] == 0) {
        cout << "口";
      }
      else if(map[i][j]==1) {
        cout << "墙";
      }
    }
    cout << endl;
  }
 }
//计算h值：endPos:终点坐标  pos:当前点坐标
int getH(MyPoint endPos, MyPoint pos) {
  int x = abs(endPos.col-pos.col);
  int y = abs(endPos.row - pos.row);
  return (x + y) * ZXDJ;
}
int main() {
  int map[ROWS][CLOS] = {
    {0,0,0,0,0,1,0,0,0,0,0},
    {0,0,0,1,0,1,0,1,1,1,0},
    {0,0,0,1,0,1,0,0,0,0,0},
    {0,0,0,1,0,1,0,0,0,0,0},
    {0,0,0,0,0,1,0,0,0,0,0},
    {0,1,1,0,0,1,0,0,0,0,0},
    {0,0,1,1,0,1,0,0,0,0,0},
    {0,0,0,0,1,1,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,0},
  };
  //2 辅助地图 false 0：没有走过  true 1：走过
  bool pathMap[ROWS][CLOS] = { 0 };
  //3.起点 终点
  MyPoint begPos = { 8,8 };
  MyPoint endPos = { 0,8 };
  //4.标记起点走过
  pathMap[begPos.row][begPos.col] = true;
  //5.创建一棵树，起点是树的根节点
  //创建一个新节点
  TreeNode* pNew = new TreeNode;//创建一个节点
  //memset(pNew, 0, sizeof TreeNode);//给节点赋值
  //pNew={0};
  //新节点成为树根
  //pRoot一直指向树的根节点
  TreeNode* pRoot = pNew;
  //新节点是记录起点
  pRoot->pos = begPos;
  pRoot->pParent = NULL;
  //6.动态数组 存储用来比较的节点
  vector<TreeNode*> buff;
  TreeNode* pCurrent = pRoot;
  TreeNode* pChild = NULL;//中间节点
  vector<TreeNode*>::iterator it;
  vector<TreeNode*>::iterator itMin;
  //7.寻路
  while (1) {
    printMap(map, pCurrent, endPos);
    Sleep(300);
    system("cls");
    //7.1找到当前点周围能走的点
    for (int i = 0; i < 8; i++) {
      pChild = new TreeNode;
      //memset(pChild, 0, sizeof TreeNode);
      //pChild = { 0 };
      pChild->pos = pCurrent->pos;
      switch(i) {
      case p_up:
        pChild->pos.row--;
        pChild->pos.g += ZXDJ;
        break;
      case p_down:
        pChild->pos.row++;
        pChild->pos.g += ZXDJ;
        break;
      case p_left:
        pChild->pos.col--;
        pChild->pos.g += ZXDJ;
        break;
      case p_right:
        pChild->pos.col++;
        pChild->pos.g += ZXDJ;
        break;
      case p_lup:
        pChild->pos.row--;
        pChild->pos.col--;
        pChild->pos.g += XXDJ;
        break;
      case p_rup:
        pChild->pos.row--;
        pChild->pos.col++;
        pChild->pos.g += XXDJ;
        break;
      case p_ldown:
        pChild->pos.row++;
        pChild->pos.col--;
        pChild->pos.g += XXDJ;
        break;
      case p_rdown:
        pChild->pos.row++;
        pChild->pos.col++;
        pChild->pos.g += XXDJ;
        break;
      default:
        break;
      }
      /*printf("(%d,%d):%d\n", pChild->pos.row, pChild->pos.col, pChild->pos.g);*/
      //7.2计算 g h f值
      pChild->pos.h = getH(endPos, pChild->pos);
      pChild->pos.setF();
      //7.3入树，入buff数组
      if (needAdd(pChild->pos, map, pathMap)) {
        //入树
        pCurrent->childs.push_back(pChild);
        pChild->pParent = pCurrent;
        /*printf("(%d,%d):%d\n", pChild->pos.row, pChild->pos.col, pChild->pos.g);*/
        //入buff数组
        buff.push_back(pChild);
        //标记走过
        pathMap[pChild->pos.row][pChild->pos.col] = true;
      }
      else {
        delete pChild;
      }
    }
    //7.4从buff数组中找到f值最小的那个  回溯也是靠这个buff回溯的
    //TreeNode* tmin=*(buff.begin());//迭代器解引用，迭代器相当于一个指针
    itMin = buff.begin();
    int differ = abs((*buff.begin())->pos.g - (*buff.begin())->pos.h);
    for (it = buff.begin();it!=buff.end();it++) {
      itMin = (*itMin)->pos.f > (*it)->pos.f ? it : itMin;
      if ((*itMin)->pos.f > (*it)->pos.f) {
        itMin = it;
      }
      else if ((*itMin)->pos.f > (*it)->pos.f) {
        itMin = itMin;
      }
      else if ((*itMin)->pos.f == (*it)->pos.f) {
        if (abs((*it)->pos.g - (*it)->pos.h) < differ) {
          differ = abs((*it)->pos.g - (*it)->pos.h);
          itMin = it;
        }
      }
    }
    //7.5删掉，变化成当前点
    pCurrent = *itMin;
    buff.erase(itMin);
    //7.6 判断是否寻路结束
    if (pCurrent->pos == endPos) {
      cout << "我到终点了！"<<endl;
      while (pCurrent) {
        printf("(%d,%d)", pCurrent->pos.row, pCurrent->pos.col);
        pCurrent = pCurrent->pParent;
      }
      cout << endl;
      break;
    }
    if (buff.empty()) {
      cout << "无路径！" << endl;
      break;
    }
  }
  //end while
  //MyPoint p1;
  //p1.row = 1;
  //p1.col = 1;
  //MyPoint p2;
  //p2.row = 0;
  //p2.col = 1;
  //cout << (p1 == p2) << endl;
  return 0;
}