High performance pathfinding