Added find_path_update

it will take a current path estimate and reestimate the best path with a
new goal or beginning. The rationate is that if goal and beginning don't
change too much, then the old estimate will be really close to the new
one, so the path finding will be a lot quicker.