It’s still far from ideal, there are a lot of edge cases that need to be handled. And the path reoccurs per navigation point pair and is then re-projected. I tried to add in some dampening functions and tried to reduce sample points on shorter distances.
