别再死记硬背了！用Python和C++手写Dijkstra算法，搞懂路径规划核心（附完整代码）

从零实现Dijkstra算法Python与C双语言实战路径规划很多同学在刷算法题时都有这样的困惑看讲解视频时觉得思路清晰但自己动手写代码却无从下手。今天我们就用最直观的方式带你用Python和C两种语言完整实现Dijkstra算法彻底掌握路径规划的核心实现技巧。1. 为什么选择Dijkstra算法作为路径规划的起点路径规划算法种类繁多从A到RRT各有特点。但Dijkstra算法作为最基础的最短路径算法它的价值在于算法思想纯粹不依赖任何启发式函数纯粹基于贪心策略教学意义重大理解它能轻松过渡到A*等更复杂算法实际应用广泛从网络路由到游戏AI都有它的身影我第一次接触这个算法时最困惑的是如何将课本上的伪代码转化为实际可运行的代码。下面我们就从最基础的图表示方法开始一步步拆解实现细节。2. 图的表示邻接矩阵与邻接表的选择实现Dijkstra算法的第一步是如何在代码中表示图结构。常见的有两种方式2.1 邻接矩阵实现# Python邻接矩阵示例 INF float(inf) graph [ [0, 7, 9, INF, INF], [7, 0, 10, 15, INF], [9, 10, 0, 11, INF], [INF, 15, 11, 0, 6], [INF, INF, INF, 6, 0] ]对应的C实现// C邻接矩阵示例 const int INF INT_MAX; vectorvectorint graph { {0, 7, 9, INF, INF}, {7, 0, 10, 15, INF}, {9, 10, 0, 11, INF}, {INF, 15, 11, 0, 6}, {INF, INF, INF, 6, 0} };适用场景稠密图边数接近顶点数的平方时更节省空间2.2 邻接表实现# Python邻接表示例 from collections import defaultdict graph defaultdict(list) graph[0] [(1, 7), (2, 9)] graph[1] [(0, 7), (2, 10), (3, 15)] graph[2] [(0, 9), (1, 10), (3, 11)] graph[3] [(1, 15), (2, 11), (4, 6)] graph[4] [(3, 6)]对应的C实现// C邻接表示例 vectorvectorpairint, int graph { {{1, 7}, {2, 9}}, {{0, 7}, {2, 10}, {3, 15}}, {{0, 9}, {1, 10}, {3, 11}}, {{1, 15}, {2, 11}, {4, 6}}, {{3, 6}} };适用场景稀疏图边数远小于顶点数的平方时更高效实际项目中邻接表的使用频率更高因为它能更好地适应各种图结构。但在算法竞赛中邻接矩阵因其编码简单也常被采用。3. Dijkstra核心实现优先队列的妙用理解了图的表示方法后我们来看算法核心实现。关键在于如何高效地找到当前距离起点最近的节点。3.1 Python实现详解import heapq def dijkstra(graph, start): n len(graph) dist [float(inf)] * n dist[start] 0 heap [(0, start)] visited set() while heap: current_dist, u heapq.heappop(heap) if u in visited: continue visited.add(u) for v, weight in graph[u]: if dist[v] current_dist weight: dist[v] current_dist weight heapq.heappush(heap, (dist[v], v)) return dist关键点解析使用优先队列最小堆快速获取当前最小距离节点visited集合避免重复处理松弛操作Relaxation更新邻居节点距离3.2 C实现对比#include vector #include queue #include climits using namespace std; vectorint dijkstra(const vectorvectorpairint, int graph, int start) { int n graph.size(); vectorint dist(n, INT_MAX); dist[start] 0; priority_queuepairint, int, vectorpairint, int, greaterpairint, int pq; pq.push({0, start}); while (!pq.empty()) { auto [current_dist, u] pq.top(); pq.pop(); if (current_dist dist[u]) continue; for (const auto [v, weight] : graph[u]) { if (dist[v] dist[u] weight) { dist[v] dist[u] weight; pq.push({dist[v], v}); } } } return dist; }语言差异注意点C需要显式指定优先队列的比较函数C17结构化绑定简化了pair的访问需要包含额外头文件4. 路径回溯记录完整路径而不仅是距离实际应用中我们不仅需要知道最短距离还需要知道具体路径。下面我们扩展实现来记录路径。4.1 Python路径回溯实现def dijkstra_with_path(graph, start): n len(graph) dist [float(inf)] * n dist[start] 0 prev [-1] * n # 记录前驱节点 heap [(0, start)] while heap: current_dist, u heapq.heappop(heap) if current_dist dist[u]: continue for v, weight in graph[u]: if dist[v] dist[u] weight: dist[v] dist[u] weight prev[v] u heapq.heappush(heap, (dist[v], v)) # 构建路径 paths [] for i in range(n): path [] current i while current ! -1: path.append(current) current prev[current] paths.append(path[::-1]) return dist, paths4.2 C路径回溯实现pairvectorint, vectorvectorint dijkstraWithPath(const vectorvectorpairint, int graph, int start) { int n graph.size(); vectorint dist(n, INT_MAX); vectorint prev(n, -1); dist[start] 0; priority_queuepairint, int, vectorpairint, int, greaterpairint, int pq; pq.push({0, start}); while (!pq.empty()) { auto [current_dist, u] pq.top(); pq.pop(); if (current_dist dist[u]) continue; for (const auto [v, weight] : graph[u]) { if (dist[v] dist[u] weight) { dist[v] dist[u] weight; prev[v] u; pq.push({dist[v], v}); } } } // 构建路径 vectorvectorint paths(n); for (int i 0; i n; i) { int current i; while (current ! -1) { paths[i].push_back(current); current prev[current]; } reverse(paths[i].begin(), paths[i].end()); } return {dist, paths}; }5. 性能优化与常见问题排查实现基本功能后我们还需要关注一些优化技巧和常见陷阱。5.1 时间复杂度对比实现方式时间复杂度适用场景邻接矩阵普通队列O(V²)小规模图邻接表优先队列O(E VlogV)稀疏图斐波那契堆O(E VlogV)理论最优但实现复杂5.2 常见错误排查负权边问题Dijkstra不能处理负权边遇到负权边应考虑Bellman-Ford算法堆优化陷阱# 错误示例直接修改堆中元素 heapq.heappush(heap, (new_dist, v)) # 正确做法 # 错误试图直接修改堆中已有元素的优先级初始化问题// 必须初始化所有距离为INF vectorint dist(n, INT_MAX); dist[start] 0; // 起点设为0大数溢出INF float(inf) # Python可用 // C中建议使用INT_MAX/2防止加法溢出 const int INF 0x3f3f3f3f; // 约1e9常用技巧5.3 实际应用中的优化技巧双向Dijkstra同时从起点和终点开始搜索相遇时停止A*算法加入启发式函数适用于已知目标位置的场景预处理技术如Contraction Hierarchies加速多次查询# 双向Dijkstra示例框架 def bidirectional_dijkstra(graph, start, end): # 初始化前向和后向搜索 forward_dist {start: 0} backward_dist {end: 0} forward_heap [(0, start)] backward_heap [(0, end)] best_dist float(inf) meeting_node None while forward_heap and backward_heap: # 前向搜索一步 if forward_heap: current_dist, u heapq.heappop(forward_heap) if u in backward_dist: total current_dist backward_dist[u] if total best_dist: best_dist total meeting_node u # 后向搜索一步 if backward_heap: current_dist, u heapq.heappop(backward_heap) if u in forward_dist: total current_dist forward_dist[u] if total best_dist: best_dist total meeting_node u # 合并路径 if meeting_node is not None: # 合并前向和后向路径 return best_dist return float(inf)6. 实战测试从算法题到实际应用为了验证我们的实现让我们用几个经典案例进行测试。6.1 LeetCode案例测试743. 网络延迟时间标准的Dijkstra应用场景def networkDelayTime(times, n, k): graph defaultdict(list) for u, v, w in times: graph[u-1].append((v-1, w)) dist dijkstra(graph, k-1) max_dist max(dist) return max_dist if max_dist float(inf) else -16.2 实际应用场景假设我们要为一个物流系统设计路径规划模块struct DeliveryPoint { int id; double latitude; double longitude; }; vectorint findOptimalRoute(const vectorDeliveryPoint points, const vectortupleint, int, int roads, int depotIndex) { // 构建图 vectorvectorpairint, int graph(points.size()); for (const auto [u, v, w] : roads) { graph[u].emplace_back(v, w); graph[v].emplace_back(u, w); // 无向图 } // 计算最短路径 auto [dist, paths] dijkstraWithPath(graph, depotIndex); // 找出最优配送顺序简化版 vectorint deliveryOrder(points.size()); iota(deliveryOrder.begin(), deliveryOrder.end(), 0); sort(deliveryOrder.begin(), deliveryOrder.end(), [dist](int a, int b) { return dist[a] dist[b]; }); return deliveryOrder; }7. 扩展思考Dijkstra的变种与应用掌握了基础实现后我们可以进一步探索一些有趣的变种多目标Dijkstra同时计算到多个目标点的最短路径时间依赖Dijkstra考虑边权重随时间变化的情况概率Dijkstra考虑边存在概率的情况# 多目标Dijkstra示例 def multi_target_dijkstra(graph, start, targets): n len(graph) dist [float(inf)] * n dist[start] 0 heap [(0, start)] remaining_targets set(targets) results {} while heap and remaining_targets: current_dist, u heapq.heappop(heap) if u in remaining_targets: results[u] current_dist remaining_targets.remove(u) for v, weight in graph[u]: if dist[v] current_dist weight: dist[v] current_dist weight heapq.heappush(heap, (dist[v], v)) return results在自动驾驶项目中我们曾遇到一个有趣的问题如何在动态变化的交通环境中实时更新最短路径。传统的Dijkstra每次都要重新计算效率太低。最终我们采用了增量式更新的方案只在道路权重变化时重新计算受影响的部分路径。