created by Dejavu
这里写的c++代码是对process大神的代码进行移植的
大神用JavaScript写的a_star视频,用到p5.js库
[完结]
a_star 简介
a star(a*) 是一种启发式路径规划的算法，该算法得到的路径不一定是全局最优的，但是因为路径规划速度较快所以被广泛使用。
-
A算法访问的节点比较少，因此可以缩短搜索时间。他的算法思想是:
最终路径长度f = 起点到该点的已知长度h + 该点到终点的估计长度g。O表(open): 待处理的节点表。 C表(close): 已处理过的节点表。 伪代码
function A*(start, goal)
// The set of nodes already evaluated
closedSet := {}
// The set of currently discovered nodes that are not evaluated yet.
// Initially, only the start node is known.
openSet := {start}
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
cameFrom := the empty map
// For each node, the cost of getting from the start node to that node.
gScore := map with default value of Infinity
// The cost of going from start to start is zero.
gScore[start] := 0
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
fScore := map with default value of Infinity
// For the first node, that value is completely heuristic.
fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
current := the node in openSet having the lowest fScore[] value
if current = goal
return reconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)
for each neighbor of current
if neighbor in closedSet
continue // Ignore the neighbor which is already evaluated.
if neighbor not in openSet // Discover a new node
openSet.Add(neighbor)
// The distance from start to a neighbor
tentative_gScore := gScore[current] + dist_between(current, neighbor)
if tentative_gScore >= gScore[neighbor]
continue // This is not a better path.
// This path is the best until now. Record it!
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
function reconstruct_path(cameFrom, current)
total_path := [current]
while current in cameFrom.Keys:
current := cameFrom[current]
total_path.append(current)
return total_path
c++ 实现
需要opencv库，如果自己添加opencv内的类型到该文件也可以不用配置opencv
-
a_star.h 存放数据结构
#ifndef _a_star_h #define _a_star_h #include <opencv2/opencv.hpp> #include <stdio.h> using namespace std; using namespace cv; #define N 100 #define M 100 #define map_info(v) obj.info.map[v.x][v.y] #define map_center(v) obj.info.map[v.x][v.y].center #define isNeighborExist (current.neighbors[i].x>=0 && current.neighbors[i].y>=0) #define copy(v1,v2) v1.assign(v2.begin(), v2.end()) #define add(v1,v2) vector<Point>::iterator it;for(it = v2.begin();it!=v2.end();++it) v1.push_back(*it) #define add_o(v1,v2) vector<Info>::iterator it_o;for(it_o = v2.begin();it_o!=v2.end();++it_o) v1.push_back(*it_o) #define COLS 1024 #define ROWS 720 #define DUBUG false class Pt { public: Point pt; int r; }; class Info { public: bool isWall; Point index; Point pt; Point center; double F; double G; double H; Point neighbors[8]; int previous; Point previous_pt; void init() { F = 0; G = 0; H = 0; previous = -1; previous_pt = Point(-1,-1); for (int i = 0;i < 8;i++) neighbors[i] = Point(-1, -1); } void addNeighbors() { int x = index.x, y = index.y; if (x > 0 && y > 0) neighbors[0] = Point((x - 1), (y - 1)); if (y > 0) neighbors[1] = Point(x, (y - 1)); if (x < COLS - 1 && y > 0) neighbors[2] = Point((x + 1), (y - 1)); if (x < COLS - 1) neighbors[3] = Point((x + 1), y); if (x < COLS - 1 && y < ROWS - 1) neighbors[4] = Point((x + 1), (y + 1)); if (y < ROWS - 1) neighbors[5] = Point(x, (y + 1)); if (x > 0 && y < ROWS - 1) neighbors[6] = Point((x - 1), (y + 1)); if (x > 0) neighbors[7] = Point((x - 1), y); } int sq() { return index.y*COLS + index.x; } void show(Mat &src, Scalar rectColor) { circle(src,center,25,Scalar(100,200,0),-1); } }; class GridMapInfo { public: Mat obj_img; Info map[N][M]; Point target, dest; }; class Obj { public: Mat obj_src; Pt target, dest; GridMapInfo info; }; class Params { public: Point start,end; vector<Point> path_pt; vector<Info> target_obj; Mat take_look; }; class Select_path { public: bool direct; double F; vector<Info> path_obj; vector<Point> path; }; double heuristic(Point spot_pt, Point end_pt); double a_star(Obj obj,Params ¶ms); #endif -
a_star.cpp
#include "a_star.h" double heuristic(Point spot_pt, Point end_pt) { double dx = spot_pt.x - end_pt.x; double dy = spot_pt.y - end_pt.y; return sqrt(dx*dx*1.0 + dy*dy*1.0); } bool includes(list<Info> _dataSet, Info *target) { list<Info>::iterator it = _dataSet.begin(); for (int i = 0; i<_dataSet.size(); i++) { if (it->sq() == target->sq()) return true; else ++it; } return false; } bool includes(Info _dataSet[],int len,Info* target) { for (int i = 0;i < len;i++) if (_dataSet[i].sq() == target->sq()) return true; return false; } void removeData(list<Info> &_dataSet, Info current) { list<Info>::iterator it = _dataSet.begin(); for (int i = 0; i<_dataSet.size(); i++) { if (it->sq() == current.sq()) { it = _dataSet.erase(it); break; } else ++it; } } double a_star(Obj obj,Params ¶ms) { Point start = params.start, dest = params.end; Mat show = params.take_look.clone(); list<Info> openSet; list<Info> path; list<Info>::iterator it; int closed_len(0); Info closedSet[N*M]; Info current = map_info(start); Info begin = current; Info end = map_info(dest); int winner(0); openSet.push_back(current); params.path_pt.clear(); params.target_obj.clear(); while (openSet.size() > 0) { it = openSet.begin(); if (winner == openSet.size()) winner--; advance(it, winner); current = *it; double minF = it->F; int index(0); for (it = openSet.begin(); it != openSet.end(); ++it, index++) { double currentF = it->F; if (currentF < minF) { winner = index; minF = currentF; current = *it; } } if (current.sq() == end.sq()) { reverse(params.path_pt.begin(),params.path_pt.end()); reverse(params.target_obj.begin(),params.target_obj.end()); return current.F; } params.take_look = show.clone(); removeData(openSet, current); closedSet[closed_len++] = current; int tmpG(0); for (int i = 0; i < 8; i++) { if (!isNeighborExist) continue; Info *neighbor = &obj.info.map[current.neighbors[i].x][current.neighbors[i].y]; if (!includes(closedSet, closed_len, neighbor) && !neighbor->isWall) { tmpG = current.G + 1; if (includes(openSet, neighbor)) { neighbor->G = tmpG; continue; } neighbor->G = tmpG; neighbor->H = heuristic(neighbor->center, end.center); neighbor->F = neighbor->G + neighbor->H; neighbor->previous = current.sq(); neighbor->previous_pt = current.index; openSet.push_back(*neighbor); } } path.clear(); Info tmp = current; path.push_back(tmp); it = path.begin(); while(tmp.previous != -1) { tmp = map_info(tmp.previous_pt); path.push_back(tmp); } params.path_pt.clear(); params.target_obj.clear(); params.path_pt.push_back(end.center); params.target_obj.push_back(map_info(end.index)); for( it = path.begin(); it != path.end(); ++it ) { it->show(params.take_look,Scalar(100,200,0)); params.path_pt.push_back(it->center); params.target_obj.push_back((*it)); } } return 0; }
a_star_gen
