Sped up searching in larger (and smaller) grids

I changed the logic from running astar.init at the top of every search, to simply tracking the nodes I've hit and cleaning them up before returning the path. I was getting reamed on grids like 500x500 and I tracked down the performance hit down to the init. The unit tests all succeed, and I added a new 500x500 benchmark that went from 6ms per to 1.5ms per search.