|
|
@@ -16,11 +16,12 @@
|
|
|
# 1 = Occupied space
|
|
|
|
|
|
from collections import deque
|
|
|
+
|
|
|
import numpy as np
|
|
|
|
|
|
grid = np.array([[0, 0, 1, 0, 0, 0, 0],
|
|
|
[0, 0, 1, 0, 1, 0, 1],
|
|
|
- [1, 0, 0, 0, 0, 0, 0],
|
|
|
+ [0, 0, 1, 0, 0, 0, 0],
|
|
|
[0, 0, 1, 1, 1, 0, 0],
|
|
|
[0, 0, 0, 0, 1, 0, 1]])
|
|
|
init = np.array([0, 0])
|
|
|
@@ -44,7 +45,7 @@ def check_navigable(goal) -> bool:
|
|
|
return False
|
|
|
|
|
|
|
|
|
-def search(grid: list, init: list, goal: list, cost: list):
|
|
|
+def search(grid: np.ndarray, init: list, goal: list, cost: list):
|
|
|
# ----------------------------------------
|
|
|
# insert code here
|
|
|
# ----------------------------------------
|
|
|
@@ -54,23 +55,29 @@ def search(grid: list, init: list, goal: list, cost: list):
|
|
|
already_checked = dict()
|
|
|
already_checked[tuple(init)] = 0
|
|
|
cost_map = np.zeros([len(grid), len(grid[0])])
|
|
|
+ extend_map = np.zeros([len(grid), len(grid[0])]) - 1
|
|
|
+ extend_now = 0
|
|
|
+ extend_map[0,0] = extend_now
|
|
|
|
|
|
while next_check:
|
|
|
checking = next_check.popleft()
|
|
|
if check_navigable(checking):
|
|
|
- # already_checked[tuple(checking)] =
|
|
|
for move in delta:
|
|
|
next = checking+move # type: np.ndarray
|
|
|
if check_navigable(next) and tuple(next) not in already_checked:
|
|
|
+ extend_now += 1
|
|
|
next_check.append(next)
|
|
|
cost_now = already_checked[checking[0], checking[1]]+1
|
|
|
already_checked[tuple(next)] = cost_now
|
|
|
cost_map[next[0], next[1]] = cost_now
|
|
|
+ extend_map[next[0], next[1]] = extend_now
|
|
|
if all(next == goal):
|
|
|
print(cost_map)
|
|
|
+ print(extend_map)
|
|
|
return [cost_now, next[0], next[1]]
|
|
|
|
|
|
print(cost_map)
|
|
|
+ print(extend_map)
|
|
|
return "fail"
|
|
|
|
|
|
|
