数据结构-图-A*算法

A*算法概述

算法背景：
Dijkstra算法类似BFS算法，每次找到跟起点最近的顶点然后往外扩展。这种往外扩渣的思路有点盲目，可能会导致路线搜索方向跑偏。没有考虑顶点到终点的距离。比如如下从S点到T点，Dijkstra算法会先遍历1，2，3这些顶点。

算法核心：估算顶点到终点的距离，增加启发函数/估价函数。

算法对Dijkstra改造之处：
1、顶点Vertex类定义多了x，y坐标，以及f（i）估价函数
2、优先级队列构造方式不同，A算法根据f值f=g+h来构建优先级队列，其中g是起始点与顶点的距离，h是顶点到结束点的距离。D算法是根据g值来构建。
3、A算法在更新顶点dist值的时候，也会同步更新f值
4、循环结束条件不一样，D算法是在终点出队列的时候才结束，A*算法是一旦遍历到终点就结束，所以未必是最短路径

A*算法代码实现

#include<iostream>
#include<vector>
#include<limits.h>
#include<cmath>
using namespace std;
class Edge {
public:
    int sid; // 边的起始顶点编号
    int tid; //边的终止顶点编号
    int w;   //权重
    Edge(int sid, int tid, int w) {
        this->sid = sid;
        this->tid = tid;
        this->w = w;
    }
};

class Vertex {
public:
    int id;   //顶点编号
    int dist; //从起始顶点到这个顶点的距离
    int x; // 比astar算法多x,y坐标
    int y; // 比astar算法多x,y坐标
    int f; // f = g + h; g = dist; h = |x1 - x2| + |y1 - y2|
    Vertex (int id, int dist, int x, int y) {
        this->id = id;
        this->dist = dist;
    }
    Vertex () {
        this->id = 0;
        this->dist = INT_MAX;
        this->f = INT_MAX;
        this->x = 0;
        this->y = 0;
    }
};

class Graph {
private:
    vector<Edge *> * adj; //邻接表
    int v; //顶点个数
public:
    Graph(int v) {
        this->v = v;
        this->adj = new vector<Edge *>[v];
    }
    ~Graph() {
        if (this->adj != NULL) {
            for (int i = 0; i < v; ++i) {
                for (int j = 0; j < adj[i].size(); ++j) {
                    delete adj[i][j];
                }
            }
            delete [] adj;
        }
    }
    void AddEdge(int s, int t, int w) {
        Edge * e = new Edge(s, t, w);
        adj[s].push_back(e);
    }
    void astar(int s, int t);
    void Print(int s, int t, int * predecessor);
    int hManhattan(Vertex s, Vertex t);
};

class PriorityQueue {
private:
    Vertex * nodes; //顶点信息
    int size;       //堆可以存储的最大数据个数
    int count;      //堆中已经存储的数据个数
public:
    PriorityQueue(int v) {
        nodes = new Vertex[v+1];
        size = v + 1;
        count = 0;
    }
    ~PriorityQueue() {
        if (nodes != NULL) {
            delete [] nodes;
        }
        count = 0;
    }

    void Add(Vertex* ver);
    void Update(Vertex* ver);
    bool IsEmpty();
    void Print();
    Vertex Poll();
    void Adjust(int index);
    void Clear();
};
void PriorityQueue::Clear(){
    if (nodes != NULL) {
        delete [] nodes;
    }
    count = 0;
}

Vertex PriorityQueue::Poll() {
    Vertex top(nodes[1].id, nodes[1].dist, nodes[1].x, nodes[1].y);
    nodes[1] = nodes[count];
    count--;
    int index = 1;
    while(true) {
        int maxpos = index;
        if (2 * index < count && nodes[2 * index].f < nodes[maxpos].f) {
            maxpos = 2 * index;
        }
        if (2 * index + 1 < count && nodes[2 * index + 1].f < nodes[maxpos].f) {
            maxpos = 2 * index + 1;
        }
        if (maxpos == index) {
            break;
        }
        int tmp_f = nodes[index].f;
        int tmp_id = nodes[index].id;
        int tmp_dist = nodes[index].dist;
        nodes[index].f = nodes[maxpos].f;
        nodes[index].dist = nodes[maxpos].dist;
        nodes[index].id = nodes[maxpos].id;
        nodes[maxpos].f = tmp_f;
        nodes[maxpos].dist = tmp_dist;
        nodes[maxpos].id = tmp_id;
    }
    return top;
}

bool PriorityQueue::IsEmpty() {
    return count == 0;
}

