shorten the path planning algo.