Optimized A* (less garbage = less work for the GC), + avoid unwalkable diagonals