1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
|
import numpy as np
# x---x---x---x---x
# | | |
# | A | |
# | | |
# x x---x x---x
# | |
# | |
# | |
# x---x x x
# | | | |
# S | | C |
# | | | |
# x x x x
# | |
# | |
# | |
# x x---x x---x
# | | |
# | B | D |
# | L | |
# x---x---x---x---x
mapWidth = 4
mapHeight = 5
currentPos = [2, 2]
allNodes = []
for x in range(mapWidth): # Like for(int x = 0; x > 20; i++) {
for y in range(mapHeight):
allNodes.append([x, y])
def queryNeighbors(node, obstacleList):
# Right Down Left Up
directions = [[1,0], [0, 1], [-1, 0], [0, -1]]
result = []
for dir in directions: # Basically for x in range(4)
neighbor = [node[0] + dir[0], node[1] + dir[1]] # [nodeX + DirectionModifierX, nodeY + DirectionModifierY]
if neighbor in allNodes: # If neighbor is present in the grid:
result.append(neighbor)
for x in range(len(obstacleList)):
if node in obstacleList[x] and neighbor in obstacleList[x]:
result.remove(neighbor)
return result
obstacleList = [
[[2, 0], [3, 0]], # obstacleList[0]
[[1, 0], [1, 1]], # obstacleList[1]
[[3, 0], [3, 1]],
[[0, 1], [0, 2]],
[[0, 2], [1, 2]],
[[2, 2], [3, 2]],
[[1, 3], [1, 4]],
[[3, 3], [3, 4]],
[[1, 4], [2, 4]]
]
# print('OBSTACLES:')
# for x in range(len(obstacleList)):
# print(obstacleList[x])
print('CURRENT POSITION:')
print(currentPos)
print('NEIGHBORS:')
print(queryNeighbors(currentPos, obstacleList))
|
