A*算法的寻路中的应用——无阻挡

按照之前转载的文章，自己先实现了下，表示还是很多坑：

#include "stdio.h"
#include <vector>
#include <queue>
#include <map>
#include <set>

using namespace std;

int neigor[][2] ={
	{-1, -1}, {0, -1}, {1, -1}, {1, 0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0}
};

struct node
{
	int x;
	int y;
	node* pre;
	int g;
	int h;

	int getrank() {
		return g+h;
	}
	node(int x,int y, node* pre): x(x)
		, y(y)
		, pre(pre)
		, g(0)
		, h(0){
	}
	node():x(0), y(0), pre(NULL), g(0), h(0){
	}
};

class AStar
{
public:
	int width;
	int height;
	vector<bool> blockVec;
	typedef map<int, node*> nodeMap;
	nodeMap  openMap;      
	nodeMap closeMap;
	node* beginNode;
	node* endNode;
	node* curNode;

	AStar(int w, int h, int* arr, int beginx, int beginy,
		int endx, int endy):width(w)
		, height(h)
		, blockVec(w*h, false)
		, curNode(NULL)
	{
		for (int i = 0; i < w; ++i){
			for (int j = 0; j < h; ++j){
				if (1 == arr[i*w+j]){
					//blockVec[i*w+j] = true;
				}
			}
		}

		beginNode = new node(beginx, beginy, NULL);
		endNode = new node(endx, endy, NULL);
	}

	~AStar() {
		for (nodeMap::iterator iter = openMap.begin(); iter != openMap.end(); ++iter){
			if (iter->second){
				delete iter->second;
			}
		}

		for (nodeMap::iterator iter = closeMap.begin(); iter != closeMap.end(); ++iter){
			if (iter->second){
				delete iter->second;
			}
		}
	}

	int geth(int x, int y) {
		int res = 0;

		int tmp = x - endNode->x;
		tmp = tmp > 0 ? tmp : -tmp;
		res += tmp;

		tmp = y - endNode->y;
		tmp = tmp > 0 ? tmp : -tmp;
		res += tmp;

		return res * 10;
	}

	int getg(int curx, int cury, int x, int y) {
		if (x == curx || y == cury)
			return 10;
		return 14;
	}

	int getid(node* pnode){
		if (NULL == pnode) {
			return 0;
		}
		return pnode->x * width + pnode->y;
	}

	void eraseFromOpenList(node* pnode) {
		openMap.erase(getid(pnode));
	}

	void eraseFromCloseList(node* pnode) {
		closeMap.erase(getid(pnode));
	}

	void addOpenList(node* pnode) {
		openMap[getid(pnode)] = pnode;
	}

	void addCloseList(node* pnode) {
		closeMap[getid(pnode)] = pnode;
	}

	bool isInCloseList(int id) {
		return closeMap.find(id) != closeMap.end();
	}

	bool isInOpenList(int id) {
		return openMap.find(id) != openMap.end();
	}

	node* getNodeFromOpenList(int id) {
		node* pnode = nullptr;

		nodeMap::iterator iter = openMap.find(id);
		if (openMap.end() != iter){
			pnode = iter->second;
		}

		return pnode;
	}

	node* getNodeFromCloseList(int id) {
		node* pnode = nullptr;

		nodeMap::iterator iter = closeMap.find(id);
		if (closeMap.end() != iter){
			pnode = iter->second;
		}

		return pnode;
	}

	bool isNeighbor(node* node1, node* node2) {
		if (!node2 || !node1){
			return false;
		}

		return node1->x == node2->x || node1->y == node2->y
			|| (node1->x == node2->x+1 && node1->y == node2->y+1 )
			|| (node1->x == node2->x-1 && node1->y == node2->y-1 );
	}

	node* getBetterNode() {
		if (openMap.empty()){
			return nullptr;
		}

		nodeMap::iterator iter = openMap.begin();
		node* resNode = iter->second;
		int res = 10000;
		for (; iter != openMap.end(); ++iter){
			if (isNeighbor(iter->second, curNode) && iter->second->getrank() < res){
				res = iter->second->getrank();
				resNode = iter->second;
			}
		}

		return resNode;
	}

	int getnaturenum(int x) {
		return x > 0 ? x : -x;
	}
	bool isBlock(int x,int y) {
		return blockVec[getnaturenum(x)*width+getnaturenum(y)];
	}

	void printInside(node* pNode){
		if (nullptr == pNode){
			return;
		}

		printInside(pNode->pre);
		printf("(%d,%d)->", pNode->x, pNode->y);
	}

	void printPath(){
		printInside(curNode);
		printf("\n");
	}

	void findpath(){
		if (beginNode == endNode) {
			return;
		}

		openMap[getid(beginNode)] = beginNode;

		node* fminNode = nullptr;
		curNode = beginNode;
		bool isFind = false;

		do {
			if (true == isFind){
				break;
			}
			fminNode = getBetterNode();
			if (fminNode == endNode || fminNode == NULL) {
				curNode = nullptr;
				break;
			}

			eraseFromOpenList(fminNode);
			addCloseList(fminNode);

			for (int i = 0; i < 8; ++i) {
				int x = fminNode->x + neigor[i][0];
				int y = fminNode->y + neigor[i][1];

				if (x >= 0 && x < width
					&& y >= 0 && y < height) {
						int id = getnaturenum(x) * width + getnaturenum(y);
						if (true == isBlock(x, y) || true == isInCloseList(id)) {
							continue;
						}
						
						node* newNode = getNodeFromOpenList(id);
						if (NULL == newNode){
							newNode = new node(x, y, fminNode);
							newNode->g = getg(x, y, fminNode->x, fminNode->y);
							newNode->h = geth(x, y);
							newNode->pre = fminNode;
							addOpenList(newNode);

							if (endNode->x == x && endNode->y == y){
								newNode->pre = curNode;
								curNode = newNode;
								printf("find it!\n");
								isFind = true;
								break;
							}
						}
						else {
							 if (newNode->g > fminNode->g + getg(newNode->x, newNode->y, fminNode->x, fminNode->y)){
								newNode->pre = curNode;
								newNode->g = curNode->g + getg(x, y, curNode->x, curNode->y);

								curNode = newNode;

								eraseFromOpenList(curNode);
								addCloseList(curNode);

								continue;
							}
						}
				}

				curNode = fminNode;
			}

		} while (true);
	}
};

int main()
{
	int a[] ={
		0,1,0,0,
		0,1,1,0
		,0,0,0,0
		, 0,1,0,0
	};

	AStar star(4, 4, a, 0, 1, 3,3);
	star.findpath();
	star.printPath();
}