notebook.community

Edit and run



In [157]:

    
# -*- coding: utf-8 -*-



In [158]:

    
%matplotlib inline



In [159]:

    
import numpy as np
import matplotlib.pyplot as plt
import math

from move_missionary import move_missionary
from Katokadai import Katokadai



In [160]:

    
mm = move_missionary()



In [161]:

    
%time mm.breadth_first_search()









    



[[3, 3, 0], [3, 2, 0], [3, 1, 0], [2, 2, 0]]
[[2, 2, 1]]
CPU times: user 207 µs, sys: 99 µs, total: 306 µs
Wall time: 285 µs






    Out[161]:





([0, 2, 0], 2, 23)



In [162]:

    
%time mm.A_star()









    



[3, 3, 0]
[3, 2, 0]
[3, 1, 0]
[2, 3, 0]
Failed
[2, 2, 0]
[1, 3, 0]
Failed
[3, 3, 0]
[3, 2, 0]
[3, 1, 0]
[2, 3, 0]
Failed
[2, 2, 0]
[1, 3, 0]
Failed
[3, 2, 0]
[3, 1, 0]
[3, 0, 0]
[2, 2, 0]
[2, 1, 0]
[1, 2, 0]
Failed
[3, 3, 0]
[3, 2, 0]
[3, 1, 0]
[2, 3, 0]
Failed
[2, 2, 0]
[1, 3, 0]
Failed
[3, 1, 0]
[3, 0, 0]
[2, 1, 0]
[2, 0, 0]
[1, 1, 0]
[3, 2, 0]
[3, 1, 0]
[3, 0, 0]
[2, 2, 0]
[2, 1, 0]
[1, 2, 0]
Failed
[3, 3, 0]
[3, 2, 0]
[3, 1, 0]
[2, 3, 0]
Failed
[2, 2, 0]
[1, 3, 0]
Failed
[2, 2, 0]
[2, 1, 0]
[2, 0, 0]
[1, 2, 0]
Failed
[1, 1, 0]
[0, 2, 0]
CPU times: user 1.76 ms, sys: 748 µs, total: 2.51 ms
Wall time: 2.01 ms






    Out[162]:





([0, 2, 0], 0, 117)

深さ優先探索が189 µs、A*探索が1.65 msで大幅に速くなる。



In [163]:

    
cell = np.loadtxt( "cell.csv", delimiter=","  )
plt.pcolormesh(cell)









    Out[163]:





<matplotlib.collections.QuadMesh at 0x10d02f250>



In [164]:

    
cells = []
for i in range(len(cell)):
    cells.append([])
    
for i in range(len(cell)):
    for j in range(len(cell[i])):
        cells[-i].append(cell[i-1][j])
cells1 = np.array(cells)
plt.pcolormesh(cells1)









    Out[164]:





<matplotlib.collections.QuadMesh at 0x10e1b4e50>

コード。もう少し簡略化できたが、時間なく断念。



In [170]:

    
# -*- coding: utf-8 -*-

import numpy as np
import matplotlib.pyplot as plt

