This works on the tests I tried so far. I will clean it up a bit. Then add a change that allows to disconsider the first part of the old path. So far it only creates 'middle partial paths'. I will then add a heuristics that stops the partial path search between collisions and experiment with that.