小船过河 宽度搜索 python实现

商人数和仆人数必须相等 且大于三个 程序无限跑
有时间改进

class SQueue():
    def __init__(self,init_len=8):
        self.len = init_len
        self.head = 0
        self.elems = [0]*init_len
        self.num = 0

    def is_empty(self):
        return self.num == 0

    def peek(self):
        if self.num == 0:
            raise QueueUnderflow
        return self.elems[self.head]

    def dequeue(self):
        if self.num == 0:
            raise QueueUnderflow
        e = self.elems[self.head]
        self.head = (self.head+1) % self.len
        self.num -= 1
        return e

    def enqueue(self,e):
        if self.num == self.len:
            self.__extend()
        self.elems[(self.head+self.num)%self.len] = e
        self.num += 1

    def __extend(self):
        old_len = self.len
        self.len *= 2
        newelems = [0] * self.len
        for i in range(old_len):
            newelems[i] = self.elems[(self.head+i)%old_len]
        self.elems ,self.head = newelems , 0

class side:
    def __init__(self,businessman=0,servant=0):
                self.businessman = businessman
                self.servant = servant

    def decision_table(self):
        lst = [[0 for i in range(self.servant+5)] for i in range(self.businessman+5)]
        for i in range(2,self.businessman+3):
            for j in range(2,self.servant+3):
                if i == 2 or i == self.businessman+2:
                    lst[i][j] = 1
                if i >= j and self.businessman-i== self.servant-j:
                    lst[i][j] = 1
        return lst

    def path(self,lst,start,end=(2,2)):
        dirs1 = [(-1,0),(-2,0),(0,-1),(0,-2),(-1,-1)] #下左
        dirs2 = [(0,1),(0,2),(1,0),(2,0),(1,1)] #上右
        def passable(lst,pos):
            return lst[pos[0][0]][pos[0][1]] == 1

        if start[0] == end:
            print('path finds.')
            return
        qu = SQueue()
        qu.enqueue(start)
        while not qu.is_empty():
            pos = qu.dequeue()
            for i in range(5):
                if pos[1]%2 == 1:
                    nextp = [(pos[0][0]+dirs1[i][0],pos[0][1]+dirs1[i][1]),pos[1]+1]
                elif pos[1]%2 == 0:
                    nextp = [(pos[0][0] + dirs2[i][0], pos[0][1] + dirs2[i][1]),pos[1]+1]
                    #print(nextp)
                if passable(lst,nextp):
                    #print('nextp',nextp[0],nextp[1])
                    if nextp[0] == end:
                        nextp[1] -= 1
                        print('path find.')
                        print('小船移动最短次数为{}次'.format(nextp[1]))
                        return

                    qu.enqueue(nextp)
        print('No path.')




def main():
    x = int(input('输入商人数'))
    y = int(input('输入仆人数'))
    a = side(x,y)
    lst = a.decision_table()
    print(lst)
    a.path(lst,[(x+2,y+2),1])

if __name__ == '__main__':
    main()