| 35 | |
| 36 | #function to search the path |
| 37 | def search(grid,init,goal,cost,heuristic): |
| 38 | |
| 39 | closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]# the referrence grid |
| 40 | closed[init[0]][init[1]] = 1 |
| 41 | action = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]#the action grid |
| 42 | |
| 43 | x = init[0] |
| 44 | y = init[1] |
| 45 | g = 0 |
| 46 | f = g + heuristic[init[0]][init[0]] |
| 47 | cell = [[f, g, x, y]] |
| 48 | |
| 49 | found = False # flag that is set when search is complete |
| 50 | resign = False # flag set if we can't find expand |
| 51 | |
| 52 | while not found and not resign: |
| 53 | if len(cell) == 0: |
| 54 | resign = True |
| 55 | return "FAIL" |
| 56 | else: |
| 57 | cell.sort()#to choose the least costliest action so as to move closer to the goal |
| 58 | cell.reverse() |
| 59 | next = cell.pop() |
| 60 | x = next[2] |
| 61 | y = next[3] |
| 62 | g = next[1] |
| 63 | f = next[0] |
| 64 | |
| 65 | |
| 66 | if x == goal[0] and y == goal[1]: |
| 67 | found = True |
| 68 | else: |
| 69 | for i in range(len(delta)):#to try out different valid actions |
| 70 | x2 = x + delta[i][0] |
| 71 | y2 = y + delta[i][1] |
| 72 | if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): |
| 73 | if closed[x2][y2] == 0 and grid[x2][y2] == 0: |
| 74 | g2 = g + cost |
| 75 | f2 = g2 + heuristic[x2][y2] |
| 76 | cell.append([f2, g2, x2, y2]) |
| 77 | closed[x2][y2] = 1 |
| 78 | action[x2][y2] = i |
| 79 | invpath = [] |
| 80 | x = goal[0] |
| 81 | y = goal[1] |
| 82 | invpath.append([x, y])#we get the reverse path from here |
| 83 | while x != init[0] or y != init[1]: |
| 84 | x2 = x - delta[action[x][y]][0] |
| 85 | y2 = y - delta[action[x][y]][1] |
| 86 | x = x2 |
| 87 | y = y2 |
| 88 | invpath.append([x, y]) |
| 89 | |
| 90 | path = [] |
| 91 | for i in range(len(invpath)): |
| 92 | path.append(invpath[len(invpath) - 1 - i]) |
| 93 | print("ACTION MAP") |
| 94 | for i in range(len(action)): |