Freeze bug fix

Fixed bug where path searching would freeze if begin and end are
separated by a wall.