A*寻路算法的实现

原理：http://www.cppblog.com/christanxw/archive/2006/04/07/5126.html

算法理论请到原理这个传送门，代码中的注释，已经比较详细，所以我不会讲太多的原理，该文章本身就是以A*的思路，对算法进行一次速度上的优化，用一些更效率的方式来代替算法原理中必要的步骤。

针对算法原理，做出如下改动：

抛弃关闭列表，取而代之的是根据地图数据生成一个BYTE类型的二维数组，因为该数组在算法中可能需要修改，所以不能直接使用原始数据。

注：二维数组动态分配应为：

BYTE **pMap = new BYTE*[地图高度];

for(int i = 0; i < 地图高度; i++)

pMap[i] = new BYTE[地图宽度];

释放：

void FreeMap(BYTE **pMap, DWORD dwHeight)
{
for(DWORD i = 0; i < dwHeight; i++)
{
delete pMap[i];
pMap[i] = NULL;
}
delete [] pMap;
}

抛弃曼哈顿算法，使用求绝对距离的方法直接计算F，使用相邻格坐标，其中（x，y）任意相等则为横纵移动判断G

在遍历开启列表查找最小距离的节点时，加入跳出逻辑。

修改后的缺点，占内存，10000*10000要算上列表要100+M，所以如果你的游戏有这么大的地图，应该分段查找，分成1000*1000一次比较理想，或者对地图数据进行压缩。

测试结果：

[3300] 出口坐标：354, B8   当前坐标：CE, 1FD
[3300] 最终寻路到：353, CC
[3300] 耗时：0 毫秒
[3300] 寻路成功，节点数：1187

根据图片看到的情况，很显然，这不是最佳路径，要选择最佳路径，我自己能想到的办法就是还是先得到这个路径，然后在这个路径中查找角度改变的地方，满足一个三角之后，或者根据距离分段，根据根据两端的坐标再进行一次A*，这样数次之后可能得到的路径也不错了。

对于优化路线最简单的办法，请到这个传送门：对A*算法的路径进行优化

代码如下：

必须依赖的VC++头文件(vc6.0)有：

#include <list>

#include <algorithm>

#include <stdarg.h>

#include <math.h>

大概就是这些，不够再百度下吧

头文件

 

namespace blx
{
#ifndef BREAK_GAP
#define BREAK_GAP 20.0
#endif
#ifndef NOTLESS_COUNT
#define NOTLESS_COUNT 14
#endif
	typedef struct _APOINT{
		int x;
		int y;
		double dbGap;
		_APOINT *parent;
	}APOINT, *LPAPOINT;

	class CAStar
	{
	public:
		CAStar();
		CAStar(BYTE **pMap, DWORD dwMapWidth, DWORD dwMapHeight);
		bool Search(int X, int Y, std::list<POINT> &lResult);//主搜索
		void SetDestinationPos(int X, int Y)//设置目标坐标
		{m_dwDestinationX = X; m_dwDestinationY = Y;}
		void SetMapAttributes(BYTE **pMap, DWORD dwWidth, DWORD dwHeight)//设置地图属性 参数为：指针，宽，高  与第二个构造函数相同
		{m_pMap = pMap; m_dwMapWidth = dwWidth; m_dwMapHeight = dwHeight;};
		void printBitmap(BYTE **pMap, int nWidth, int nHeight, std::list<POINT> &lPath, LPCTSTR lpFile);//该接口可将寻路结果保存为bmp图像
		//参数为 地图指针，宽度，高度，寻路路径，文件
		//不要使用m_pMap，应该在搜索完毕后新建一个地图，然后再来调用该接口生成图像
		
	private:
		LPAPOINT GenerateSuccessors(std::list<LPAPOINT>::iterator it);//处理每一个节点
		std::list<LPAPOINT>::iterator GetMingapNode();//获取开启列表中最小距离的节点
		
