A*算法的实现

看见CSDN博主小满（bill man）个人原创实现A*算法，没有全部的代码，且实现的是游戏中的应用。故想到不如自己在他的基础上把代码补全，同时将算法独立出来。只实现A*算法，不涉及游戏引擎部分。

算法思想在这篇博客里有很详尽的说明：http://blog.vckbase.com/panic/archive/2005/03/20/3778.html

代码核心部分皆取自小满（bill man）的博文：http://blog.csdn.net/bill_man/article/details/7296560

 

这是我的第一篇CSDN博客，感谢老婆一直鼓励我，我会继续努力的！哈哈……

 

直接上代码：

//*** Map.h ***

#ifndef __MAP_H
#define __MAP_H

class Map
{
public:
	Map(int wid, int hgt);
	~Map();

	int  width()  {	return m_width;	 }
	int  height() {	return m_height; }

	void setobstacle(int col, int row);
	void setstep(int col, int row);
	char value(int col, int row);

private:
	char* matrix;
	int   m_width;
	int   m_height;
};

#endif

 

 

//*** Map.cpp ***

#include <string>
#include "Map.h"

Map::Map(int wid, int hgt)
{
	m_width  = wid;
	m_height = hgt;

	int size = wid * hgt;
	matrix = new char[size];
	memset(matrix, '0', size * sizeof(char)); //大小不是size ！！！
}

Map::~Map()
{
	delete[] matrix;
}

void Map::setobstacle(int col, int row)
{
	if(matrix)
		matrix[row * m_width + col] = '1';
}

void Map::setstep(int col, int row)
{
	if(matrix)
		matrix[row * m_width + col] = '*';
}

char Map::value(int col, int row)
{
	return matrix[row * m_width + col];
}

 

 

//*** AstarItem.h ***

#ifndef __ASTARITEM_H
#define __ASTARITEM_H


class AstarItem
{
public:
	AstarItem(void)  {}
	~AstarItem(void) {}

	void setpos(int col, int row);

	int col() { return id_col; }
	int row() { return id_row; }

	int  getcol() { return id_col; }
	int  getrow() { return id_row; }

	void setf(int f);
	int  getf()   { return id_f;   }

	void setg(int g);
	int  getg()   { return id_g;   }

	void seth(int h);
	int  geth()   { return id_h;   }

	void setfid(int fid);
	int  getfid() { return id_fid; }

private:
	int id_row;  //行
	int id_col;  //列
	int id_f;    //估价函数f = g + h
	int id_g;    //实际代价
	int id_h;    //估计代价
	int id_fid;  //父节点id
};

#endif

 

 

 

//*** AstarItem.cpp ***

#include "AStarItem.h"

void AstarItem::setpos(int col, int row)
{
	id_col = col;
	id_row = row;
}

void AstarItem::setf(int f)
{
	id_f = f;
}

void AstarItem::setg(int g)
{
	id_g = g;
}

void AstarItem::seth(int h)
{
	id_h = h;
}

void AstarItem::setfid(int fid)
{
	id_fid = fid;
}

 

 

//***** AStar.h ****

#ifndef __ASTAR_H
#define __ASTAR_H

#include <vector>
#include <stack>
using namespace std;

class Map;
class AstarItem;


class Astar
{
public:
	Astar(Map* the_map) : map(the_map) {}
	~Astar(void);

	void reset();

	void findpath(int curX, int curY, int aimX, int aimY);

	int  getH(int col, int row);
	int  getG(int col, int row, int fid);
	void fromopentoclose();
	void removefromopen();
	void starsearch(int fid);
	void getpath();
	void showpath();

	bool checkmap(int col, int row);
	bool checkopen(int col, int row, int fid);
	bool checkclose(int col, int row);
	void addtoopen(int col, int row, int fid);

	void resetsort(int last);

private:
	Map* map;

	int  curCol;
	int  curRow;
	int  aimCol;
	int  aimRow;

	stack<AstarItem*>  path;
	vector<AstarItem*>  open;
	vector<AstarItem*>  close;
};

#endif

//**** AStar.cpp ****

#include <iostream>
#include <stdlib.h>
#include "AStar.h"
#include "Map.h"
#include "AStarItem.h"

using namespace std;


Astar::~Astar(void)
{
	reset();
}

void Astar::reset()
{
	open.clear();
	close.clear();
	
	while(!path.empty())
		path.pop();
}


int Astar::getH(int col, int row)
{
	return abs(aimCol - col) * 10 + abs(aimRow - row) * 10;
}


int Astar::getG(int col, int row, int fid)
{
	int fx = close[fid]->getcol();
	int fy = close[fid]->getrow();
	int fg = close[fid]->getg();

	if((fx - col != 0) && (fy - row != 0))
	{ // 上下左右
		return fg + 14;
	}
	else
	{ // 四角
		return fg + 10;
	}
}


void Astar::findpath(int curX, int curY, int aimX, int aimY)
{
	curCol = curX;
	curRow = curY;
	aimCol = aimX;
	aimRow = aimY;

	//优先队列的0项，无意义
	AstarItem* temp = new AstarItem();
	open.push_back(temp);

	AstarItem* temp1 = new AstarItem();
	temp1->setpos(curCol, curRow);
	temp1->setg(0);
	int ag = getH(curCol, curRow);
	temp1->seth(ag);
	temp1->setf(ag);
	temp1->setfid(0);
	open.push_back(temp1);

	while(open.size() > 1)
	{
		fromopentoclose();
		int fatherid = close.size() - 1;
		int mycol = close.back()->getcol();
		int myrow = close.back()->getrow();

		if((aimCol == mycol) && (aimRow == myrow))
		{
			getpath();
			break;
		}
		else
		{
			starsearch(fatherid);
		}
	}

	//show path and reset
	showpath();
	reset();
}


