C++ направленная сортировка ближайшего соседа

Question

Итак, в моем коде я получаю координаты xyz точек, которые должны образовывать контур. Проблема в том, что эти точки не отсортированы правильно. Когда я получаю координаты, они сортируются по возрастанию значений x и y. Поэтому сначала значения x сортируются, и если две точки имеют одинаковое значение x, они сортируются по их значению y. Значение z всегда одно и то же, поэтому его можно игнорировать. Чтобы отсортировать точки для формирования контура, я использую вариацию алгоритмов ближайшего соседа. Так вот мой код для сортировки:

double squareDistancePoints(const std::array<double, 3>& a, const std::array<double, 3>& b) 
{ 
    assert(a.size() == b.size()); 
    double sum = 0; 
    for(size_t i = 0; i < a.size(); ++i) 
     sum += pow(b[i]-a[i], 2); 
    return sum; 
} 


for(auto it = matrix.begin(); it != matrix.end(); ++it) 
{ 
    auto bestIt = matrix.end(); 

    double bestSquareDistance = DBL_MAX; 

    for(auto nextIt = it + 1; nextIt != matrix.end(); ++nextIt) 
    { 
     const auto squareDistance = squareDistancePoints(*it, *nextIt); 
     if(squareDistance < bestSquareDistance) 
     { 
      bestSquareDistance = squareDistance; 
      bestIt = nextIt; 
     } 
    } 
    if(bestIt != matrix.end()) 
    { 
     std::swap(*(it + 1), *bestIt); 
    } 

} 

Так что это прекрасно работает для стандартных контуров, как куб или круг. Но у меня также есть контуры, где это не сработает. Так вот картина несортированным контура

Когда я использую сортировочный код я предоставил я получаю этот результат:

Я уже нашел проблему в идее моего кода. Контур значительно длиннее ширины, поэтому алгоритм сортировки также всегда является ближайшей точкой в ​​неправильном месте. Так правильная картина будет выглядеть следующим образом:

Мой подход здесь должен был начаться с одной точки, поиск ближайшей точки и следовать в направлении точки дали мне. Я сделал это все вручную. Итак, теперь моя проблема заключается в том, что я не знаю, как оптимизировать свой код, чтобы он следил за направлением при сортировке. Надеюсь, кто-то может мне помочь. Я также мог бы отправить CSV-файл с несортированными и отсортированными точками, если бы это помогло.

Answer 1

Вы получаете доступ к неправильной области данных здесь, когда она находится в последней итерации: (it + 1)==matrix.end() в этом случае => неопределенное поведение.

for(auto it = matrix.begin(); it != matrix.end(); ++it) 
{ 
    ... 
    if(bestIt != matrix.end()) 
    { 
     std::swap(*(it + 1), *bestIt); 
    }