small improvements to path finding algorithm