A*/AStar规划算法(C++版本）

AStart.h头文件

#pragma once
#include<vector>
#include<list>
#include<iostream>
using namespace std;
const int kCost1 = 10; //直移一格消耗
const int kCost2 = 14; //斜移一格消耗
typedef struct Point {
	Point* parent;
	int x;
	int y;
	int f;
	int g;
	int h;
	Point(int _x,int _y) :x(_x), y(_y), f(0), g(0), h(0), parent(NULL) {};
}* PPoint;
class AStart
{
private:
	vector<vector<int>> map;
	list<PPoint> openList;
	list<PPoint> closeList;
public:
	void initMap(vector<vector<int>> &_map);
	PPoint findPath(PPoint start, PPoint end);
	PPoint inInList(PPoint point, list<PPoint> &list);
	list<PPoint> findAroundPoint(PPoint curPoint);
	PPoint findLeastFPoint();
	int calG(PPoint& point, PPoint& parentPoint);
	void calF(PPoint& point, PPoint& endPoint);
	int calStep(PPoint& p1, PPoint& p2);
	int calDistance(PPoint& p1, PPoint& p2);
};

AStart.cpp文件

#include "AStart.h"

void AStart::initMap(vector<vector<int>> &_map) {
	map = _map;
	//map[0][0] = 99;
	//cout << map[0][0] << ' '<<map[0][1]<< endl;
}
int AStart::calDistance(PPoint& p1, PPoint& p2) {
	return (abs(p1->x - p1->x) + abs(p1->y + p2->y)) * kCost1;
}
int AStart::calStep(PPoint& p1, PPoint& p2) {
	int dt = abs(p1->x - p2->x) + abs(p1->y - p2->y);
	if (dt == 1) {
		return kCost1;
	}
	else if (dt == 0) {
		return 0;
	}
	else {
		return kCost2;
	}
}
int AStart::calG(PPoint& point, PPoint &parentPoint) {
	return parentPoint->g + calStep(point, parentPoint);
}
void AStart::calF(PPoint& point,PPoint &endPoint) {
	if (point->parent) {
		point->g = calG(point,point->parent);
		point->h = calDistance(point, endPoint);
		point->f = point->g + point->h;
	}
}
PPoint AStart::findPath(PPoint start, PPoint end) {
	calF(start,end);
	openList.push_back(start);
	while (openList.size()>0)
	{
		// 取得F最小的点
		PPoint curPoint= findLeastFPoint();
		// 将点从open列表中移除
		openList.remove(curPoint);
		// 将点添加到close列表中
		closeList.push_back(curPoint);
		//查找当前点的周边节点
		list<PPoint> aroundList =  findAroundPoint(curPoint);
		for (auto arountPoint : aroundList) {
			if (arountPoint->x == end->x && arountPoint->y == end->y) {
				//已经找到目标点
				arountPoint->parent = curPoint;
				calF(arountPoint,end);
				return arountPoint;
			}
			// 在close表
			else if (inInList(arountPoint, closeList)) {
				//不用处理
			}
			else if (inInList(arountPoint, openList)) {
				int tmpG = calG(arountPoint, curPoint);
				if (tmpG < arountPoint->g) {
				//更新节点
					arountPoint->g = tmpG;
					arountPoint->f = arountPoint->g + arountPoint->h;
					arountPoint->parent = curPoint;
				}
			}
			else {
				//添加到open表中
				arountPoint->parent = curPoint;
				calF(arountPoint, end);
				openList.push_back(arountPoint);
			}
		}
	}
	return NULL;
}
PPoint AStart::findLeastFPoint() {
	PPoint tmpPoint = NULL;
	for (auto point : openList) {
		if (!tmpPoint || tmpPoint->f>point->f) {
			tmpPoint = point;
		}
	}
	return tmpPoint;
}
PPoint AStart::inInList(PPoint point, list<PPoint> &list) {
	for (auto item : list) {
		if (item->x == point->x && item->y == point->y) {
			return item;
		}
	}
	return NULL;
}

list<PPoint> AStart::findAroundPoint(PPoint curPoint) {
	list<PPoint> around;
	int mapXSize = map.size();
	int mapYSize = map[0].size();
	for (int x = curPoint->x - 1; x <= curPoint->x + 1; x++) {
		if (x<0 || x>=mapXSize) {
			continue;
		}
		for (int y = curPoint->y - 1; y <= curPoint->y + 1; y++) {
			if (y < 0 || y >= mapYSize) {
				continue;
			}
			if (map[x][y] == 1) {
				continue; // 不可达
			}
			else {
				PPoint newPoint = new Point(x, y);
				around.push_back(newPoint);
			}
		}
	}
	return around;
}

主函数Main

#include <iostream>
#include <vector>
#include "AStart.h"
using namespace std;
void printMap(vector<vector<int>> map) {
    for (auto& row : map) {
        for (auto& cell : row) {
            cout << cell << ' ';
        }
        cout << endl;
    }
}
void pirntPath(PPoint path, vector<vector<int>> &map) {
    PPoint p = path;
    while (p->parent) {
        cout << p->x << ',' << p->y << endl;
        map[p->x][p->y] = 7;
        p = p->parent;
    }
    map[p->x][p->y] = 5;
}
int main(){
	vector<vector<int>> map= { {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
                                       {1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
                                       {1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
                                       {1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
                                       {1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
                                       {1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
                                       {1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
                                       {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1} };

    AStart astar;
    PPoint start=new Point(1, 1);
    PPoint end=new Point(6, 10); 
    astar.initMap(map);
    PPoint path=astar.findPath(start, end);
    printMap(map);
    cout << "行驶轨迹" << endl;
    pirntPath(path, map);
    printMap(map);
    return 1;
	}