133 lines
5.0 KiB
Python
133 lines
5.0 KiB
Python
Sh = 200
|
|
St = 10
|
|
WAREHOUSE_MAP = [
|
|
[0, 0, 0, St, St, 0, 0, 0, 0],
|
|
[0, St, 0, St, 0, St, 0, 0, 0],
|
|
[0, St, 0, 0, 0, Sh, Sh, Sh, Sh],
|
|
[0, 0, St, 0, 0, 0, 0, 0, 0],
|
|
[0, 0, St, 0, 0, Sh, Sh, Sh, Sh],
|
|
[0, 0, St, St, 0, 0, 0, 0, 0],
|
|
[St, 0, 0, 0, St, Sh, Sh, Sh, Sh],
|
|
[0, 0, St, St, St, 0, 0, 0, 0],
|
|
[0, St, 0, 0, 0, Sh, Sh, Sh, Sh],
|
|
]
|
|
|
|
|
|
def a_star_search(start, end, agent_direction):
|
|
|
|
open_nodes = []
|
|
closed_nodes = []
|
|
|
|
start_node = NodeAStar(agent_direction, None, start)
|
|
start_node.g = start_node.h = start_node.f = 0
|
|
end_node = NodeAStar(None, None, end)
|
|
end_node.g = end_node.h = end_node.f = 0
|
|
|
|
open_nodes.append(start_node)
|
|
|
|
while len(open_nodes) != 0:
|
|
current_node = open_nodes[0]
|
|
current_node_index = 0
|
|
for index, node in enumerate(open_nodes):
|
|
if node.f < current_node.f:
|
|
current_node = node
|
|
current_node_index = index
|
|
|
|
open_nodes.pop(current_node_index)
|
|
closed_nodes.append(current_node)
|
|
|
|
# Sprawdzam czy jesteśmy u celu jeżeli tak zwracamy ścieżkę
|
|
neighbour_nodes = []
|
|
if current_node == end_node:
|
|
path = []
|
|
current = current_node
|
|
while current is not None:
|
|
#path.append(current.position)
|
|
if current.action is not None:
|
|
current.action.reverse()
|
|
for each_action in current.action:
|
|
path.append(each_action)
|
|
|
|
current = current.parent
|
|
#path = path.pop()
|
|
return path[::-1]
|
|
#
|
|
|
|
x = current_node.position[0]
|
|
y = current_node.position[1]
|
|
if x < 8: # DOWN NEIGHBOUR
|
|
if current_node.agent_direction == "right":
|
|
actions = ["rotate_right", "move"]
|
|
elif current_node.agent_direction == "left":
|
|
actions = ["rotate_left", "move"]
|
|
elif current_node.agent_direction == "up":
|
|
actions = ["rotate_right", "rotate_right", "move"]
|
|
elif current_node.agent_direction == "down":
|
|
actions = ["move"]
|
|
neighbour_nodes.append(
|
|
NodeAStar("down", current_node, (x + 1, y), actions))
|
|
|
|
if x > 0: # UP NEIGHBOUR
|
|
if current_node.agent_direction == "right":
|
|
actions = ["rotate_left", "move"]
|
|
elif current_node.agent_direction == "left":
|
|
actions = ["rotate_right", "move"]
|
|
elif current_node.agent_direction == "up":
|
|
actions = ["move"]
|
|
elif current_node.agent_direction == "down":
|
|
actions = ["rotate_right", "rotate_right", "move"]
|
|
neighbour_nodes.append(NodeAStar("up", current_node, (x - 1, y), actions))
|
|
|
|
if y > 0: # LEFT NEIGHBOUR
|
|
if current_node.agent_direction == "right":
|
|
actions = ["rotate_left", "rotate_left", "move"]
|
|
elif current_node.agent_direction == "left":
|
|
actions = ["move"]
|
|
elif current_node.agent_direction == "up":
|
|
actions = ["rotate_left", "move"]
|
|
elif current_node.agent_direction == "down":
|
|
actions = ["rotate_right", "move"]
|
|
neighbour_nodes.append(NodeAStar("left", current_node, (x, y - 1), actions))
|
|
|
|
if y < 8: # RIGHT NEIGHBOUR
|
|
if current_node.agent_direction == "right":
|
|
actions = ["move"]
|
|
elif current_node.agent_direction == "left":
|
|
actions = ["rotate_left", "rotate_left", "move"]
|
|
elif current_node.agent_direction == "up":
|
|
actions = ["rotate_right", "move"]
|
|
elif current_node.agent_direction == "down":
|
|
actions = ["rotate_left", "move"]
|
|
neighbour_nodes.append(NodeAStar("right", current_node, (x, y + 1), actions))
|
|
for neighbour in neighbour_nodes:
|
|
if len([closed_neighbour for closed_neighbour in closed_nodes if closed_neighbour == neighbour]) > 0:
|
|
continue
|
|
|
|
action_len = 0
|
|
if current_node.action is not None:
|
|
action_len = len(current_node.action)
|
|
neighbour.g = current_node.g + WAREHOUSE_MAP[neighbour.position[0]][neighbour.position[1]] + action_len
|
|
neighbour.h = abs(neighbour.position[0] - end_node.position[0]) + abs(
|
|
neighbour.position[1] - end_node.position[1])
|
|
neighbour.f = neighbour.g + neighbour.h
|
|
|
|
if len([open_node for open_node in open_nodes if
|
|
neighbour.position == open_node.position and neighbour.g > open_node.g]) > 0:
|
|
continue
|
|
|
|
open_nodes.append(neighbour)
|
|
|
|
|
|
class NodeAStar:
|
|
def __init__(self, agent_direction, parent=None, position=None, action=None):
|
|
self.agent_direction = agent_direction
|
|
self.parent = parent
|
|
self.position = position
|
|
self.action = action
|
|
self.g = 0
|
|
self.h = 0
|
|
self.f = 0
|
|
|
|
def __eq__(self, other):
|
|
return self.position == other.position
|