Moved some checks in FindPath()