Corrected path finding algorithm to give the correct endpoint.