带转向成本的3x3网格最短路径寻路问题优化求助

1. 准确整合90度转向成本的核心思路 #

• 扩展Dijkstra的状态维度：传统Dijkstra仅记录(x,y)的最小成本，但转向成本依赖到达当前节点的移动方向，因此状态需改为(x, y, dir)，其中dir表示进入当前单元格的方向（如上下左右，可额外设初始状态表示起点无前置方向）。
• 转向成本的计算逻辑：当从状态(prev_x, prev_y, prev_dir)移动到(curr_x, curr_y, curr_dir)时，若prev_dir不是初始状态，且prev_dir与curr_dir垂直（即90度转向），则额外叠加当前单元格(curr_x, curr_y)的成本作为转向惩罚。
• 起点特殊处理：起点无前置方向，从起点出发到相邻单元格时，仅计算单元格成本，无转向成本。

2. 确保找到最优路径的实现改进 #

• 修正状态存储：为每个(x,y,dir)状态单独记录最小累计成本，而非仅记录(x,y)的最小成本。不同方向到达同一单元格时，后续产生的转向成本可能不同，不能直接覆盖原有记录。
• 调整优先级队列元素：队列元素需包含(累计总成本, x, y, 当前方向)，而非仅存坐标与总成本。
• 优化转向判断逻辑：无需通过父节点、当前节点、下一个节点追踪转向，直接对比prev_dir与curr_dir：
  • 定义方向常量（如UP=0, DOWN=1, LEFT=2, RIGHT=3）
  • 若两个方向为垂直关系（如UP与LEFT/RIGHT、DOWN与LEFT/RIGHT等），则判定为90度转向，添加对应转向成本。
• 障碍物处理：遍历相邻单元格时直接跳过障碍物，不加入优先级队列。
• 调整终止条件：到达终点时，需对比所有不同方向到达终点的状态，取总成本最小的路径，而非第一次到达就返回——不同方向到达终点的路径成本可能存在差异。

内容的提问来源于stack exchange，提问作者Mojtaba Heidari