algoritm for to find a path for units