Changed find_nearest_navigable, still need to modify astar
A* is fixed! Needs tested!
Merge branch 'master' into astar-fix