[学习笔记]A星寻路算法实例

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;}

演示效果: