The overhaul of the path finding is complete! The new algorithm is very complex and was difficult to implement but it will allow you to control your characters more comfortably. I have developed the algorithm from different ideas. However your soldiers might not be able to reach very distant goals with lots of obstacles on their way, I have built in a limit on purpose to prevent the CPU from consuming too much time.