void Astar::fromopentoclose()
{
	AstarItem* temp = open[1];
	close.push_back(temp);

	removefromopen();
}

void Astar::removefromopen()
{
	int last = open.size() - 1;
	open[1] = open[last];
	open.pop_back();
	last = open.size() - 1;
	int head = 1;
	while((head * 2) <= last)
	{ // shift down
		int child = head * 2;
		if((child + 1) <= last && open[child + 1]->getf() < open[child]->getf())
			child++;
		if(open[head]->getf() <= open[child]->getf())
			break;
	
		AstarItem* temp = open[child];
		open[child] = open[head];
		open[head]  = temp;

		head = child;
	}

}


void Astar::starsearch(int fid)
{
	int col = close[fid]->getcol();
	int row = close[fid]->getrow();

	int mycol = col;
	int myrow = row - 1;
	if(myrow >= 0 && checkmap(mycol, myrow))
	{
		if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow))
		{
			addtoopen(mycol, myrow, fid);
		}
	}

	mycol = col - 1;
	myrow = row;
	if(mycol >= 0 && checkmap(mycol, myrow))
	{
		if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow))
		{
			addtoopen(mycol, myrow, fid);
		}
	}

	mycol = col;
	myrow = row + 1;
	int w = map->width();
	if(myrow < map->width() && checkmap(mycol, myrow))
	{
		if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow))
		{
			addtoopen(mycol, myrow, fid);
		}
	}

	mycol = col + 1;
	myrow = row;
	if(mycol < map->height() && checkmap(mycol, myrow))
	{
		if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow))
		{
			addtoopen(mycol, myrow, fid);
		}
	}

}


bool Astar::checkmap(int col, int row)
{
	// 是否为障碍物
	return (map->value(col, row) == '0');
}




bool Astar::checkopen(int col, int row, int fid)
{ // 是否在open表中。若在，比较g值
	vector<AstarItem*>::iterator iter = open.end() - 1; //第0个为填充项，无意义
	int i = open.size() - 1;
	for( ; iter != open.begin(), i > 0; --iter, --i)
	{
		AstarItem* item = *iter;
		if((item->getcol() == col) && (item->getrow() == row))
		{
			int tempG = getG(col, row, fid);
			if(tempG < (item->getg()))
			{
				item->setg(tempG);
				item->setf(tempG + item->geth());
				item->setfid(fid);

				resetsort(i);
			}
			return false;
		}
	}
	return true;
}


bool Astar::checkclose(int col, int row)
{ // 是否在open表中，避免重复
	vector<AstarItem*>::iterator iter = close.begin();
	for( ; iter != close.end(); ++iter)
	{
		AstarItem* item = *iter;
		if((item->getcol() == col) && (item->getrow() == row))
		{
			return false;
		}
	}
	return true;
}

void Astar::addtoopen(int col, int row, int fid)
{
	AstarItem* temp = new AstarItem();
	temp->setpos(col, row);
	temp->setfid(fid);
	int g = getG(col, row, fid);
	int h = getH(col, row);
	temp->setg(g);
	temp->seth(h);
	temp->setf(g + h);
	open.push_back(temp);
	
	resetsort(open.size() - 1);
}

void Astar::resetsort(int last)
{ // shift up(因为加入的是一个更小值，所以只需向上调整！)
	while(last > 1)
	{
		int half = last / 2;
		if(open[half]->getf() <= open[last]->getf())
		{
			break;
		}
		AstarItem* temp = open[last];
		open[last] = open[half];
		open[half] = temp;

		last = half;
	}
}


void Astar::getpath()
{
	path.push(close.back());

	while(true)
	{
		if(path.top()->getg() == 0)
		{
			break;
		}
		path.push(close[path.top()->getfid()]);
	}
}

void Astar::showpath()
{
	int width  = map->width();
	int height = map->height();
	while(!path.empty())
	{
		AstarItem* item = path.top();
		path.pop();

		map->setstep(item->col(), item->row());
	}

	cout << "the step:" << endl;
	for(int i = 0; i < height; ++i)
	{
		for(int j = 0; j < width; ++j)
		{
			cout << map->value(i, j) << " " ;
		}
		cout << endl;
	}
}

//***** main.cpp *****

#include <iostream>
#include "Map.h"
#include "AStar.h"
using namespace std;

int main()
{
	Map   map(10, 10);
	Astar astar(&map);

	//set obstacle
	map.setobstacle(0, 2);
	map.setobstacle(1, 2);
	map.setobstacle(2, 2);
	map.setobstacle(3, 2);
	map.setobstacle(4, 2);
	map.setobstacle(2, 5);
	map.setobstacle(3, 5);
	map.setobstacle(4, 5);
	map.setobstacle(5, 5);
	map.setobstacle(6, 5);
	map.setobstacle(7, 5);
	map.setobstacle(8, 5);
	map.setobstacle(9, 5);

	//show map
	cout << "the map: " << endl;
	for(int i = 0; i < 10; ++i)
	{
		for(int j = 0; j < 10; ++j)
		{
			cout << map.value(i, j) << " " ;
		}
		cout << endl;
	}
	cout << endl;

	//find path
	astar.findpath(1, 0, 4, 8);

	cin.get();
	return 0;
}

运行结果：