Enhanced pathfinding a bit