A_start算法C++实现

#include <algorithm>
#include <iostream>
#include <vector>
#include <stdio.h>

using namespace std;
class Node;
bool compare(Node* n1, Node* n2);

class Node {
public:  //public表示公有基类，private表示私有基类，protected表示被保护基类
	int x;
	int y;
	int f;//g+h
	int g;//起点到当前点的消耗 
	int h;//终点到当前点的理论消耗 
	Node* parent;  //定义的Node类后面加*表示指针，&表示取地址符
	Node(int x, int y) {
		this->x = x; //对结构体中的成员变量x进行赋值
		this->y = y;
		this->f = 0;
		this->g = 0;
		this->h = 0;
		this->parent = NULL; //C语言中NULL指代0，在C++中NULl表示当前指针没有指向任何位置，即允许不指向特定位置的指针存在且合法
	}
	Node(int x, int y, Node* parent) {
		this->x = x;
		this->y = y;
		this->f = 0;
		this->g = 0;
		this->h = 0;
		this->parent = parent;
	}
};

//0表示可通过1表示障碍物 
int map[101][101] = {
	{ 0, 0, 0, 1, 0, 1, 0, 0, 0 },
	{ 0, 0, 0, 1, 0, 1, 0, 0, 0 },
	{ 0, 0, 0, 0, 0, 1, 0, 0, 0 },
	{ 0, 0, 0, 1, 0, 1, 0, 1, 0 },
	{ 0, 0, 0, 1, 0, 1, 0, 1, 0 },
	{ 0, 0, 0, 1, 0, 0, 0, 1, 0 },
	{ 0, 0, 0, 1, 0, 0, 0, 1, 0 }
};
vector<Node*> openList;				// 要能要走的点
vector<Node*> closeList;
int row;//地图总行数 
int col;//地图总列数 
Node* startPos;//开始点 
Node* endPos;//结束点 
int weightW;//平移权重 
int weightWH;//斜移权重 

// H 为到终点的距离
int countH(Node* sNode, Node* eNode) {
	// 这里的计算方法多种多样，选取一个合适即可
	int w = abs(sNode->x - eNode->x);
	int h = abs(sNode->y - eNode->y);
	int cost = min(w, h)*weightWH + abs(w - h)*weightW;
	return cost;
}

//************************************
// Method:    countFGH
// Access:    public 
// Returns:   void
// Qualifier: 计算一个节点的 F G H 值
// Parameter: Node * sNode	当前结点
// Parameter: Node * eNode    终点
// Parameter: int cost        父结点到当前结点的消耗g
//************************************
void countFGH(Node* sNode, Node* eNode, int cost) {
	int h = countH(sNode, eNode);
	int g = sNode->parent->g + cost;
	int f = h + g;
	sNode->f = f;
	sNode->h = h;
	sNode->g = g;
}
//检测列表是否包含指定节点
int isContains(vector<Node*>* v, int x, int y) {
	for (int i = 0; i < v->size(); i++) {
		if (v->at(i)->x == x&&v->at(i)->y == y) {
			return i;
		}
	}
	return -1;
}
void initMap() {
	row = 7;
	col = 9;
	weightW = 10;
	weightWH = 15;
	startPos = new Node(5, 1);
	endPos = new Node(3, 8);
}

