Modified FindSupplyPath with queue sorting.