	private:
		BYTE **m_pMap;
		DWORD m_dwDestinationX, m_dwDestinationY, m_dwMapWidth, m_dwMapHeight;
		
		std::list<LPAPOINT> m_lOpen;
		std::list<LPAPOINT> m_lSafe;
	};
}

源文件

 

 

namespace blx
{
	CAStar::CAStar()
	{
		m_pMap = NULL;
		m_dwDestinationX = 0;
		m_dwDestinationY = 0;
		m_dwMapWidth = 0xFFFFFFFF;
		m_dwMapHeight = 0xFFFFFFFF;
	}

	CAStar::CAStar(BYTE **pMap, DWORD dwMapWidth, DWORD dwMapHeight)
	{
		m_pMap = pMap;
		m_dwMapWidth = dwMapWidth;
		m_dwMapHeight = dwMapHeight;
	}

	std::list<LPAPOINT>::iterator CAStar::GetMingapNode()
	{
		//获取开启列表中，距离目的地最近的节点
		std::list<LPAPOINT>::iterator itResult;
		double dbMinGap = 100000000.0;
		int nIntoMaxCount = 0;
		bool bIntoState = true;
		for(std::list<LPAPOINT>::iterator it = m_lOpen.begin(); it != m_lOpen.end(); ++it)
		{
			if((*it)->dbGap < dbMinGap)
			{
				//这种类似冒泡排序的逻辑不用我多说吧
				dbMinGap = (*it)->dbGap;
				itResult = it;

				//如果选中一个最小的，把检查循环状态设置为关闭
				bIntoState = false;
			}
			else
			{

				if(bIntoState)//如果检查循环状态为开启
					nIntoMaxCount++;//则计数+1
				else
				{
					//如果检查循环状态为关闭
					nIntoMaxCount = 1;//置计数为1
					bIntoState = true;//把检查循环状态打开
				}
			}

			//当检查循环计数大于 NOTLESS_COUNT 时 跳出
			if(nIntoMaxCount > NOTLESS_COUNT) // #define NOTLESS_INDEX 14
			{
				//除了起始点之外，每次处理之后加入到开启列表的节点不会大于5（它本身已经关闭，它的父节点之前就关闭了）
				//你可以在纸上画出来，走一步之后，当前格子相邻的8个格子中，有一个是父节点，有两个与他的父节点相邻
				//我用了14来判断跳出，当然这个14是列表中的节点一直大于或等于已经搜索到的最小节点之后的第14次循环之后
				//这样就可以把查找开启列表的成本降低到最小，但是路径很可能就不是最好的，因为不知道经过这14次之后，还会不会有更好的节点
				//要满足这个前提，你要反向的遍历开启列表，本例中的开启列表是向前插入的(push_front)，所以我是从链表的正向遍历
				break;
			}
		}
		
		return itResult;
	}

	struct _find_map_note{
		_find_map_note(int x, int y) : _x(x), _y(y) {}
		bool operator()(LPAPOINT p){return (p->x == _x && p->y == _y);}
		int _x, _y;
	};

