Элегантный способ найти следующий пиксель в строке

Я следую за одной пиксельной линией, которая иногда пересекается с другими линиями, и я пытаюсь найти элегантный способ остаться на той же самой линии.

Текущий пиксель 0 (Синий). предыдущий пиксель -1 (Черный).

На первом изображении есть два возможных следующих пикселя (зеленый и красный), но зеленый пиксель (1) должен быть выбран, потому что он продолжает линию.

На втором изображении два зеленых пикселя (1) с которым я был бы одинаково счастлив — я не против оставить выбор между ними неопределенным, если только он не красный.

В псевдо-cpp:

vector<Point> points;

for (i = x - 1; i < x + 2; i++) {
for (j = y - 1; j < y + 2; j++) {
if (i == x && j == y) {
continue;
}

if (IS_ON(i, j) && NOT_VISITED(i, j)) {
points.push_back(Point(i, j));
}
}
}

// sort points to find closest to opposite lastX,lastY

lastX = x;
lastY = y;

x = points[0].x;
y = points[0].y;

Шаг сортировки — это бит, с которым я борюсь.

Я думал об использовании std :: sort, но мне трудно сформулировать функцию сравнения.

Как будет выглядеть соответствующая функция сравнения или есть более элегантный подход?