C++之AStar寻路算法

仅以记录

有一种算法

名为AStar

它的作用是求图中两点之间的最短路径

“沉迷”该算法的我

自己编写了一个版本

注释虽少

但求传神

代码虽“恶心”

但求理解

#include <iostream>
#include <vector>
#include <algorithm>
using namespace std;
/* run this program using the console pauser or add your own getch, system("pause") or input loop */

int weight = 10;
bool dj = true;

class Map{
	public:
		vector<vector<int>> data;
		int start_x, start_y, end_x, end_y;
		Map(vector<vector<int>> Array, int Start_x, int Start_y, int End_x, int End_y){
			data = Array;
			start_x = Start_x;
			start_y = Start_y;
			end_x = End_x;
			end_y = End_y;
		}
};

class Node{
	public:
		int x, y, G, H;
		vector<pair<int, int>> path;
		Node(int x_pos, int y_pos, int g, int h, vector<pair<int, int>> Path){
			x = x_pos;
			y = y_pos;
			G = g;
			H = h;
			path = Path;
			path.push_back(make_pair(x, y));
		}
		vector<Node> getNeighbor(vector<vector<int>> mapData, int end_x, int end_y){

			int LengthY = mapData.size() - 1;
			int LengthX = mapData[0].size() - 1;
			vector<Node> results;
			if (x != 0 and mapData[y][x - 1] != 0){
				Node WestNode(x - 1, y, G + weight, (abs(x - 1 - end_x) + abs(y - end_y)) * weight, path);
				results.push_back(WestNode);
			}
			if (x != LengthX and mapData[y][x + 1] != 0){
				Node EastNode(x + 1, y, G + weight, (abs(x + 1 - end_x) + abs(y - end_y)) * weight, path);
				results.push_back(EastNode);
			}
			if (y != 0 and mapData[y - 1][x] != 0){
				Node NorthNode(x, y - 1, G + weight, (abs(x - end_x) + abs(y - 1 - end_y)) * weight, path);
				results.push_back(NorthNode);
			}
			if (y != LengthY and mapData[y + 1][x] != 0){
				Node SouthNode(x, y + 1, G + weight, (abs(x - end_x) + abs(y + 1 - end_y)) * weight, path);
				results.push_back(SouthNode);
			}
			if (dj){
				if (x != 0 and y != 0 and mapData[y - 1][x - 1] != 0){
					Node NWNode(x - 1, y - 1, G + weight * 1.4, (abs(x - 1 - end_x) + abs(y - 1 - end_y)) * weight, path);
					results.push_back(NWNode);
				}
				if (x != LengthX and y != 0 and mapData[y - 1][x + 1] != 0){
					Node NENode(x + 1, y - 1, G + weight * 1.4, (abs(x + 1 - end_x) + abs(y - 1 - end_y)) * weight, path);
					results.push_back(NENode);
				}
				if (x != 0 and y != LengthY and mapData[y + 1][x - 1] != 0){
					Node SWNode(x - 1, y + 1, G + weight * 1.4, (abs(x - 1 - end_x) + abs(y + 1 - end_y)) * weight, path);
					results.push_back(SWNode);
				}
				if (x != LengthX and y != LengthY and mapData[y + 1][x + 1] != 0){
					Node SENode(x + 1, y + 1, G + weight * 1.4, (abs(x + 1 - end_x) + abs(y + 1 - end_y)) * weight, path);
					results.push_back(SENode); 
				}
			}
			return results;
		}
		
		bool inList(vector<Node> List){
			for (Node i : List){
				if (i.x == x and i.y == y)
					return true;
			}
			return false;
		}
		
		void changeG(vector<Node> List){
			for (Node i : List){
				if (i.x == x and i.y == y){
					if (i.G > G)
						i.G = G;
				}
			}
		}
};

bool ST(Node x, Node y){
	if (x.G < y.G ) 
		return true;
	return false;
}

vector<pair<int, int>> AStar(Map map_array){
	vector<Node> open_list;
	vector<Node> close_list;
	int start_x, start_y, end_x, end_y;
	start_x = map_array.start_x;
	start_y = map_array.start_y;
	end_x = map_array.end_x;
	end_y = map_array.end_y;
	vector<pair<int, int>> v;
	Node startNode(start_x, start_y, 0, 0, v); 
	close_list.push_back(startNode);
	Node currentNode = startNode;
	vector<Node> Neighbors;
	while (end_x != currentNode.x or end_y != currentNode.y){
		Neighbors = currentNode.getNeighbor(map_array.data, end_x, end_y);
		for (Node i : Neighbors){ 
			if (not (i.inList(close_list))){ // 问题出在这里 

				if (i.inList(open_list)){
					i.changeG(open_list);
				}
				else{
					open_list.push_back(i);
				}
			}
		}

		sort(open_list.begin(), open_list.begin()+open_list.size(), ST);
		currentNode = open_list[0];
		open_list.erase(open_list.begin());
		close_list.push_back(currentNode);
	}

	vector<pair<int, int>> results;
	for (int i=0; i<currentNode.path.size(); i++){
			results.push_back(make_pair(currentNode.path[i].first, currentNode.path[i].second));
	}
	return results;
} 

int main(int argc, char* argv) {
	vector<vector<int>> mymap;
	vector<int> v;
	v.push_back(1);
	v.push_back(1);
	v.push_back(1);
	v.push_back(1);
	v.push_back(1);
	v.push_back(1);
	mymap.push_back(v);
	v.clear();
	v.push_back(1);
	v.push_back(0);
	v.push_back(0);
	v.push_back(0);
	v.push_back(0);
	v.push_back(1);
	mymap.push_back(v);
	v.clear();
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(0);
	v.push_back(0);
	v.push_back(1);
	mymap.push_back(v);
	v.clear();
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(0);
	mymap.push_back(v);
	v.clear();
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(1);
	mymap.push_back(v);
	v.clear();
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(0);
	v.push_back(1);
	v.push_back(1);
	mymap.push_back(v);
	Map map(mymap, 0, 1, 4, 5);
	vector<pair<int, int>> results;
	results = AStar(map);
	for (pair<int, int> p: results){
		int a = p.first;
		int b = p.second;
		cout << "{" << a << ", " << b << "}, ";
	}
	return 0;
	
}