	LPAPOINT CAStar::GenerateSuccessors(std::list<LPAPOINT>::iterator it)
	{
		int x = (*it)->x, y = (*it)->y;
		int aX[3] = {x - 1, x, x + 1};
		int aY[3] = {y - 1, y, y + 1};
		LPAPOINT pNow = *it;
		m_lOpen.erase(it);//从开启列表中移除
		m_pMap[y][x] = 0;//设置为关闭
		//
		BYTE bState = 0;
		LPAPOINT p;
		std::list<LPAPOINT>::iterator it2;
		for(int i = 0; i < 3; i++)
		{
			for(int j = 0; j < 3; j++)
			{
				//交叉遍历
				if(aX[j] >= m_dwMapWidth || aY[i] >= m_dwMapHeight)
					continue;
				
				bState = m_pMap[aY[i]][aX[j]];
				
				if(!bState)
					continue;//如果这个坐标是障碍或者已经置为关闭，则忽略它
				else if(bState == 1)
				{
					//如果它不在开启列表，将它加入到开启列表中，并设置它的父节点为当前节点
					p = new APOINT;
					p->x = aX[j];
					p->y = aY[i];
					p->dbGap = _p2g(aX[j], aY[i], m_dwDestinationX, m_dwDestinationY);
					p->parent = pNow;
					m_lOpen.push_front(p);//反向加入到容器头部
					m_lSafe.push_back(p);//加入到公共容器
					m_pMap[aY[i]][aX[j]] = 2;
				}
				else if(bState == 2)
				{
					//如果它已经在开启列表中
					if(x == aX[j] || y == aY[i])//判断它与当前节点的关系是否为横纵移动
					{
						//如果是
						it2 = std::find_if(m_lOpen.begin(), m_lOpen.end(), _find_map_note(aX[j], aY[i]));//从开启列表中把它的指针拿出来
						//因为开启列表是反向的，所以正向遍历能尽可能的减少开销，如果还要比这个更效率的，可能二叉堆会快一点
						if((*it2)->parent->x != aX[j] && (*it2)->parent->y != aY[i])//判断它与它之前父节点的关系，是否是横纵移动
							(*it2)->parent = pNow;//如果不是，则设置它的父节点为当前节点
					}

					//更形象的说，这是一个很成功的阴谋：
					//当你与他的关系好到可以让他认贼作父时
					//你调查一下他和他爹的关系如何
					//如果他跟他爹的关系不好
					//那你就对他说：“和你爹断绝关系吧，从此以后你就是我儿子！”
					//如果他跟他爹的关系跟你一样好，那你就最好打消上面一句话这种不实际的念头。

					//如果你不管你们之间的关系，就想要考虑让他认贼作父，那你就有可能付出了调查他的代价，最后却什么都得不到
					//最后只能怨天尤人的说：“草，这票白干了！”
				}
			}
		}
		return NULL;
	}

	bool CAStar::Search(int X, int Y, std::list<POINT> &lResult)
	{
		if(X < 0 || Y < 0
			|| X > m_dwMapWidth || Y > m_dwMapWidth ||
			m_dwDestinationX < 0 || m_dwDestinationX < 0 ||
			m_dwDestinationX > m_dwMapWidth || m_dwDestinationY > m_dwMapHeight)
		{
			_outf("坐标或地图参数错误！");
			return false;
		}

		LPAPOINT p = new APOINT;
		p->x = X;
		p->y = Y;
		p->parent = NULL;
		p->dbGap = _p2g(X, Y, m_dwDestinationX, m_dwDestinationY);
		m_lOpen.push_front(p);//起始节点加入到开启列表
		m_lSafe.push_back(p);//加入到公共容器，任何新分配的节点，都要加入到这里，便于算法执行完后清理

		std::list<LPAPOINT>::iterator it;
		DWORD dwTime = clock();
		while(!m_lOpen.empty())
		{
			//这里就是反复遍历开启列表选择距离最小的节点
			it = GetMingapNode();
			if((*it)->dbGap < BREAK_GAP)//#define BREAK_GAP 20.0在头文件中，我对寻路的要求不是很高，所以用了与目的地小于20则跳出
				break;
			p = *it;
			GenerateSuccessors(it);
		}

		if(!m_lOpen.empty())
		{
			//如果列表不为空，从最后一个节点开始拷贝路径到返回值中
			_outf("最终寻路到：%X, %X", p->x, p->y);
			POINT point;
			while(p)
			{
				point.x = p->x;
				point.y = p->y;
				lResult.push_front(point);
				p = p->parent;
			}
		}

		for(it = m_lSafe.begin(); it != m_lSafe.end(); ++it)
		{
			//清理内存
			if(*it != NULL)
			{
				delete (*it);
				*it = NULL;
			}
		}

		m_lSafe.clear();//清空容器

		_outf("耗时：%d 毫秒", clock() - dwTime);

		if(m_lOpen.empty())
		{
			_outf("寻路失败");
			return false;
		}

		m_lOpen.clear();//清空开启列表
		_outf("寻路成功，节点数：%d", lResult.size());
		return true;
	}

