算法原理

Dijkstra算法采用贪心算法的思想，解决的问题可以描述为：在无向图G=(V,E)中，假设每条边E[i] 的长度为 w[i]，找到由顶点vs到其余各点的最短路径。 通过Dijkstra计算图G中的最短路径时，需要指定起点vs(即从顶点vs开始计算)。此外，引进两个集合S和U。S的作用是记录已求出最短路径的顶点（以及相应的最短路径长度），而U则是记录还未求出最短路径的顶点（以及该顶点到起点vs的距离）。初始时，S中只有起点vs；U中是除vs之外的顶点，并且U中顶点的路径是“起点vs到该顶点的路径”。然后，从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。然后，再从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。重复该操作，直到遍历完所有顶点。 时间复杂度为O(V^2)其中V为顶点数，但可以通过优先队列实现最小堆来优化时间复杂度。

所谓贪心是指每一点都存储该点到起点的最短路径。

算法实现

C++算法核心：

std::tuple<vector<pair<int, int>>, vector<double>> Dijkstra::get_neighbors(pair<int, int> node) {vector<pair<int, int>> neighbors;vector<double> distances;vector<PlanNode> nexts = {PlanNode({-1, 0}, 1), PlanNode({-1, 1}, sqrt(2)), PlanNode({0, 1}, 1), PlanNode({1, 1}, sqrt(2)),PlanNode({1, 0}, 1), PlanNode({1, -1}, sqrt(2)), PlanNode({0, -1}, 1), PlanNode({-1, -1}, sqrt(2))};for (auto& next : nexts) {pair<int, int> neighbor = {node.first + next.directions.first, node.second + next.directions.second};if (0 <= neighbor.first && neighbor.first < grid.size() && 0 <= neighbor.second && neighbor.second < grid[0].size()) {if (grid[neighbor.first][neighbor.second] == 0) {if (next.directions.first != 0 && next.directions.second != 0) {if (grid[node.first + next.directions.first][node.second] == 0 && grid[node.first][node.second + next.directions.second] == 0) {neighbors.push_back(neighbor);distances.push_back(next.cost);}} else {neighbors.push_back(neighbor);distances.push_back(next.cost);}}}}return make_tuple(neighbors,distances);
}double Dijkstra::plan() {priority_queue<pair<double, pair<int, int>>, vector<pair<double, pair<int, int>>>, greater<>> priority_queue;priority_queue.push({0, start});map<pair<int, int>, double> costs;map<pair<int, int>, pair<int, int>> previous_nodes;costs[start] = 0;while (!priority_queue.empty()) {auto [current_cost, current_node] = priority_queue.top();priority_queue.pop();if (visited.find(current_node) != visited.end()) continue;visited.insert(current_node);visit_order.push_back(current_node);if (current_node == goal) break;vector<pair<int, int>> neighbors;vector<double> distances;tie(neighbors,distances) = get_neighbors(current_node);for (size_t i=0; i<neighbors.size();++i) {pair<int, int> neighbor = neighbors[i];double distance = distances[i];double cost = current_cost + distance;// double cost = current_cost + sqrt(pow(neighbor.first - current_node.first, 2) + pow(neighbor.second - current_node.second, 2));if (costs.find(neighbor) == costs.end() || cost < costs[neighbor]) {costs[neighbor] = cost;previous_nodes[neighbor] = current_node;priority_queue.push({cost, neighbor});}}}path.clear();pair<int, int> current_node = goal;while (current_node != start) {path.push_back(current_node);current_node = previous_nodes[current_node];std::cout<<"node: "<<current_node.first<<","<<current_node.second<<" ";printf("cost:%lf\n",costs[current_node]);}path.push_back(start);reverse(path.begin(), path.end());return costs[goal];
}

 

Python算法核心：

class Node:def __init__(self,directions,cost):self.directions = directionsself.cost = costdef get_neighbors(self, node):neighbors = []distances = []nexts = [self.Node((-1, 0),1), self.Node((0, 1),1), self.Node((0, -1),1), self.Node((1,0),1),self.Node((-1,1),math.sqrt(2)), self.Node((1,1),math.sqrt(2)),self.Node((1, -1),math.sqrt(2)), self.Node((-1,-1),math.sqrt(2))]for next in nexts:neighbor = (node[0] + next.directions[0], node[1] + next.directions[1])if self.board_size <= neighbor[0] < len(self.grid)-self.board_size and self.board_size <= neighbor[1] < len(self.grid[0])-self.board_size:if self.grid[neighbor[0]][neighbor[1]] == 0:if next.directions[0] != 0 and next.directions[1] != 0:  # 对角线方向if self.grid[node[0] + next.directions[0]][node[1]] == 0 and self.grid[node[0]][node[1] + next.directions[1]] == 0:neighbors.append(neighbor)distances.append(next.cost)else:neighbors.append(neighbor)distances.append(next.cost)return neighbors,distancesdef plan(self):priority_queue = []heapq.heappush(priority_queue,(0,self.start))costs = {self.start: 0}previous_nodes = {self.start: None}self.visited = set()self.visit_order = []while priority_queue:current_cost, current_node = heapq.heappop(priority_queue)# Determines whether the current node has already been visitedif current_node in self.visited:continueself.visited.add(current_node)self.visit_order.append(current_node)if current_node == self.goal:break# Find passable neighborsneighbors, distances = self.get_neighbors(current_node)for neighbor,distance in zip(neighbors,distances):if neighbor[0] == -1:continue# Compute the cost from the start pointcost = current_cost + distance# Store cost and update to minimum valueif neighbor not in costs or cost < costs[neighbor]:costs[neighbor] = cost# The parent node of neighbor is current_nodeprevious_nodes[neighbor] = current_node# Push the node into priority_queueheapq.heappush(priority_queue,(cost, neighbor))self.path = []current_node = self.goalwhile current_node is not None:self.path.append(current_node)current_node = previous_nodes.get(current_node)self.path = self.path[::-1]return costs[self.goal]