Move pathTo function outside astar.search

This small optimization avoid to redefine the pathTo function in memory
each time astar.search() is called.