1.预处理
地图栅格化,确定格子属性:可走+不可走,定义openlist列表存放待考察节点,closelist存放已考察的最优节点,定义起点与终点。
Init函数
定义起点为父节点A,G_cost=0,并存放于closelist中;
定义父节点的周围八个子节点,放入openlist
定义布尔型/01型二维数组,标记地图上可走与不可走点
2.寻路
f(n)=g(n)+h(n);//f(n)代价估计 g(n)实际代价 h(n)估计代价
计算父节点A的八个子节点的f(n),排序,从openlist中选择f(n)最小的节点,从openlist中删除,并放入closelist中,同时将此刻最小f点设置为新的父节点B;
忽略不可走点,忽略closelist中点,检查B点相邻八个子节点,若不在openlist,存入,若已经在openlist,则表明两个父节点相邻,计算此时的八个子节点的G(n)(这里只看实际代价,因为咱们实际代价都已经高了,还能是最优路径嘛),如果没有更小的G(n),无操作。
举个栗子:假定C点已在openlist即是父节点A与B的公共节点,此刻父节点是B点,A-B-C=20,A-C=14,既然在起点时,都没选A-C,为啥在B点,还要选A-B-C?
循环,直到出结果。
注:若还有节点没在open和close中,表明寻路未结束,
若终点在openlist,路径已找到
若openlist为空,路径不存在
从终点起,沿着每一格的父节点回到起点,就是路径