A寻路算法

关键点实现，备忘。