Changed some parts of an algorithm; added the constant to distance of starting points and ending points