void PriorityQueue::Add(Vertex* ver) {
   if(count < size) {
       count++;
       nodes[count].dist = ver->dist;
       nodes[count].f = ver->f;
       nodes[count].id = ver->id;
       //自下而上进行堆化
       Adjust(count);
   } else {
       cout << "error! size is full!" << endl;
   }
}

void PriorityQueue::Adjust(int index) {
    int i = index;
    while (i / 2 > 0 && nodes[i / 2].f > nodes[i].f) {
        int tmp_f  = nodes[i / 2].f;
        int tmp_id = nodes[i / 2].id;
        nodes[i / 2].f = nodes[i].f;
        nodes[i / 2].id = nodes[i].id;
        nodes[i].f = tmp_f;
        nodes[i].id = tmp_id;
    }
}

void PriorityQueue::Update(Vertex* ver) {
   bool isupdate = false;
   for (int i = 1; i <= count; ++i) {
      if (nodes[i].id == ver->id) {
          nodes[i].f = ver->f;
          isupdate = true;
          Adjust(i);
          break;
      }
   }
   if (isupdate == false) {
       cout << "error! not find id:" << ver->id << endl;
   }
}

void PriorityQueue::Print() {
   for (int i = 1; i <= count; ++i) {
       cout << nodes[i].f << " ";
   }
   cout << endl;
}

int Graph::hManhattan(Vertex s, Vertex t) {
    return abs(s.x- t.x) + abs(s.y - t.y);
}

void Graph::astar(int s, int t) {
    // predecessor 存放前驱节点，用来还原最短路径
    int * predecessor = new int[v];
    Vertex* vers = new Vertex[v];
    // inqueue 标记是否进入过队列
    bool * inqueue = new bool[v];
    for (int i = 0; i < v; ++i) {
        vers[i].id = i;
        inqueue[i] = false;
        vers[i].x = i;
        vers[i].y = i;
    }
    //小顶堆
    PriorityQueue* q = new PriorityQueue(v);
    q->Print();
    vers[s].dist = 0;
    vers[s].f = 0;
    inqueue[s] = true;
    q->Add(&vers[s]);
    q->Print();
    while (!q->IsEmpty()) {
        Vertex minV = q->Poll();  //取出堆顶元算并删除
        if (minV.id == t) break; // 这就是最短路径
        for (int i = 0; i < adj[minV.id].size(); ++i) {
            Edge * e = adj[minV.id][i];
            Vertex nextV = vers[e->tid];
            cout << "minV.id: " << minV.id  << "nextV.id:  " << nextV.id << " begin" << endl;
            if (minV.dist + e->w < nextV.dist) {
                nextV.dist = minV.dist + e->w;
                nextV.f = nextV.dist + hManhattan(nextV, vers[t]);
                predecessor[nextV.id] = minV.id;
                if (inqueue[nextV.id] == true) {
                    q->Update(&nextV);
                    q->Print();
                } else {
                    q->Add(&nextV);
                    q->Print();
                    inqueue[nextV.id] = true;
                    cout << "update inqueu nextV.id : " << nextV.id << endl;
                }
            }
            cout << "minV.id: " << minV.id  << "nextV.id:  " << nextV.id << " end" << endl;
            if (nextV.id == t) { // 只要到达t就结束while
                q->Clear(); // 清空queue，才能退出while循环
                break;
            }
        }
    }
    cout << s;
    Print(s, t, predecessor);
    cout << endl;
}

void Graph::Print(int s, int t, int * predecessor) {
    if (s == t) return;
    Print(s, predecessor[t], predecessor);
    cout << "->" << t;
}

int main() {
   Graph g(6);
   g.AddEdge(0, 1, 10);
   g.AddEdge(0, 4, 15);
   g.AddEdge(1, 2, 15);
   g.AddEdge(1, 3, 2);
   g.AddEdge(2, 5, 5);
   g.AddEdge(3, 2, 1);
   g.AddEdge(3, 5, 12);
   g.AddEdge(4, 5, 10);
   g.astar(0, 5);
}

A*算法应用

可以应用于游戏寻路，游戏中的角色从一个地方到另外一个地方，鼠标点击后角色就走过去，其背后的路径选择可以用A星算法。这里把地图抽象成图，把整个地图分割成一个一个的小方块，在某个方块上的人物只能往上下左右四个方向的方块上移动。每个方块看作为一个顶点。两个方块相邻，则连两条有向边，边权重为1.这样就转化为从一个顶点到另一个顶点的路径问题，进而可以使用A*算法求解。