Вне головы мне кажется, что вы делаете ненужный квадратный корень при расчете расстояния. Вместо этого вы можете проверить квадрат расстояния и сравнить с квадратной константой.
Кроме того, должно быть сделано улучшение, когда, если вы найдете солдата, которого следует избегать, другой солдат также избежит первого. Поэтому каждую пару нужно проверять только один раз, если вы отслеживаете несколько списков избегания.
Это можно сделать, сохранив только те пары, которые находятся слишком близко. В псевдокоде
for (int i = 0; i < numSoldiers - 1; ++i) {
for (int j = i + 1; j < numSoldiers; ++j) {
if (Distance(soldier[i], soldier[j]) < 5) {
closePairs.Add(new Pair(i, j));
}
}
}
// Calculate directions based on the pairs found.
Это основные оптимизации. Если вам нужно пойти глубже, вам нужно будет отсортировать солдат по позициям. Если вы отслеживаете, какие солдаты находятся на каждом 5-м блоке, то ни один солдат, находящийся на расстоянии более 2-х клеток, не сможет повлиять на данного солдата.
Сортировка солдат по осям X и Y также даст аналогичные возможности, поскольку вы знаете, что если расстояние до следующего парня по одной оси больше радиуса, нет необходимости проверять расстояние.
Это даст вам лучшую производительность, но это зависит от ваших данных и от того, насколько обычно солдаты редки.