Fixed pathfind algorithm.