单向A*：
v>
算法思想：
每次都选择权值最小的结点拓展并记录路径，
如果选择结点为终点，
则找到一条代价最小路径
算法使用优先队列实现
#include <iostream> #include <cstring> #include <cmath> #include <utility> #include <map> #include <queue> using namespace std; const int maxn = 1010; string graph[maxn]; int cost[maxn][maxn]; bool vis[maxn][maxn]; int n, m, sx, sy, tx, ty; // 8个方向 int dir[8][2] = {{1, 0}, {0, 1}, {-1, 0}, {0, -1}, {1, 1}, {-1, -1}, {1, -1}, {-1, 1}}; struct node { int x, y; double dis; struct node* pre; node(int x, int y, double dis, node *pre) { this->x = x; this->y = y; this->dis = dis; this->pre = pre; }; }; map<pair<int, int>, node*> hashmap; int manha(int x, int y) { return abs(x - tx) + abs(y - ty); } class Compare_Node_Pointer { public: bool operator () (node* &a, node* &b) const { return a->dis + manha(a->x, a->y) > b->dis + manha(b->x, b->y); } }; // 记录最后的相遇点，与最短距离 node *ltemp = NULL, *rtemp = NULL; double maxDis = 0x7fffffff; void Astar(node* &s) { memset(vis, 0, sizeof(vis)); // 正向队列和反向队列 priority_queue<node*, vector<node*>, Compare_Node_Pointer> Q; Q.push(s); // dis[u->x][u->y] = 0; while(!Q.empty()) { node* p; p = Q.top(); Q.pop(); if(p->x == tx && p->y == ty) { if(ltemp == NULL) ltemp = p; // else // if(ltemp->dis > p->dis) // ltemp = p; return; } if(vis[p->x][p->y]) continue; vis[p->x][p->y] = 1; for(int i = 0; i < 8; i++) { int vx = p->x + dir[i][0]; int vy = p->y + dir[i][1]; double w = i < 4 ? 1.0 : (double)sqrt(2.0); if(vx < 0 || vy < 0 || vx >= n || vy >= m || graph[vx][vy] == '#') continue; node* v; if(hashmap[pair<int, int> (vx, vy)] != NULL) { v = hashmap[pair<int, int> (vx, vy)]; } else { v = new node(vx, vy, 0x7fffffff, NULL); hashmap[pair<int, int> (vx, vy)] = v; } if(v->dis > p->dis + w + cost[vx][vy]) { v->dis = p->dis + w + cost[vx][vy]; v->pre = p; Q.push(v); } } } } int main() { memset(cost, 0, sizeof(cost)); cout << "Please input row of the map" << endl; cout << "空地用.表示 沙漠用y表示 溪流用r表示 障碍用#表示" << endl; cin >> n; for(int i = 0; i < n; i++) { cin >> graph[i]; for(int j = 0; j < graph[i].size(); j++) { if(graph[i][j] == 'y') cost[i][j] = 4; else if(graph[i][j] == 'r') cost[i][j] = 2; } } m = graph[0].size(); cout << "Please input start and end points" << endl; cin >> sx >> sy; cin >> tx >> ty; cost[sx][sy] = 0; node* s = new node(sx, sy, 0, NULL); hashmap[pair<int, int> (sx, sy)] = s; Astar(s); if(ltemp != NULL) { cout << "DistCost: " << ltemp->dis << endl; node *q = ltemp; while(q) { if(q == s) graph[q->x][q->y] = 'S'; else if(q->x == tx && q->y == ty) graph[q->x][q->y] = 'T'; else graph[q->x][q->y] = '&'; q = q->pre; } for(int i = 0; i < n; i++) cout << graph[i] << endl; } return 0; }
双向A*：
v>
算法思想：
分别从起点和终点使用A*算法，
每次选择两个队列中拥有最小权值的结点进行拓展，
并记录每个点是由来自哪个开始点拓展的，
如果相遇则记录相遇时两端路程加和的最小值。
算法使用两个优先队列实现
#include <iostream> #include <cstring> #include <cmath> #include <utility> #include <map> #include <queue> using namespace std; const int maxn = 1010; string graph[maxn]; int cost[maxn][maxn]; bool vis[maxn][maxn]; int subj[maxn][maxn]; int n, m, sx, sy, tx, ty; // 8个方向 int dir[8][2] = {{1, 0}, {0, 1}, {-1, 0}, {0, -1}, {1, 1}, {-1, -1}, {1, -1}, {-1, 1}}; struct node { int x, y; double dis; struct node* pre; node(int x, int y, double dis, node *pre) { this->x = x; this->y = y; this->dis = dis; this->pre = pre; }; }; map<pair<int, int>, node*> hashmap; int manha(int x, int y, int Tx, int Ty) { return abs(x - Tx) + abs(y - Ty); } class Compare_Node_Pointer { public: bool operator () (node* &a, node* &b) const { if(subj[a->x][a->y] == 0) return a->dis + manha(a->x, a->y, tx, ty) > b->dis + manha(b->x, b->y, tx, ty); else return a->dis + manha(a->x, a->y, sx, sy) > b->dis + manha(b->x, b->y, sx, sy); } }; // 记录最后的相遇点，与最短距离 node *ltemp = NULL, *rtemp = NULL; double maxDis = 0x7fffffff; void Astar(node* &s, node* &t) { memset(vis, 0, sizeof(vis)); memset(subj, -1, sizeof(subj)); // 正向队列和反向队列 priority_queue<node*, vector<node*>, Compare_Node_Pointer> Q; priority_queue<node*, vector<node*>, Compare_Node_Pointer> Q1; Q.push(s); Q1.push(t); subj[s->x][s->y] = 0; subj[t->x][t->y] = 1; // dis[u->x][u->y] = 0; while(!Q.empty() || !Q1.empty()) { node* p; if(Q.empty() || (!Q1.empty() && Q.top() > Q1.top())) { p = Q1.top(); Q1.pop(); } else { p = Q.top(); Q.pop(); } if(vis[p->x][p->y]) continue; vis[p->x][p->y] = 1; for(int i = 0; i < 8; i++) { int vx = p->x + dir[i][0]; int vy = p->y + dir[i][1]; double w = i < 4 ? 1.0 : (double)sqrt(2.0); if(vx < 0 || vy < 0 || vx >= n || vy >= m || graph[vx][vy] == '#') continue; node* v; if(hashmap[pair<int, int> (vx, vy)] != NULL) { v = hashmap[pair<int, int> (vx, vy)]; } else { v = new node(vx, vy, 0x7fffffff, NULL); hashmap[pair<int, int> (vx, vy)] = v; } // 判断是否相遇，若相遇，则记录总距离最小的相遇点 // 相遇时并不将v放入队列 if(subj[p->x][p->y] == 0 && subj[v->x][v->y] == 1) { double c = i < 4 ? 1 : sqrt(2); if(maxDis > p->dis + v->dis + c + cost[tx][ty]) { maxDis = p->dis + v->dis + c + cost[tx][ty]; ltemp = p, rtemp = v; } continue; } else if(subj[p->x][p->y] == 1 && subj[v->x][v->y] == 0) { double c = i < 4 ? 1 : sqrt(2); if(maxDis > p->dis + v->dis + c + cost[tx][ty]) { maxDis = p->dis + v->dis + c + cost[tx][ty]; ltemp = v, rtemp = p; } continue; } if(v->dis > p->dis + w + cost[vx][vy]) { v->dis = p->dis + w + cost[vx][vy]; if(subj[p->x][p->y] == 0) { v->pre = p; subj[v->x][v->y] = 0; Q.push(v); } else { v->pre = p; subj[v->x][v->y] = 1; Q1.push(v); } } } } } int main() { memset(cost, 0, sizeof(cost)); cout << "Please input row of the map" << endl; cout << "空地用.表示 沙漠用y表示 溪流用r表示 障碍用#表示" << endl; cin >> n; for(int i = 0; i < n; i++) { cin >> graph[i]; for(int j = 0; j < graph[i].size(); j++) { if(graph[i][j] == 'y') cost[i][j] = 4; else if(graph[i][j] == 'r') cost[i][j] = 2; } } m = graph[0].size(); cout << "Please input start and end points" << endl; cin >> sx >> sy; cin >> tx >> ty; cost[sx][sy] = 0; node* s = new node(sx, sy, 0, NULL); node* t = new node(tx, ty, 0, NULL); hashmap[pair<int, int> (sx, sy)] = s; hashmap[pair<int, int> (tx, ty)] = t; Astar(s, t); if(ltemp != NULL) { cout << "DistCost: " << maxDis << endl; node *q = ltemp; while(q) { if(q == s) graph[q->x][q->y] = 'S'; else graph[q->x][q->y] = '&'; q = q->pre; } q = rtemp; while(q != t) { graph[q->x][q->y] = '%'; q = q->pre; } graph[q->x][q->y] = 'T'; for(int i = 0; i < n; i++) cout << graph[i] << endl; } return 0; }