	void CAStar::printBitmap(BYTE **pMap, int nWidth, int nHeight, std::list<POINT> &lPath, LPCTSTR lpFile)
	{
		int _nWidth = (nWidth + 3) * 4 / 4;
		HDC hdc = CreateCompatibleDC(NULL);
		PVOID pBits = NULL;
		BITMAPINFO bi;
		bi.bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
		bi.bmiHeader.biWidth = nWidth;
		bi.bmiHeader.biHeight = nHeight;
		bi.bmiHeader.biPlanes = 1;
		bi.bmiHeader.biBitCount = 24;
		bi.bmiHeader.biCompression = BI_RGB;
		bi.bmiHeader.biSizeImage = nWidth * nHeight * 3;
		HBITMAP hBitmap = CreateDIBSection(NULL, &bi, DIB_RGB_COLORS, &pBits, NULL, NULL);
		HBITMAP hOld = (HBITMAP)SelectObject(hdc, hBitmap);
		for(int i = 0; i < nHeight - 1; i++)
		{
			for(int j = 0; j < nWidth - 1; j++)
			{
				if(pMap[i][j] == 1)
				{
					SetPixel(hdc, j, i, RGB(0xFF,0xFF,0xFF));
				}
				else
				{
					SetPixel(hdc, j, i, RGB(0x80,0x80,0x80));
				}
			}
		}

		for(std::list<POINT>::iterator it = lPath.begin(); it != lPath.end(); ++it)
			SetPixel(hdc, it->x, it->y, RGB(0xFF,0,0));
		SaveHBITMAP2File(NULL, lpFile, hBitmap, hdc);
		SelectObject(hdc, hOld);
		DeleteObject(hBitmap);
		DeleteDC(hdc);
	}
}

 

必须的函数

 

//调试输出
void _outf(const char *format, ...)
{
	va_list al;
	char buf[BLX_MAXSIZE];
	va_start(al, format);
	_vsnprintf(buf, BLX_MAXSIZE, format, al);
	va_end(al);
	OutputDebugStringA(buf);
}

//距离比价函数，计算两个坐标的绝对距离
double _p2g(int x1, int y1, int x2, int y2)
{
	return sqrt(pow(double(abs(x1 - x2)), 2) + pow(double(abs(y1 - y2)), 2));
}


//这个是度娘上抄来的，反正可用，就没管，就是用来保存为bmp用的
PBITMAPINFO CreateBitmapInfoStruct(HWND hwnd, HBITMAP hBmp)   
{   
	BITMAP bmp;    
	PBITMAPINFO pbmi;    
	WORD    cClrBits;    
	if (!GetObject(hBmp, sizeof(BITMAP), (LPSTR)&bmp))    
		return NULL;  
	cClrBits = (WORD)(bmp.bmPlanes * bmp.bmBitsPixel);    
	if (cClrBits == 1)    
		cClrBits = 1;    
	else if (cClrBits <= 4)    
		cClrBits = 4;    
	else if (cClrBits <= 8)    
		cClrBits = 8;    
	else if (cClrBits <= 16)    
		cClrBits = 16;    
	else if (cClrBits <= 24)    
		cClrBits = 24;    
	else cClrBits = 32;    
	if (cClrBits != 24)    
		pbmi = (PBITMAPINFO) LocalAlloc(LPTR,    
		sizeof(BITMAPINFOHEADER) +    
		sizeof(RGBQUAD) * (1<< cClrBits));    
	else    
		pbmi = (PBITMAPINFO) LocalAlloc(LPTR,    
		sizeof(BITMAPINFOHEADER));
	pbmi->bmiHeader.biSize = sizeof(BITMAPINFOHEADER);    
	pbmi->bmiHeader.biWidth = bmp.bmWidth;    
	pbmi->bmiHeader.biHeight = bmp.bmHeight;    
	pbmi->bmiHeader.biPlanes = bmp.bmPlanes;    
	pbmi->bmiHeader.biBitCount = bmp.bmBitsPixel;    
	if (cClrBits < 24)    
		pbmi->bmiHeader.biClrUsed = (1<<cClrBits);     
	pbmi->bmiHeader.biCompression = BI_RGB;    
	pbmi->bmiHeader.biSizeImage = ((pbmi->bmiHeader.biWidth * cClrBits +31) & ~31) /8   
		* pbmi->bmiHeader.biHeight;    
	pbmi->bmiHeader.biClrImportant = 0;    
	return pbmi;    
} 

