Change std::for_each to while.
Move the point according to the effective polygon before dsm.
Finish the logic before shortpath.