Fix weypoints coords and now use only few units fro short path compute