Optimized pathfinding some more