//这个是度娘上抄来的，反正可用，就没管，就是用来保存为bmp用的
BOOL SaveHBITMAP2File(HWND hwnd, LPCTSTR pszFile, HBITMAP hBMP, HDC hDC)   
{   
	PBITMAPINFO pbi = CreateBitmapInfoStruct(hwnd, hBMP);   
	
	HANDLE hf;                 // file handle    
	BITMAPFILEHEADER hdr;       // bitmap file-header    
	PBITMAPINFOHEADER pbih;     // bitmap info-header    
	LPBYTE lpBits;              // memory pointer    
	DWORD dwTotal;              // total count of bytes    
	DWORD cb;                   // incremental count of bytes    
	BYTE *hp;                   // byte pointer    
	DWORD dwTmp;    
	DWORD fileSizeInfo=0;
	
	pbih = (PBITMAPINFOHEADER) pbi;    
	lpBits = (LPBYTE) GlobalAlloc(GMEM_FIXED, pbih->biSizeImage);   
	
	if (!lpBits)    
		return FALSE;
	
	if (!GetDIBits(hDC, hBMP, 0, (WORD) pbih->biHeight, lpBits, pbi,    
		DIB_RGB_COLORS))    
	{   
		return FALSE;    
	}   
	
	
	fileSizeInfo = (DWORD) (sizeof(BITMAPFILEHEADER) +    
		pbih->biSize + pbih->biClrUsed    
		* sizeof(RGBQUAD) + pbih->biSizeImage); 
	if(fileSizeInfo==58)
		return FALSE;
	hf = CreateFile(pszFile,    
		GENERIC_READ | GENERIC_WRITE,    
		(DWORD) 0,    
		NULL,    
		OPEN_ALWAYS,    
		FILE_ATTRIBUTE_NORMAL,    
		(HANDLE) NULL);    
	if (hf == INVALID_HANDLE_VALUE)    
		return FALSE;    
	
	hdr.bfType = 0x4d42; 
	hdr.bfSize = fileSizeInfo;  
	
	hdr.bfReserved1 = 0;    
	hdr.bfReserved2 = 0;      
	hdr.bfOffBits = (DWORD) sizeof(BITMAPFILEHEADER) +    
		pbih->biSize + pbih->biClrUsed    
		* sizeof (RGBQUAD);      
	if (!WriteFile(hf, (LPVOID) &hdr, sizeof(BITMAPFILEHEADER),    
		(LPDWORD) &dwTmp,  NULL))    
	{   
		return FALSE;    
	}   
	
	if (!WriteFile(hf, (LPVOID) pbih, sizeof(BITMAPINFOHEADER)    
		+ pbih->biClrUsed * sizeof (RGBQUAD),    
		(LPDWORD) &dwTmp, ( NULL)))   
	{   
		return FALSE;  
	}   
	
	dwTotal = cb = pbih->biSizeImage;    
	hp = lpBits;    
	if (!WriteFile(hf, (LPSTR) hp, (int) cb, (LPDWORD) &dwTmp,NULL))    
	{   
		return FALSE;    
	}   
	
	if (!CloseHandle(hf))    
		return FALSE;     
	GlobalFree((HGLOBAL)lpBits);   
	return TRUE;   
}