|
|
|
@ -1,5 +1,6 @@
|
|
|
|
|
import math
|
|
|
|
|
import collections
|
|
|
|
|
import random
|
|
|
|
|
|
|
|
|
|
import numpy as np
|
|
|
|
|
import matplotlib.patches
|
|
|
|
@ -23,7 +24,8 @@ class Grid:
|
|
|
|
|
return self._cells[p[0]][p[1]]
|
|
|
|
|
|
|
|
|
|
def set(self, p: tuple, v: float) -> None:
|
|
|
|
|
self._cells[p[0]][p[1]] = v
|
|
|
|
|
if self.contains(p):
|
|
|
|
|
self._cells[p[0]][p[1]] = v
|
|
|
|
|
|
|
|
|
|
def line(self, p: tuple, dp: tuple, c: int, v: float) -> None:
|
|
|
|
|
for i in range(c):
|
|
|
|
@ -38,16 +40,16 @@ class Grid:
|
|
|
|
|
if acc >= INF:
|
|
|
|
|
out[y][x] = acc
|
|
|
|
|
else:
|
|
|
|
|
for d in (-1, 0), (1, 0), (0, 1), (0, -1):
|
|
|
|
|
for d in DIRS.values():
|
|
|
|
|
v = self.get((y + d[0], x + d[1]))
|
|
|
|
|
if v >= INF:
|
|
|
|
|
v = 1
|
|
|
|
|
v = 10
|
|
|
|
|
acc += v
|
|
|
|
|
out[y][x] = acc / 5
|
|
|
|
|
out[y][x] = acc / len(DIRS)
|
|
|
|
|
self._cells = out
|
|
|
|
|
|
|
|
|
|
def contains(self, p):
|
|
|
|
|
y, x = p
|
|
|
|
|
y, x = p[:2]
|
|
|
|
|
if y < 0 or x < 0:
|
|
|
|
|
return False
|
|
|
|
|
if y >= len(self._cells) or x >= len(self._cells[0]):
|
|
|
|
@ -68,16 +70,22 @@ def plot(g: Grid, keepout: Grid):
|
|
|
|
|
xx = x * s - s / 2
|
|
|
|
|
if keepout is not None and keepout._cells[y][x] >= INF:
|
|
|
|
|
color = 'purple'
|
|
|
|
|
elif cell <= 0:
|
|
|
|
|
color = '#eeeeee'
|
|
|
|
|
else:
|
|
|
|
|
color = cmap(cell)
|
|
|
|
|
r = matplotlib.patches.Rectangle((xx, yy), w, w, facecolor=color, linewidth=s)
|
|
|
|
|
ax.add_patch(r)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def h(n, objective):
|
|
|
|
|
def h_city_block(n, objective):
|
|
|
|
|
return abs(n[0] - objective[0]) + abs(n[1] - objective[1])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def h(n, objective):
|
|
|
|
|
return math.sqrt((n[0] - objective[0])**2 + (n[1] - objective[1])**2)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def gf(g, p):
|
|
|
|
|
v = g.get(p)
|
|
|
|
|
if v <= 0:
|
|
|
|
@ -90,63 +98,84 @@ def kf(p, keepout):
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
DIRS = {
|
|
|
|
|
None: ((1, 0), (-1, 0), (0, 1), (0, -1)),
|
|
|
|
|
0: ((1, 0), (1, -1), (1, 1)), # North
|
|
|
|
|
1: ((0, 1), (1, 1), (-1, 1)), # East
|
|
|
|
|
2: ((-1, 0), (-1, -1), (-1, 1)), # South
|
|
|
|
|
3: ((0, -1), (1, -1), (-1, -1)), # West
|
|
|
|
|
0: (1, 0, 1), # North
|
|
|
|
|
1: (1, 1, 1.4), # NE
|
|
|
|
|
2: (0, 1, 1), # East
|
|
|
|
|
3: (-1, 1, 1.4), # SE
|
|
|
|
|
4: (-1, 0, 1), # South
|
|
|
|
|
5: (-1, -1, 1.4), # SW
|
|
|
|
|
6: (0, -1, 1), # West
|
|
|
|
|
7: (1, -1, 1.4), # NW
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def plan(g: Grid, keepout: Grid, start, goal):
|
|
|
|
|
def plan(gr: Grid, keepout: Grid, start, goal):
|
|
|
|
|
closed = set()
|
|
|
|
|
open = [start]
|
|
|
|
|
came_from = {}
|
|
|
|
|
g.set(start, 0.001)
|
|
|
|
|
g = {start: 0}
|
|
|
|
|
|
|
|
|
|
while open:
|
|
|
|
|
c = open[0]
|
|
|
|
|
if c == goal:
|
|
|
|
|
open = open[1:]
|
|
|
|
|
if c[:2] == goal[:2]:
|
|
|
|
|
path = []
|
|
|
|
|
p = goal
|
|
|
|
|
p = c
|
|
|
|
|
while True:
|
|
|
|
|
path.append(p)
|
|
|
|
|
if p == start:
|
|
|
|
|
if p[:2] == start[:2]:
|
|
|
|
|
path.reverse()
|
|
|
|
|
return path
|
|
|
|
|
p = came_from[p]
|
|
|
|
|
open = open[1:]
|
|
|
|
|
closed.add(c)
|
|
|
|
|
|
|
|
|
|
for d in DIRS[None]:
|
|
|
|
|
n = (c[0] + d[0], c[1] + d[1])
|
|
|
|
|
for d in (c[-1] - 1, c[-1], c[-1] + 1):
|
|
|
|
|
if d < 0:
|
|
|
|
|
d += 8
|
|
|
|
|
if d >= 8:
|
|
|
|
|
d -= 8
|
|
|
|
|
di = DIRS[d]
|
|
|
|
|
n = (c[0] + di[0], c[1] + di[1], d)
|
|
|
|
|
if n in closed:
|
|
|
|
|
continue
|
|
|
|
|
if not g.contains(n):
|
|
|
|
|
if not keepout.contains(n):
|
|
|
|
|
continue
|
|
|
|
|
gn = g.get(c) + 1
|
|
|
|
|
if keepout.get(n) >= INF:
|
|
|
|
|
continue
|
|
|
|
|
gn = g[c] + di[-1]
|
|
|
|
|
if gr.get(n) < gn:
|
|
|
|
|
gr.set(n, gn)
|
|
|
|
|
if n not in open:
|
|
|
|
|
open.append(n)
|
|
|
|
|
elif gn >= g.get(n):
|
|
|
|
|
elif gn >= g[n]:
|
|
|
|
|
continue
|
|
|
|
|
came_from[n] = c
|
|
|
|
|
g.set(n, gn)
|
|
|
|
|
g[n] = gn
|
|
|
|
|
open.sort(key=lambda x: gf(g, x) + h(x, goal) + kf(x, keepout))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main():
|
|
|
|
|
g = Grid(30, 30, 1)
|
|
|
|
|
keepout = Grid(30, 30, 1)
|
|
|
|
|
keepout.line((10, 0), (0, 1), 5, INF)
|
|
|
|
|
keepout.line((12, 12), (1, 0), 15, INF)
|
|
|
|
|
w, h = 30, 30
|
|
|
|
|
g = Grid(h, w, 1)
|
|
|
|
|
keepout = Grid(h, w, 1)
|
|
|
|
|
|
|
|
|
|
for i in range(20):
|
|
|
|
|
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)
|
|
|
|
|
keepout.smear()
|
|
|
|
|
|
|
|
|
|
path = plan(g, keepout, (1, 2), (29, 18))
|
|
|
|
|
start = (random.randint(0, h), random.randint(0, w), 0)
|
|
|
|
|
goal = (random.randint(0, h), random.randint(0, w))
|
|
|
|
|
path = plan(g, keepout, start, goal)
|
|
|
|
|
path = np.array(path)
|
|
|
|
|
|
|
|
|
|
plot(g, keepout)
|
|
|
|
|
plt.tight_layout()
|
|
|
|
|
plt.plot(path[:, 1], path[:, 0])
|
|
|
|
|
plt.axis('equal')
|
|
|
|
|
# plt.plot(X[:, 0, ], X[:, 1])
|
|
|
|
|
plt.savefig('fig.png')
|
|
|
|
|
plt.show()
|
|
|
|
|
|
|
|
|
|