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. 
I think we should just go ahead and make this a normal for loop (I don't think caching the length value or checking `< len` vs `<= len` makes any real performance difference)