Алгоритм поиска минимального расстояния между объектами

public class WaspAndFly {
 
    public static int WASPCOUNT = 500;
    public static int FLYCOUNT = 500;
    static final long START = System.nanoTime();
    static final double NANO = 1000000000;
 
    public static void main(String[] args) {
        double[][] waspCoord = new double[WASPCOUNT][2];
        double[][] flyCoord = new double[FLYCOUNT][2];
        for (int i = 0; i < WASPCOUNT; i++) {
            waspCoord[i][0] = Math.random();
            waspCoord[i][1] = Math.random();
        }
        for (int i = 0; i < FLYCOUNT; i++) {
            flyCoord[i][0] = Math.random();
            flyCoord[i][1] = Math.random();
        }
        //формируем матрицу расстояний
        double[][] distanceSquare = new double[WASPCOUNT][FLYCOUNT];
        for (int i = 0; i < WASPCOUNT; i++) {
            for (int j = 0; j < FLYCOUNT; j++) {
                distanceSquare[i][j] = Math.pow(waspCoord[i][0] - flyCoord[j][0], 2) + Math.pow(waspCoord[i][1] - flyCoord[j][1], 2);
            }
        }
        Map<Integer, Integer> swapToFly = new HashMap();//сюда складываем найденные соответствия
        //формируем 2 списка с нераспределенными осами и мухами
        ArrayList<Integer> swapFree = IntStream.range(0, WASPCOUNT).collect(ArrayList::new, ArrayList::add, ArrayList::addAll);
        ArrayList<Integer> flyFree = IntStream.range(0, FLYCOUNT).collect(ArrayList::new, ArrayList::add, ArrayList::addAll);
        //основная часть алгоритма. Ищем пару с минимальным расстоянием
        while (!swapFree.isEmpty() && !flyFree.isEmpty()) {
            double minDistance = Double.MAX_VALUE;
            int flyMin = 0;
            int swapMin = 0;
            for (Integer i : swapFree) {
                for (Integer j : flyFree) {
                    if (distanceSquare[i][j] < minDistance) {
                        minDistance = distanceSquare[i][j];
                        flyMin = j;
                        swapMin = i;
                    }
                }
            }
            swapToFly.put(swapMin, flyMin); //добавляем найденное соответствие
            //исключаем пару из дальнейшего рассмотрения
            swapFree.remove(Integer.valueOf(swapMin)); 
            flyFree.remove(Integer.valueOf(flyMin));
        }
        //Выводим найденные соответствия (10 значений)
        swapToFly.entrySet().stream()
                .limit(10)
                .collect(Collectors.toMap(p -> p.getKey(), p -> p.getValue()))
                .forEach((k, v) -> System.out.println("Swap " + k + " to fly " + v));
        System.out.println("Время выполнения - " + (System.nanoTime() - START) / NANO + "с");
    }
}