Преобразование ломаной линии

Я имею неравномерную сетку в виде координат узлов в двумерном пространстве Узлы сетки хранятся в одномерном векторе, где нумерация снизу-вверх слева-направо

Также мне дана ломаная монотонная линия (обозначена синим цветом на рисунке), из которой необходимо получить ломаную линию, проходящую через узлы сетки (обозначено красной линией на рисунке).
Количество точек ломаной линии не совпадает с количеством точек результирующей ломаной.
Есть ли у кого-нибудь идеи по решению данной задачи?

Ответ

0) Учащаем точки ломаной: в цикле определяем расстояние между соседними точками (D) у ломаной, если это расстояние больше, чем расстояние диагонали ячейки сетки (d), то вставляем дополнительною точку.

Это шаг предобработки ломаной. В конце объясню зачем это нужно
1) Определяем ближайший узел (curr_node) к начальной точке (p) исходной ломаной
Далее в цикле:
2) Определяем соседние узлы для curr_node (соседями не считать предыдущий посещенный узел)

3) Определяем расстояние от curr_node и от его соседних узлов до следующей точки ломаной (p_next).

4) Если расстояние от curr_node до p_next меньше, чем минимальное расстояние от его соседей до p_next, то p_next станет следующей точкой ломаной, иначе "посещаю" следующий узел сетки (тот соседний узел, который ближе всего к точке p_next)
Далее возвращаемся к шагу 2
В итоге получаем такую картину посещения узлов

т.е. собирая посещенные узлы (черные), получаем такую ломаную, проходящую по узлам сетки:

А теперь покажу зачем нужно было делать нулевой шаг:
Как видно из этих двух рисунков, первый вариант неверный. Вставкой дополнительных точек, добьемся правильного результата