|
|
|
@ -43,7 +43,7 @@ class Grid:
|
|
|
|
|
for d in DIRS.values():
|
|
|
|
|
v = self.get((y + d[0], x + d[1]))
|
|
|
|
|
if v >= INF:
|
|
|
|
|
v = 10
|
|
|
|
|
v = 3
|
|
|
|
|
acc += v
|
|
|
|
|
out[y][x] = acc / len(DIRS)
|
|
|
|
|
self._cells = out
|
|
|
|
@ -142,7 +142,7 @@ def plan(gr: Grid, keepout: Grid, start, goal):
|
|
|
|
|
continue
|
|
|
|
|
if keepout.get(n) >= INF:
|
|
|
|
|
continue
|
|
|
|
|
gn = g[c] + di[-1]
|
|
|
|
|
gn = g[c] + di[-1] + kf(n, keepout)
|
|
|
|
|
if gr.get(n) < gn:
|
|
|
|
|
gr.set(n, gn)
|
|
|
|
|
if n not in open:
|
|
|
|
@ -151,24 +151,46 @@ def plan(gr: Grid, keepout: Grid, start, goal):
|
|
|
|
|
continue
|
|
|
|
|
came_from[n] = c
|
|
|
|
|
g[n] = gn
|
|
|
|
|
open.sort(key=lambda x: gf(g, x) + h(x, goal) + kf(x, keepout))
|
|
|
|
|
open.sort(key=lambda x: gf(g, x) + h(x, goal)) # + kf(x, keepout))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main():
|
|
|
|
|
w, h = 30, 30
|
|
|
|
|
g = Grid(h, w, 1)
|
|
|
|
|
keepout = Grid(h, w, 1)
|
|
|
|
|
|
|
|
|
|
for i in range(20):
|
|
|
|
|
def random_walls(g):
|
|
|
|
|
for i in range(2):
|
|
|
|
|
d = (1, 0)
|
|
|
|
|
if random.randint(0, 1) == 1:
|
|
|
|
|
d = (0, 1)
|
|
|
|
|
keepout.line((random.randint(0, h), random.randint(0, w)), d, random.randint(5, h // 2),
|
|
|
|
|
INF)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def parking(g):
|
|
|
|
|
h = 20
|
|
|
|
|
w = 10
|
|
|
|
|
y = 6
|
|
|
|
|
g.line((y, 10), (1, 0), h, INF)
|
|
|
|
|
g.line((y + h, 10), (0, 1), w, INF)
|
|
|
|
|
g.line((y, 10), (0, 1), w, INF)
|
|
|
|
|
g.line((y, 10 + w - 1), (1, 0), h, INF)
|
|
|
|
|
g.line((y, 10 + w - 1 + 3), (1, 0), h, INF)
|
|
|
|
|
g.line((y, 10 + w - 1 + 3), (0, 1), 9, INF)
|
|
|
|
|
g.line((y, 10 - 2), (1, 0), h, INF)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# g.line((5, 0), (0, 1), 9, INF)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main():
|
|
|
|
|
w, h = 30, 30
|
|
|
|
|
g = Grid(h, w, 1)
|
|
|
|
|
keepout = Grid(h, w, 1)
|
|
|
|
|
parking(keepout)
|
|
|
|
|
keepout.smear()
|
|
|
|
|
|
|
|
|
|
start = (random.randint(0, h), random.randint(0, w), 0)
|
|
|
|
|
goal = (random.randint(0, h), random.randint(0, w))
|
|
|
|
|
#start = (random.randint(0, h), random.randint(0, w), 0)
|
|
|
|
|
#goal = (random.randint(0, h), random.randint(0, w))
|
|
|
|
|
start = (2, 16, 3)
|
|
|
|
|
goal = (28, 16)
|
|
|
|
|
|
|
|
|
|
path = plan(g, keepout, start, goal)
|
|
|
|
|
path = np.array(path)
|
|
|
|
|
|
|
|
|
|