Fixed bug in path finding algorithm.