grid: take steering transitions into account.

master
Michael Hope 4 years ago
parent 898cf63df0
commit 2d89714772

@ -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()

Loading…
Cancel
Save