A星寻路算法的实现，单击左键编辑障碍物，单击右键清除障碍物，双击右键开始自动寻路。
AStar.h:
#pragma once
#include <windows.h>
#include <vector>
#define F_H_WHITE 0x0004 | 0x0002 | 0x0001 | 0x0008
#define B_H_WHITE 0x0010|0x0020|0x0040|0x0080
#define F_H_YELLOW 0x0002|0x0004|0x0008
#define F_H_GREEN 0x0002|0x0008
#define F_RED 0x0004
using std::vector;
class CAStar
{
public:
CAStar();
~CAStar();
public:
//表的节点结构体
typedef struct _NODE
{
COORD stcPos; //当前点的坐标
COORD stcPrtPos; //上一个点的坐标
int F; //移动损耗
int G; //估算距离
int H; //G+H
}NODE,*PNOED;
public:
//寻路函数,获取最短路径
bool GetPath(_Out_ vector<COORD>& vecPath);
//设置控制台信息
HANDLE hStdOut = GetStdHandle(STD_OUTPUT_HANDLE);
void SetConsoleInfo();
//描点画图函数
void WriteChar(int x, int y, wchar_t* pszChar, WORD wArr);
//画地图
void DrawMap();
//编辑地图
void EditMap();
private:
COORD m_Begin; //起始点
COORD m_End; //终止点
int m_Map[40][40]; //地图
vector<NODE> m_vecOpen; //Open表
vector<NODE> m_vecClose;//Close表
};
AStar.cpp:
#include "stdafx.h"
#include "AStar.h"
CAStar::CAStar()
{
m_Begin.X = 10;
m_Begin.Y = 10;
m_End.X = 30;
m_End.Y = 30;
}
CAStar::~CAStar()
{
}
//************************************************************
// 函数名称: GetPath
// 函数说明: 寻路函数,获取最短路径
// 作 者: Dylan
// 时 间: 2015/08/28
// 参 数: _Out_ vector<COORD> & vecPath
// 返 回 值: bool
//************************************************************
bool CAStar::GetPath(_Out_ vector<COORD>& vecPath)
{
//0.起点就是终点，直接返回
if (m_Begin.X == m_End.X&&m_Begin.Y == m_End.Y)
{
return true;
}
//1.将起始点加入到Open表中
NODE FirstNode = { 0 };
FirstNode.stcPos = m_Begin;
FirstNode.G = 0;
FirstNode.H =
abs(FirstNode.stcPos.X - m_End.X) +
abs(FirstNode.stcPos.Y - m_End.Y);
FirstNode.F = FirstNode.G + FirstNode.H;
m_vecOpen.push_back(FirstNode); //Open表中有了第一个点
while (true)
{
//2.开始循环，从Open表中，选取F值最小的点
int nMin = m_vecOpen[0].F;
int nPos = 0;
int nSize = m_vecOpen.size();
for (int i = 0; i < nSize; i++)
{
if (nMin>m_vecOpen[i].F)
{
nMin = m_vecOpen[i].F;
nPos = i;
}
}
//3.从此点派生出周围的四个点,并为其赋值
NODE stcTemp[4] = { 0 };
//上
stcTemp[0].stcPos.X = m_vecOpen[nPos].stcPos.X;
stcTemp[0].stcPos.Y = m_vecOpen[nPos].stcPos.Y - 1;
//下
stcTemp[1].stcPos.X = m_vecOpen[nPos].stcPos.X;
stcTemp[1].stcPos.Y = m_vecOpen[nPos].stcPos.Y + 1;
//左
stcTemp[2].stcPos.X = m_vecOpen[nPos].stcPos.X - 1;
stcTemp[2].stcPos.Y = m_vecOpen[nPos].stcPos.Y ;
//右
stcTemp[3].stcPos.X = m_vecOpen[nPos].stcPos.X + 1;
stcTemp[3].stcPos.Y = m_vecOpen[nPos].stcPos.Y;
for (int i = 0; i < 4;i++)
{
stcTemp[i].stcPrtPos = m_vecOpen[nPos].stcPos;
stcTemp[i].G = m_vecOpen[nPos].G + 1;
stcTemp[i].H =
abs(stcTemp[i].stcPos.X - m_End.X) +
abs(stcTemp[i].stcPos.Y - m_End.Y);
stcTemp[i].F = stcTemp[i].G + stcTemp[i].H;
}
//3.1将此最小点，将其加入到Close表中,从Open表中删除
m_vecClose.push_back(m_vecOpen[nPos]);
m_vecOpen.erase(m_vecOpen.begin() + nPos);
//4.检测一下这四个点是否可用
for (int i = 0; i < 4; i++)
{
int nX = stcTemp[i].stcPos.X;
int nY = stcTemp[i].stcPos.Y;
//4.0找到了终点
if (nX == m_End.X&&nY == m_End.Y)
{
//4.0.1已经找到终点，从Close表中进行回溯，得到最短的路径，退出
//回溯
//-------------------------------------------------
vecPath.push_back(stcTemp[i].stcPos);
NODE PathNode = stcTemp[i];
for (int i = m_vecClose.size()-1; i >= 0; i--)
{
if (PathNode.stcPrtPos.X == m_vecClose[i].stcPos.X&&
PathNode.stcPrtPos.Y == m_vecClose[i].stcPos.Y
)
{
vecPath.push_back(m_vecClose[i].stcPos);
PathNode = m_vecClose[i];
}
}
//-------------------------------------------------
m_vecOpen.clear();
m_vecClose.clear();
return true;
}
//4.1是否越界
if (nX<0||nY<0||nX>39||nX>39)
{
continue;
}
//4.2是否在障碍物上
if (m_Map[nY][nX]!=0)
{
continue;
}
//4.3是否在Open表中
int j = 0;
for (; j < m_vecOpen.size();j++)
{
if (m_vecOpen[j].stcPos.X == nX&&m_vecOpen[j].stcPos.Y == nY)
{
break;
}
}
if (j != m_vecOpen.size())
{
continue;
}
//4.4是否在Close表中
for (j = 0; j < m_vecClose.size();j++)
{
if (m_vecClose[j].stcPos.X == nX&&m_vecClose[j].stcPos.Y == nY)
{
break;
}
}
if (j != m_vecClose.size())
{
continue;
}
//4.5将可用的点加入到Open表中
m_vecOpen.push_back(stcTemp[i]);
}
//5.Open表中没有点了，说明没有找到路径，退出函数
if (m_vecOpen.size()==0)
{
m_vecClose.clear();
return false;
}
}
}
//************************************************************
// 函数名称: SetConsoleInfo
// 函数说明: 设置控制台信息
// 作 者: Dylan
// 时 间: 2015/08/28
// 返 回 值: void
//************************************************************
void CAStar::SetConsoleInfo()
{
//设置控制台标题
SetConsoleTitle(L"A星寻路算法");
//设置控制台缓冲区大小
COORD BufferSize = { 99, 42 };
SetConsoleScreenBufferSize(hStdOut, BufferSize);
//设置控制台窗口大小
SMALL_RECT strctWindow = { 0, 0, BufferSize.X - 1, BufferSize.Y - 1 };
SetConsoleWindowInfo(hStdOut, true, &strctWindow);
//设置光标位置
COORD pos = { 0, 40 };
SetConsoleCursorPosition(hStdOut, pos);
}
//************************************************************
// 函数名称: WriteChar
// 函数说明: 描点画图函数
// 作 者: Dylan
// 时 间: 2015/08/28
// 参 数: int x
// 参 数: int y
// 参 数: wchar_t * pszChar
// 参 数: WORD wArr
// 返 回 值: void
//************************************************************
void CAStar::WriteChar(int x, int y, wchar_t* pszChar, WORD wArr)
{
CONSOLE_CURSOR_INFO cci;
cci.dwSize = 1;
cci.bVisible = FALSE;//光标是否可见
SetConsoleCursorInfo(hStdOut, &cci);//将光标属性应用到控制台
COORD loc = { x * 2, y };
DWORD dwRes1, dwRes2;
WORD wClr[10];
wmemset((wchar_t*)wClr, wArr, 10);
WriteConsoleOutputAttribute(hStdOut, wClr, 2, loc, &dwRes1);//打印字符串颜色
WriteConsoleOutputCharacter(hStdOut, pszChar, 1, loc, &dwRes2); //打印字符串到指定位置
}
//************************************************************
// 函数名称: DrawMap
// 函数说明: 画地图
// 作 者: Dylan
// 时 间: 2015/08/28
// 返 回 值: void
//************************************************************
void CAStar::DrawMap()
{
SetConsoleInfo();
WriteChar(m_Begin.X, m_Begin.Y, L"☆", F_H_YELLOW);
WriteChar(m_End.X, m_End.Y, L"★", F_H_YELLOW);
for (int i = 0; i < 40; i++)
{
for (int j = 0; j < 40; j++)
{
m_Map[i][j] = 0;
if (i == 0 || j == 0 || i == 39 || j == 39)
{
m_Map[i][j] = 1;
}
}
}
for (int i = 0; i < 40; i++)
{
for (int j = 0; j < 40; j++)
{
if (m_Map[j][i] == 1)
{
WriteChar(i, j, L"▇", B_H_WHITE | F_RED);
}
}
}
}
//************************************************************
// 函数名称: EditMap
// 函数说明: 编辑地图
// 作 者: Dylan
// 时 间: 2015/08/28
// 返 回 值: void
//************************************************************
void CAStar::EditMap()
{
//画张空地图
DrawMap();
//获取标地图准输入输出句柄
HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
HANDLE hIn = GetStdHandle(STD_INPUT_HANDLE);
CONSOLE_SCREEN_BUFFER_INFO bInfo;
INPUT_RECORD mouseRec;
DWORD res;
COORD crPos = { 0, 0 }, crHome = { 82, 2 };
SetConsoleMode(hIn, ENABLE_MOUSE_INPUT);//设置鼠标可用，这样cls以后鼠标就不会不好使
while (true)
{
ReadConsoleInput(hIn, &mouseRec, 1, &res);
if (mouseRec.EventType == MOUSE_EVENT)
{
if (mouseRec.Event.MouseEvent.dwButtonState == RIGHTMOST_BUTTON_PRESSED)
{
if (mouseRec.Event.MouseEvent.dwEventFlags == DOUBLE_CLICK)
{
//双击右键退出循环，画路径
break;
}
}
//设置坐标显示信息
crPos = mouseRec.Event.MouseEvent.dwMousePosition;
GetConsoleScreenBufferInfo(hOut, &bInfo);
SetConsoleTextAttribute(hOut, F_H_WHITE);
SetConsoleCursorPosition(hOut, crHome);
printf("坐标X: %2lu Y: %2lu", crPos.X, crPos.Y);
SetConsoleCursorPosition(hOut, bInfo.dwCursorPosition);
//鼠标点击事件
switch (mouseRec.Event.MouseEvent.dwButtonState)
{
//单击左键
case FROM_LEFT_1ST_BUTTON_PRESSED:
if (crPos.X > 1 && crPos.X<77 && crPos.Y>0 && crPos.Y < 39)
{
crPos.X = crPos.X / 2 * 2;
if (!((crPos.X / 2 == m_Begin.X&&crPos.Y == m_Begin.Y) ||
(crPos.X / 2 == m_End.X&&crPos.Y == m_End.Y)))
{
WriteChar(crPos.X / 2, crPos.Y, L"▇", B_H_WHITE | F_RED);
m_Map[crPos.Y][crPos.X / 2] = 1;
}
}
break;
//单击右键
case RIGHTMOST_BUTTON_PRESSED:
if (crPos.X > 1 && crPos.X<77 && crPos.Y>0 && crPos.Y < 39)
{
crPos.X = crPos.X / 2 * 2;
if (!((crPos.X / 2 == m_Begin.X&&crPos.Y == m_Begin.Y) ||
(crPos.X / 2 == m_End.X&&crPos.Y == m_End.Y)))
{
WriteChar(crPos.X / 2, crPos.Y, L" ",NULL);
m_Map[crPos.Y][crPos.X / 2] = 0;
}
}
break;
}
}
}
}
A_Star.cpp:
// A_Star.cpp : 定义控制台应用程序的入口点。
// A星算法
#include "stdafx.h"
#include "AStar.h"
int _tmain(int argc, _TCHAR* argv[])
{
vector<COORD> Path;
CAStar obj;
obj.EditMap();
obj.GetPath(Path);
//画路线
for (int i = Path.size() - 2;i>0;i--)
{
obj.WriteChar(Path[i].X, Path[i].Y, L"☉", F_H_GREEN);
}
system("pause");
return 0;
}
演示效果:
![[学习笔记]A星寻路算法实例](https://image.shishitao.com:8440/aHR0cHM6Ly93d3cuaXRkYWFuLmNvbS9pbWdzLzcvMi82LzIvODAvMGNkNjgyNDI3N2JiMmE5Y2ExY2NiNWVkOWE4NGJkN2QuanBl.jpe?w=700&webp=1 "[学习笔记]A星寻路算法实例")