fix goal distance estimate in findpath