// 输出地图
void printMap() {
	for (int i = 0; i < row; i++) {
		for (int j = 0; j < col; j++) {
			printf("%d ", map[i][j]);
		}
		printf("
");
	}
}

// 对 f 又进行了一次排序
/*
返回 0 互换位置，返回 1 不换
如果n1 大于 n2，返回0， 则需要进行一次互换
如果n1 小于等于 n2，返回1，则不需要互换

具体来说，就是当前面的大于后面的时，那么就要进行一次互换，则排序完成后，
是由小到大开始进行排序的。
*/
bool compare(Node* n1, Node* n2) {
	return n1->f < n2->f;
}

//************************************
// Method:    checkMove
// Returns:   void
// Qualifier: 寻找下一步移动的相关的节点，如果有合并将此结点填入openlist中	
// Parameter: int x			下一步要移动的x坐标
// Parameter: int y			下一步要移的y坐标
// Parameter: Node * parent    移动后的父结点
// Parameter: Node * end		 这个是真正的终点坐标	
// Parameter: int cost		 下一步要移动的结点到父节点的消耗 
//************************************
void checkMove(int x, int y, Node* parent, Node* end, int cost) {

	// 如果下一步移动的节点是墙体等障碍物，那直接返回
	if (map[x][y] == 1) {
		return;
	}

	// 如果下一步要走的格点为以经走过的格点，那么直接返回
	if (isContains(&closeList, x, y) != -1) {
		return;
	}

	// 取出
	int index = -1;
	if ((index = isContains(&openList, x, y)) != -1) {
		//是否存在更小的G值
		Node* sNode = openList[index];		// 其实这里也是下一步要走的节点
		if (parent->g + cost < sNode->g) {
			// 备： 这里共两步
			// 第一步:它的父亲设置为当前方格 (未完成相关代码)
			// 第二步:并重新计算它的 G 和 F 值 (完成了相关代码)
			sNode->g = parent->g + cost;
			sNode->f = sNode->g + sNode->h;
		}
	}
	else {
		Node* n = new Node(x, y, parent);
		countFGH(n, end, cost);
		// 在check里面进行了push
		openList.push_back(n);
	}
}
void printPath(Node* node) {
	if (node->parent != NULL) {
		printPath(node->parent);
	}
	//将走过的点标记为2 
	//map[node->x][node->y] = 2;
	printf("->%d,%d", node->x, node->y);
}
void releaseNode(Node* n) {
	// 这个递归使的我也是服了气了
	if (n->parent != NULL) {
		releaseNode(n->parent);
	}
	delete n;
}

//************************************
// Method:    startSearch
// Returns:   int         -1错误0没找到1找到 
// Qualifier: 主寻路算法
// Parameter: Node * start  起点
// Parameter: Node * end    终点
//************************************
int startSearch(Node* start, Node* end) {
	// 两次判错
	// 开始或结束点是否符合要求
	if (start->x < 0 || start->y < 0 || start->x >= row || start->y >= col
		|| end->x < 0 || end->y < 0 || end->x >= row || end->y >= col) {
		return -1;
	}
	// 如果结束点或开始点在墙上，那么直接为错 
	if (map[start->x][start->y] == 1 || map[end->x][end->y] == 1) {
		return -1;
	}

	start->h = countH(start, end);
	start->f = start->h + start->g;
	openList.push_back(start);
	Node* root = NULL;
	int find = 0;

	while (openList.size() > 0) {
		// root 这里是第一个根节点嘛，就是起始点
		root = openList[0];
		// 当找到终点时
		if (root->x == end->x&&root->y == end->y) {
			find = 1;
			break;
		}

		// 对八个方向分批次判断
		//上下左右 
		if (root->x > 0) {
			checkMove(root->x - 1, root->y, root, end, weightW);
		}
		if (root->y > 0) {
			checkMove(root->x, root->y - 1, root, end, weightW);
		}
		if (root->x < row - 1) {
			checkMove(root->x + 1, root->y, root, end, weightW);
		}
		if (root->y < col - 1) {
			checkMove(root->x, root->y + 1, root, end, weightW);
		}

		// 左下
		if (root->x < row - 1 && root->y>0) {
			checkMove(root->x + 1, root->y - 1, root, end, weightW);
		}
		// 右上
		if (root->y < col - 1 && root->x>0) {
			checkMove(root->x - 1, root->y + 1, root, end, weightW);
		}
		// 左上
		if (root->x > 0 && root->y > 0) {
			checkMove(root->x - 1, root->y - 1, root, end, weightW);
		}
		// 右下
		if (root->y < col - 1 && root->x < row - 1) {
			checkMove(root->x + 1, root->y + 1, root, end, weightW);
		}
		closeList.push_back(root);
		openList.erase(openList.begin());
		sort(openList.begin(), openList.end(), compare);
	}
	if (find) {
		printPath(root);
		printf("
");
		//printMap();
	}
	releaseNode(root);
	openList.clear();
	closeList.clear();
	return find;
}


int main(int argc, char *argv[]) {
	initMap();
	printMap();
	int t = startSearch(startPos, endPos);
	if (t == 1) {
		printf("find!");
	}
	else if (t == 0) {
		printf("no find.");
	}
	else {
		printf("error!");
	}
	system("pause");
	return 0;
}