Made the nearGoal method eliminate weird problems using interpolation