class Katokadai():
    def __init__(self):
        self.start = [11, 3]
        self.goal = [11, 13]
    
    def cells(self):
        cell = np.loadtxt( "cell.csv", delimiter=","  )
        cells = []
        for i in range(len(cell)):
            cells.append([])

        for i in range(len(cell)):
            for j in range(len(cell[i])):
                cells[-i].append(cell[i-1][j])
        return cells 
        
    
    def log(self):
        log = []
        for i in range(12):
            log.append([0 for i in range(17)])
        return log

    def cost_function(self, place_0, place_1, log):
        cells = self.cells()
        if log[place_1[0]][place_1[1]] == 0:
            mark = cells[place_1[0]][place_1[1]]
            if mark == 0:
                return log[place_0[0]][place_0[1]] + 1
            elif mark == 1:
                return log[place_0[0]][place_0[1]] + 3
            else:
                return 1000
        else:
            return log[place_1[0]][place_1[1]]
        
    def cost_function2(self, place_0, place_1, log):
        cells = self.cells()
        if log[place_1[0]][place_1[1]] == 0:
            mark = cells[place_1[0]][place_1[1]]
            if mark == 0:
                return 1
            elif mark == 1:
                return 3
            else:
                return 1000
        else:
            return log[place_1[0]][place_1[1]]
        
    def manhattan(self, present_place, goal):
        distance_x = math.fabs(present_place[0] - goal[0])
        distance_y = math.fabs(present_place[1] - goal[1])
        distance = distance_x + distance_y 
        return distance
    
    def minimum(self, log):
        log = np.array(log)
        log1 = log
        for i in range(len(log1)):
            for j in range(len(log1[i])):
                if log1[i][j] == 0:
                    log1[i][j] = 1000
        indexies = np.where(log1 == log1.min())
        index = [indexies[0][0], indexies[1][0]]

        return index                      
    
    def breadth_first_search(self): 
        cells = self.cells()
        log = self.log()
        depth = 0
        # 上、下、左、右の順に評価する。
        while log[11][13] == 0:
            same_depth = []
            if depth == 0:
                same_depth.append(self.start)
            else:
                for i in range(len(cells)):
                    for j in range(len(cells[i])):
                        if log[i][j] == depth:
                            same_depth.append([i, j])
            for past_place in same_depth: 
                up = [past_place[0] + 1, past_place[1]]
                down = [past_place[0] - 1, past_place[1]]
                left = [past_place[0], past_place[1]-1]
                right = [past_place[0], past_place[1]+1]
                if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
                elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log)
                elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
                elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
                elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log)
                elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log) 
                else:
                    log[up[0]][up[1]] = self.cost_function(past_place, up, log)
                    log[down[0]][down[1]] = self.cost_function(past_place, down, log)
                    log[left[0]][left[1]] = self.cost_function(past_place, left, log)
                    log[right[0]][right[1]] = self.cost_function(past_place, right, log)
            depth += 1
        return log
    
    def A_star(self):
        cells = self.cells()
        log = self.log()
        past_place = self.start
        before_past_place = [0, 0]
        morebefore_past_place = [-1,-1]
        goal = self.goal
        k = 0
        # 上、下、左、右の順に評価する。
        while log[11][13] == 0:    
            up = [past_place[0] + 1, past_place[1]]
            down = [past_place[0] - 1, past_place[1]]
            left = [past_place[0], past_place[1]-1]
            right = [past_place[0], past_place[1]+1]
            morebefore_past_place = before_past_place
            before_past_place = past_place
            if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = up
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
            elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = down
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = up
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right
            elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal)
                past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left 
            elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal) 
                past_place = up
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left 
            else:
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(past_place, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(past_place, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(past_place, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(past_place, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right
            if past_place == morebefore_past_place:
                if past_place[1] != 16:
                    right = [past_place[0], past_place[1]+1]
                    if log[past_place[0]][past_place[1]] == log[right[0]][right[1]]:
                        log[past_place[0]][past_place[1]] += 100
                        past_place = right
                    else:
                        log[past_place[0]][past_place[1]] += 100
                        past_place = self.minimum(log)
                else:
                    log[past_place[0]][past_place[1]] += 100
                    past_place = self.minimum(log)
            k += 1
            if k == 200:
                break
            
        return log
    
    def LRTA_star1(self):
        cells = self.cells()
        log = self.log()
        past_place = self.start
        before_past_place = [0, 0]
        morebefore_past_place = [-1,-1]
        goal = self.goal
        k = 0
        h = self.log()
        # 上、下、左、右の順に評価する。
        while log[11][13] == 0:    
            up = [past_place[0] + 1, past_place[1]]
            down = [past_place[0] - 1, past_place[1]]
            left = [past_place[0], past_place[1]-1]
            right = [past_place[0], past_place[1]+1]
            morebefore_past_place = before_past_place
            before_past_place = past_place
            h[before_past_place[0]][before_past_place[1]] = h[past_place[0]][past_place[1]]
            if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = up
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
            elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = down
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right 
            elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = up
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right
            elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left 
            elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal) 
                past_place = up
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left 
            else:
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = up
                if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                    past_place = down
                if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                    past_place = left
                if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                    past_place = right
            if past_place == morebefore_past_place:
                if past_place[1] != 16:
                    right = [past_place[0], past_place[1]+1]
                    if log[past_place[0]][past_place[1]] == log[right[0]][right[1]]:
                        log[past_place[0]][past_place[1]] += 100
                        past_place = right
                    else:
                        log[past_place[0]][past_place[1]] += 100
                        past_place = self.minimum(log)
                else:
                    log[past_place[0]][past_place[1]] += 100
                    past_place = self.minimum(log)
            k += 1
            if k == 200:
                break
            
        return log, h
    
    def LRTA_star2(self):
        cells = self.cells()
        before_past_place = [0, 0]
        morebefore_past_place = [-1,-1]
        goal = self.goal
        k = 0
        log1, h = self.LRTA_star1()
        # 上、下、左、右の順に評価する。
        while k < 7:
            log = self.log()
            path = self.log()
            past_place = self.start
            while log[11][13] == 0:    
                up = [past_place[0] + 1, past_place[1]]
                down = [past_place[0] - 1, past_place[1]]
                left = [past_place[0], past_place[1]-1]
                right = [past_place[0], past_place[1]+1]
                morebefore_past_place = before_past_place
                before_past_place = past_place
                h[before_past_place[0]][before_past_place[1]] = h[past_place[0]][past_place[1]]
                if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = down
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right 
                elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = up
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right 
                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = up
                    if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                        past_place = down
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right 
                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = up
                    if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                        past_place = down
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left
                elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = down
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right 
                elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = up
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right
                elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = down
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left 
                elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = up
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left 
                else:
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = up
                    if log[down[0]][down[1]] < log[past_place[0]][past_place[1]]:
                        past_place = down
                    if log[left[0]][left[1]] < log[past_place[0]][past_place[1]]:
                        past_place = left
                    if log[right[0]][right[1]] < log[past_place[0]][past_place[1]]:
                        past_place = right
                if past_place == morebefore_past_place:
                    if past_place[1] != 16:
                        right = [past_place[0], past_place[1]+1]
                        if log[past_place[0]][past_place[1]] == log[right[0]][right[1]]:
                            log[past_place[0]][past_place[1]] += 100
                            past_place = right
                        else:
                            log[past_place[0]][past_place[1]] += 100
                            past_place = self.minimum(log)
                    else:
                        log[past_place[0]][past_place[1]] += 100
                        past_place = self.minimum(log)
                h = log
                path[past_place[0]][past_place[1]] = 1
            k += 1

        return log, path 
    
    def LRTA_star3(self):
        cells = self.cells()
        log = self.log()
        past_place = self.start
        before_past_place = [0, 0]
        morebefore_past_place = [-1,-1]
        goal = self.goal
        k = 0
        h = self.log()
        # ランダムに評価する
        while log[11][13] == 0:    
            up = [past_place[0] + 1, past_place[1]]
            down = [past_place[0] - 1, past_place[1]]
            left = [past_place[0], past_place[1]-1]
            right = [past_place[0], past_place[1]+1]
            morebefore_past_place = before_past_place
            before_past_place = past_place
            h[before_past_place[0]][before_past_place[1]] = h[past_place[0]][past_place[1]]
            if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice(down, left, right)
            elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice([up, left, right])
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice([up, down,right])
            elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                past_place = np.random.choice([up, down, left])
            elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice([down, right])
            elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice([upright])
            elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                past_place = np.random.choice([down, left])
            elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal) 
                past_place = np.random.choice([up,  left])
            else:
                log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + self.manhattan(up, goal)
                log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + self.manhattan(down, goal)
                log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + self.manhattan(left, goal)
                log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + self.manhattan(right, goal)
                past_place = np.random.choice([up, down, left, right])
            k += 1
            if k == 200:
                break
            
        return log, h
    
    def LRTA_star4(self):
        cells = self.cells()
        before_past_place = [0, 0]
        morebefore_past_place = [-1,-1]
        goal = self.goal
        k = 0
        log1, h = self.LRTA_star3()
        # ランダムに評価する。
        while k < 7:
            log = self.log()
            path = self.log()
            past_place = self.start
            while log[11][13] == 0:    
                up = [past_place[0] + 1, past_place[1]]
                down = [past_place[0] - 1, past_place[1]]
                left = [past_place[0], past_place[1]-1]
                right = [past_place[0], past_place[1]+1]
                morebefore_past_place = before_past_place
                before_past_place = past_place
                h[before_past_place[0]][before_past_place[1]] = h[past_place[0]][past_place[1]]
                if past_place[0] == 11 and (past_place[1] != 0 and past_place[1] != 16): #上の辺
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([down, left, right])

                elif past_place[0] == 0 and (past_place[1] != 0 and past_place[1] != 16): #下の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([up, left, right])

                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 0: #左の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([up, down, right])
                elif (past_place[0] != 0 and past_place[0] != 11) and past_place[1] == 16: #右の辺
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = np.random.choice([up, down, left])
                elif past_place[0] == 11 and past_place[1] == 0: #左上隅
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([down, right])
                elif past_place[0] == 0 and past_place[1] == 0: #左下隅
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([up, right])
                elif past_place[0] == 11 and past_place[1] == 16: #右上隅
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = np.random.choice([down, left])
                elif past_place[0] == 0 and past_place[1] == 16: #右下隅
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    past_place = np.random.choice([up, left])
                else:
                    log[up[0]][up[1]] = self.cost_function2(past_place, up, log) + h[up[0]][up[1]]
                    log[down[0]][down[1]] = self.cost_function2(past_place, down, log) + h[down[0]][down[1]]
                    log[left[0]][left[1]] = self.cost_function2(past_place, left, log) + h[left[0]][left[1]]
                    log[right[0]][right[1]] = self.cost_function2(past_place, right, log) + h[right[0]][right[1]]
                    past_place = np.random.choice([up, down, left, right])
            k += 1

        return log, path



In [171]:

    
k = Katokadai()

深さ優先探索



In [172]:

    
%time log1 = k.breadth_first_search()
log1









    



CPU times: user 163 ms, sys: 11.8 ms, total: 175 ms
Wall time: 182 ms






    Out[172]:





[[16, 15, 16, 17, 18, 19, 20, 21, 22, 25, 28, 27, 28, 29, 30, 31, 32],
 [15, 14, 1000, 18, 19, 20, 1000, 22, 23, 24, 25, 26, 1000, 30, 31, 32, 33],
 [14,
  13,
  1000,
  1000,
  1000,
  1000,
  1000,
  23,
  24,
  25,
  26,
  1000,
  0,
  1000,
  1000,
  33,
  34],
 [13, 12, 1000, 0, 0, 0, 1000, 24, 25, 26, 27, 1000, 0, 0, 0, 1000, 35],
 [12, 11, 1000, 0, 0, 0, 0, 1000, 1000, 1000, 1000, 0, 0, 0, 1000, 37, 36],
 [11, 10, 1000, 0, 0, 0, 0, 50, 49, 1000, 1000, 0, 0, 1000, 43, 1000, 37],
 [12, 9, 1000, 0, 0, 0, 1000, 49, 48, 47, 46, 1000, 1000, 1000, 42, 1000, 38],
 [9, 8, 7, 1000, 0, 0, 1000, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39],
 [6, 7, 6, 7, 1000, 0, 1000, 49, 48, 47, 46, 1000, 1000, 1000, 1000, 1000, 42],
 [5, 8, 5, 6, 1000, 0, 0, 50, 49, 48, 1000, 0, 0, 0, 1000, 46, 45],
 [4, 5, 4, 3, 2, 1000, 0, 0, 50, 49, 50, 0, 0, 1000, 48, 47, 48],
 [3, 2, 1, 2, 1, 1000, 0, 0, 0, 50, 0, 0, 0, 50, 49, 48, 49]]

A*探索



In [151]:

    
%time log2 = k.A_star()
log2









    



CPU times: user 160 ms, sys: 10.4 ms, total: 171 ms
Wall time: 178 ms






    Out[151]:





[[47.0,
  69.0,
  45.0,
  43.0,
  60.0,
  37.0,
  37.0,
  35.0,
  18.0,
  17.0,
  29.0,
  25.0,
  25.0,
  23.0,
  23.0,
  13.0,
  0],
 [69.0,
  68.0,
  1064.0,
  41.0,
  39.0,
  39.0,
  1052.0,
  33.0,
  31.0,
  27.0,
  27.0,
  27.0,
  1034.0,
  23.0,
  23.0,
  23.0,
  25.0],
 [66.0,
  65.0,
  1021.0,
  1020.0,
  1019.0,
  1018.0,
  1015.0,
  31.0,
  42.0,
  27.0,
  27.0,
  1012.0,
  0,
  1010.0,
  1022.0,
  25.0,
  23.0],
 [63.0,
  62.0,
  1020.0,
  0,
  0,
  0,
  1014.0,
  29.0,
  27.0,
  27.0,
  13.0,
  0,
  0,
  0,
  0,
  1022.0,
  23.0],
 [60.0,
  59.0,
  1019.0,
  0,
  0,
  0,
  0,
  1026.0,
  1024.0,
  1012.0,
  0,
  0,
  0,
  0,
  1007.0,
  11.0,
  21.0],
 [57.0,
  56.0,
  1018.0,
  0,
  0,
  0,
  1012.0,
  23.0,
  23.0,
  1020.0,
  1008.0,
  0,
  0,
  1007.0,
  7.0,
  1016.0,
  19.0],
 [56.0,
  53.0,
  1017.0,
  0,
  0,
  0,
  1011.0,
  33.0,
  32.0,
  119.0,
  17.0,
  1014.0,
  1005.0,
  1010.0,
  113.0,
  1020.0,
  17.0],
 [69.0,
  50.0,
  31.0,
  1026.0,
  0,
  0,
  1010.0,
  21.0,
  21.0,
  24.0,
  21.0,
  13.0,
  11.0,
  11.0,
  17.0,
  13.0,
  15.0],
 [63.0,
  63.0,
  55.0,
  27.0,
  1026.0,
  0,
  1009.0,
  19.0,
  17.0,
  13.0,
  15.0,
  1012.0,
  1005.0,
  1004.0,
  1005.0,
  1006.0,
  10.0],
 [61.0,
  61.0,
  139.0,
  164.0,
  1042.0,
  0,
  9.0,
  17.0,
  15.0,
  13.0,
  1012.0,
  0,
  0,
  0,
  0,
  0,
  0],
 [57.0,
  67.0,
  60.0,
  77.0,
  132.0,
  1030.0,
  8.0,
  15.0,
  18.0,
  11.0,
  9.0,
  1002.0,
  1001.0,
  0,
  0,
  0,
  0],
 [53.0,
  49.0,
  57.0,
  54.0,
  151.0,
  1009.0,
  7.0,
  13.0,
  11.0,
  14.0,
  7.0,
  5.0,
  3.0,
  2.0,
  0,
  0,
  0]]

深さ優先探索は191ms、A探索は178ms。 A探索の方がわずかに速かった。

LRTA*探索学習前



In [148]:

    
log3, h = k.LRTA_star1()
log3









    Out[148]:





[[25.0,
  47.0,
  45.0,
  43.0,
  61.0,
  39.0,
  37.0,
  35.0,
  17.0,
  0,
  31.0,
  27.0,
  25.0,
  23.0,
  25.0,
  14.0,
  0],
 [24.0,
  45.0,
  1063.0,
  41.0,
  39.0,
  37.0,
  1051.0,
  33.0,
  16.0,
  15.0,
  27.0,
  25.0,
  1033.0,
  21.0,
  23.0,
  25.0,
  27.0],
 [23.0,
  43.0,
  1020.0,
  1019.0,
  1018.0,
  1017.0,
  1016.0,
  31.0,
  29.0,
  27.0,
  25.0,
  1022.0,
  0,
  1009.0,
  1020.0,
  23.0,
  25.0],
 [22.0,
  41.0,
  1019.0,
  0,
  0,
  0,
  1015.0,
  29.0,
  27.0,
  25.0,
  23.0,
  1010.0,
  0,
  0,
  0,
  1020.0,
  23.0],
 [21.0,
  39.0,
  1018.0,
  0,
  0,
  0,
  0,
  1013.0,
  1012.0,
  1011.0,
  1010.0,
  0,
  0,
  0,
  0,
  10.0,
  21.0],
 [20.0, 37.0, 1017.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1008.0, 19.0],
 [21.0,
  35.0,
  1032.0,
  0,
  0,
  0,
  0,
  0,
  0,
  0,
  9.0,
  1007.0,
  1006.0,
  1005.0,
  7.0,
  1014.0,
  17.0],
 [20.0,
  33.0,
  31.0,
  1028.0,
  0,
  0,
  0,
  0,
  0,
  17.0,
  15.0,
  13.0,
  11.0,
  9.0,
  11.0,
  13.0,
  15.0],
 [0,
  31.0,
  29.0,
  27.0,
  1012.0,
  0,
  0,
  0,
  9.0,
  15.0,
  13.0,
  1010.0,
  1004.0,
  1003.0,
  1004.0,
  1005.0,
  9.0],
 [0, 0, 27.0, 27.0, 1033.0, 0, 0, 0, 8.0, 13.0, 1010.0, 0, 0, 0, 0, 0, 0],
 [0,
  0,
  15.0,
  47.0,
  21.0,
  1018.0,
  0,
  0,
  7.0,
  11.0,
  9.0,
  1003.0,
  1002.0,
  0,
  0,
  0,
  0],
 [0,
  0,
  12.0,
  21.0,
  128.0,
  1008.0,
  0,
  0,
  6.0,
  9.0,
  7.0,
  5.0,
  2.0,
  1.0,
  0,
  0,
  0]]

経路可視化。水色が通過場所、赤色は進入不可域との境。



In [155]:

    
graph3 = np.array(log3)
for i in range(len(graph3)):
    for j in range(len(graph3[i])):
        if graph3[i][j] > 0 and graph3[i][j] < 500:
            graph3[i][j] = 3
        elif graph3[i][j] > 900:
            graph3[i][j] = 10
plt.pcolormesh(graph3)









    Out[155]:





<matplotlib.collections.QuadMesh at 0x10e12d410>

LRTA*探索、7回実行。



In [156]:

    
log4, path= k.LRTA_star2()
log4









    Out[156]:





[[2, 4, 2, 2, 4, 2, 2, 4, 102, 6, 6, 2, 2, 2, 4, 204, 102],
 [4, 4, 4000, 2, 2, 2, 4000, 4, 4, 4, 4, 2, 4000, 2, 2, 8, 8],
 [4, 4, 1000, 1000, 1000, 1000, 1000, 2, 8, 4, 4, 2000, 0, 1000, 2000, 2, 4],
 [4, 8, 1000, 0, 0, 0, 1000, 2, 2, 4, 102, 1000, 0, 0, 0, 4000, 4],
 [208, 8, 2000, 0, 0, 0, 0, 2000, 2000, 1000, 1000, 0, 0, 0, 4000, 102, 4],
 [4, 16, 2000, 0, 0, 0, 1000, 2, 2, 2000, 2000, 0, 0, 2000, 102, 8000, 4],
 [12, 8, 4000, 0, 0, 0, 1000, 4, 4, 204, 4, 4000, 1000, 4000, 204, 8000, 2],
 [12, 8, 4, 2000, 0, 0, 1000, 2, 2, 16, 16, 4, 2, 2, 8, 2, 2],
 [2, 8, 8, 2, 1000, 0, 1000, 2, 4, 8, 8, 4000, 1000, 1000, 1000, 1000, 3],
 [2, 12, 2, 106, 1000, 0, 1, 2, 4, 8, 16000, 0, 0, 0, 0, 0, 0],
 [2, 6, 6, 2490, 0, 0, 1, 2, 8, 8, 4, 2000, 1000, 0, 0, 0, 0],
 [2, 2, 446, 1, 421, 0, 1, 2, 2, 8, 4, 2, 1, 1, 0, 0, 0]]

7回の間の通過経路。水色が通過場所、赤色は進入不可域との境。



In [145]:

    
graph4 = np.array(log4)
for i in range(len(graph4)):
    for j in range(len(graph4[i])):
        if graph4[i][j] > 0 and graph4[i][j] < 500:
            graph4[i][j] = 3
        elif graph4[i][j] > 900:
            graph4[i][j] = 10
plt.pcolormesh(graph4)









    Out[145]:





<matplotlib.collections.QuadMesh at 0x10d047d90>

最終的な通過経路。赤色を通ってゴールへ。



In [146]:

    
graph5 = np.array(path)
plt.pcolormesh(graph5)









    Out[146]:





<matplotlib.collections.QuadMesh at 0x10bfb8d50>



In [ ]: