A*算法

简介: A*算法

公式:F=G+H+W

F是代价函数,G代表从起点到当前点的距离,H代表从当前点到终点的距离,W是权值

H不考虑障碍物,只计算直线距离(计算横纵距离差)

image.png

算法思想:从起始点出发计算周围8个点的F,选择最小的点进行拓展,如果F相等选择|G-H|小的结点,循环操作。若走进死胡同(周围所有点都被访问或者是障碍物)回溯。

#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;
}
相关文章
|
1月前
|
自然语言处理 算法 BI
Baum-Welch算法
Baum-Welch算法是一种用于隐马尔可夫模型(HMM)的训练算法,通过期望最大化(EM)框架迭代估计模型参数,直至收敛。该算法主要应用于语音识别、生物信息学和自然语言处理等领域,通过优化初始状态概率、状态转移概率和观测概率,提高模型对观测数据的拟合度。尽管存在局部最优和计算复杂性等挑战,但仍是HMM参数估计的重要工具。
|
3月前
|
算法
算法题(8)
算法题(8)
18 4
|
3月前
|
算法
算法题(9)
算法题(9)
25 4
|
5月前
|
存储 传感器 编解码
|
7月前
|
算法
多边形裁剪算法
多边形裁剪算法
|
算法 索引
插值查找算法
插值查找算法
83 0
|
算法 数据安全/隐私保护
《算法》世界 二
一.算法要素 1.数据对象的运算和操作:计算机可以执行的基本操作是以指令的形式描述的。一个计算机系统能执行的所有指令的集合,成为该计算机系统的指令系统。一个计算机的基本运算和操作有如下四类: 1.算术运算:加减乘除等运算 2.逻辑运算:或、且、非等运算 3.关系运算:大于、小于、等于、不等于等运算 4.数据传输:输入、输出、赋值等运算 2.算法的控制结构:一个算法的功能结构不仅取决于所选用的操作,而且还与各操作之间的执行顺序有关。
177 1
《算法》世界 二
推公式算法的实现
推公式算法的实现
推公式算法的实现
|
机器学习/深度学习 算法 搜索推荐
C#算法大全(中)
今天有人想让我搞一期C#算法大全。算法就算法,安排上!
|
算法
【算法之初步认识】
【算法之初步认识】
151 0
【算法之初步认识】