Improved path finding (more optimal)