Pathfinding improvements

added functions to follow multipoint path from function call