207 lines
5.1 KiB
Python
207 lines
5.1 KiB
Python
import math
|
|
import collections
|
|
import random
|
|
|
|
import numpy as np
|
|
import matplotlib.patches
|
|
import matplotlib.pyplot as plt
|
|
|
|
INF = 1e3
|
|
|
|
|
|
class Grid:
|
|
def __init__(self, w: float, h: float, s: float):
|
|
self._w = w
|
|
self._h = h
|
|
self._s = s
|
|
|
|
self._stride = int(math.ceil(w / s))
|
|
self._cells = np.zeros((self._stride, int(math.ceil(h / s))))
|
|
|
|
def get(self, p: tuple) -> float:
|
|
if not self.contains(p):
|
|
return 0
|
|
return self._cells[p[0]][p[1]]
|
|
|
|
def set(self, p: tuple, v: float) -> None:
|
|
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):
|
|
self.set((int(p[0]), int(p[1])), v)
|
|
p = (p[0] + dp[0], p[1] + dp[1])
|
|
|
|
def smear(self) -> None:
|
|
out = np.zeros(self._cells.shape)
|
|
for y in range(len(self._cells)):
|
|
for x in range(len(self._cells[0])):
|
|
acc = self.get((y, x))
|
|
if acc >= INF:
|
|
out[y][x] = acc
|
|
else:
|
|
for d in DIRS.values():
|
|
v = self.get((y + d[0], x + d[1]))
|
|
if v >= INF:
|
|
v = 3
|
|
acc += v
|
|
out[y][x] = acc / len(DIRS)
|
|
self._cells = out
|
|
|
|
def contains(self, p):
|
|
y, x = p[:2]
|
|
if y < 0 or x < 0:
|
|
return False
|
|
if y >= len(self._cells) or x >= len(self._cells[0]):
|
|
return False
|
|
return True
|
|
|
|
|
|
def plot(g: Grid, keepout: Grid):
|
|
cells = g._cells / np.max(g._cells)
|
|
s = g._s
|
|
w = s * 0.9
|
|
cmap = plt.get_cmap('hot')
|
|
plt.figure()
|
|
ax = plt.gca()
|
|
for y, row in enumerate(cells):
|
|
yy = y * s - s / 2
|
|
for x, cell in enumerate(row):
|
|
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_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:
|
|
return INF
|
|
return v
|
|
|
|
|
|
def kf(p, keepout):
|
|
return keepout.get(p)
|
|
|
|
|
|
DIRS = {
|
|
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(gr: Grid, keepout: Grid, start, goal):
|
|
closed = set()
|
|
open = [start]
|
|
came_from = {}
|
|
g = {start: 0}
|
|
|
|
while open:
|
|
c = open[0]
|
|
open = open[1:]
|
|
if c[:2] == goal[:2]:
|
|
path = []
|
|
p = c
|
|
while True:
|
|
path.append(p)
|
|
if p[:2] == start[:2]:
|
|
path.reverse()
|
|
return path
|
|
p = came_from[p]
|
|
closed.add(c)
|
|
|
|
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 keepout.contains(n):
|
|
continue
|
|
if keepout.get(n) >= INF:
|
|
continue
|
|
gn = g[c] + di[-1] + kf(n, keepout)
|
|
if gr.get(n) < gn:
|
|
gr.set(n, gn)
|
|
if n not in open:
|
|
open.append(n)
|
|
elif gn >= g[n]:
|
|
continue
|
|
came_from[n] = c
|
|
g[n] = gn
|
|
open.sort(key=lambda x: gf(g, x) + h(x, goal)) # + kf(x, keepout))
|
|
|
|
|
|
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 = (2, 16, 3)
|
|
goal = (28, 16)
|
|
|
|
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.savefig('fig.png')
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|