ИМХО, что то Вы перемудрили.
Я бы и желтые точки опустил на Z=0, после чего расстояние между желтыми точками принял бы за единицу(длину линии), расстояние от начальной желтой точки до синей, как часть от единицы и эту часть от единицы умножил бы на разность высот желтых точек. Так получил бы приращение Z координаты для синих точек относительно начальной желтой точки, ну и потом вернул бы точки 3d полилинии на полученные отметки.
Это самый простой вариант, не учитывающих всяких петель и крутых загогулин, но можно учесть и их, для этого нужны не сами синие точки, а их перпендикулярные проекции на красную линию...