Improved path search alghoritm