a star, robot uwzglednia koszt
This commit is contained in:
parent
5af2f22a64
commit
4d91d86aef
@ -89,11 +89,17 @@ class AI:
|
|||||||
def bfs(self):
|
def bfs(self):
|
||||||
dists = [] # how long way is to all mines from start([0, 0])
|
dists = [] # how long way is to all mines from start([0, 0])
|
||||||
for mine in self.current_map.mines:
|
for mine in self.current_map.mines:
|
||||||
dists.append(sqrt((self.saper.position_x-mine.position_x)**2 + (self.saper.position_y-mine.position_y)**2))
|
# dists.append(sqrt((self.saper.position_x-mine.position_x)**2 + (self.saper.position_y-mine.position_y)**2))
|
||||||
|
dists.append(abs(self.saper.position_x - mine.position_x) + abs(self.saper.position_y - mine.position_y))
|
||||||
print(dists)
|
print(dists)
|
||||||
ind = dists.index(min(dists)) # choose nearest mine
|
ind = dists.index(min(dists)) # choose nearest mine
|
||||||
goal_state = [self.current_map.mines[ind].position_x, self.current_map.mines[ind].position_y]
|
goal_state = [self.current_map.mines[ind].position_x, self.current_map.mines[ind].position_y]
|
||||||
print(f'We go to {goal_state}')
|
print(f'We go to {goal_state}')
|
||||||
|
|
||||||
find_path = bfs.BFS(self.saper, self.window)
|
find_path = bfs.BFS(self.saper, self.window)
|
||||||
self.the_way = find_path.graphsearch([], [], bfs.BFS.successor, goal_state)
|
tmp = find_path.graphsearch([], [], bfs.BFS.successor, goal_state)
|
||||||
|
print(f'tmp = {tmp}')
|
||||||
|
if tmp is None:
|
||||||
|
raise Exception("Error, path does not exist")
|
||||||
|
else:
|
||||||
|
self.the_way = tmp
|
265
classes/bfs.py
265
classes/bfs.py
@ -1,6 +1,5 @@
|
|||||||
import heapq # dla utrzymania fringe
|
import heapq # dla utrzymania fringe
|
||||||
from classes import node, minesweeper, system
|
from classes import node, minesweeper, system
|
||||||
import time
|
|
||||||
|
|
||||||
|
|
||||||
class BFS:
|
class BFS:
|
||||||
@ -15,180 +14,122 @@ class BFS:
|
|||||||
def successor(self, current_position):
|
def successor(self, current_position):
|
||||||
new_nodes = []
|
new_nodes = []
|
||||||
neighbours_list = self.agent.sensor(current_position[0], current_position[1])
|
neighbours_list = self.agent.sensor(current_position[0], current_position[1])
|
||||||
#print(f'Current position {current_position}')
|
|
||||||
#print(neighbours_list[0])
|
|
||||||
#print(neighbours_list[1])
|
|
||||||
#print(neighbours_list[2])
|
|
||||||
#print(neighbours_list)
|
|
||||||
|
|
||||||
|
|
||||||
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# new_nodes.append([current_position[0] - 1, current_position[1]])
|
|
||||||
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# new_nodes.append([current_position[0], current_position[1] + 1])
|
|
||||||
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# new_nodes.append([current_position[0] + 1, current_position[1]])
|
|
||||||
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# new_nodes.append([current_position[0],current_position[1] - 1])
|
|
||||||
|
|
||||||
# if current_position[2] == 0: # jesli patrzy na polnoc
|
|
||||||
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('forward', [current_position[0] - 1, current_position[1], 0])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('left', [current_position[0],current_position[1], 270])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('right', [current_position[0], current_position[1], 90])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
#
|
|
||||||
# if current_position[2] == 90: # jesli patrzy na wschod
|
|
||||||
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('forward', [current_position[0], current_position[1] + 1, 90])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('left', [current_position[0], current_position[1], 0])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('right', [current_position[0], current_position[1], 180])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
#
|
|
||||||
# if current_position[2] == 180: # jesli patczy na poludzie
|
|
||||||
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('forward', [current_position[0] + 1, current_position[1], 180])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('left', [current_position[0], current_position[1], 90])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('right', [current_position[0],current_position[1], 270])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
#
|
|
||||||
# if current_position[2] == 270: # jesli patczy na wschod
|
|
||||||
# if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('forward', [current_position[0],current_position[1] - 1, 270])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('left', [current_position[0], current_position[1], 180])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
# if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
|
||||||
# tmp = ('right', [current_position[0], current_position[1], 0])
|
|
||||||
# new_nodes.append(tmp)
|
|
||||||
|
|
||||||
if current_position[2] == 180: # jesli patrzy na polnoc
|
if current_position[2] == 180: # jesli patrzy na polnoc
|
||||||
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[0][1] == 'grass':
|
if neighbours_list[0][1] == 'grass':
|
||||||
cost = 5
|
cost = 10
|
||||||
elif neighbours_list[0][1] == 'sand':
|
elif neighbours_list[0][1] == 'sand':
|
||||||
cost = 1
|
cost = 2
|
||||||
elif neighbours_list[0][1] == 'mine':
|
elif neighbours_list[0][1] == 'mine':
|
||||||
cost = 0
|
cost = 0
|
||||||
tmp = ('forward', [current_position[0], current_position[1] - 1, 180], cost)
|
tmp = ('forward', [current_position[0], current_position[1] - 1, 180], cost)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][0] == 'grass':
|
# if neighbours_list[1][0] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[1][0] == 'sand':
|
# elif neighbours_list[1][0] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[1][0] == 'mine':
|
# elif neighbours_list[1][0] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('left', [current_position[0],current_position[1], 270], cost)
|
tmp = ('left', [current_position[0],current_position[1], 270], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][2] == 'grass':
|
# if neighbours_list[1][2] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[1][2] == 'sand':
|
# elif neighbours_list[1][2] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[1][2] == 'mine':
|
# elif neighbours_list[1][2] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('right', [current_position[0], current_position[1], 90], cost)
|
tmp = ('right', [current_position[0], current_position[1], 90], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
|
|
||||||
if current_position[2] == 90: # jesli patrzy na wschod
|
if current_position[2] == 90: # jesli patrzy na wschod
|
||||||
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][2] == 'grass':
|
if neighbours_list[1][2] == 'grass':
|
||||||
cost = 5
|
cost = 10
|
||||||
elif neighbours_list[1][2] == 'sand':
|
elif neighbours_list[1][2] == 'sand':
|
||||||
cost = 1
|
cost = 2
|
||||||
elif neighbours_list[1][2] == 'mine':
|
elif neighbours_list[1][2] == 'mine':
|
||||||
cost = 0
|
cost = 0
|
||||||
tmp = ('forward', [current_position[0] + 1, current_position[1], 90], cost)
|
tmp = ('forward', [current_position[0] + 1, current_position[1], 90], cost)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[0][1] == 'grass':
|
# if neighbours_list[0][1] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[0][1] == 'sand':
|
# elif neighbours_list[0][1] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[0][1] == 'mine':
|
# elif neighbours_list[0][1] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('left', [current_position[0], current_position[1], 180], cost)
|
tmp = ('left', [current_position[0], current_position[1], 180], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[2][1] == 'grass':
|
# if neighbours_list[2][1] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[2][1] == 'sand':
|
# elif neighbours_list[2][1] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[2][1] == 'mine':
|
# elif neighbours_list[2][1] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('right', [current_position[0], current_position[1], 0], cost)
|
tmp = ('right', [current_position[0], current_position[1], 0], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
|
|
||||||
if current_position[2] == 0: # jesli patczy na poludzie
|
if current_position[2] == 0: # jesli patczy na poludzie
|
||||||
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[2][1] == 'grass':
|
if neighbours_list[2][1] == 'grass':
|
||||||
cost = 5
|
cost = 10
|
||||||
elif neighbours_list[2][1] == 'sand':
|
elif neighbours_list[2][1] == 'sand':
|
||||||
cost = 1
|
cost = 2
|
||||||
elif neighbours_list[2][1] == 'mine':
|
elif neighbours_list[2][1] == 'mine':
|
||||||
cost = 0
|
cost = 0
|
||||||
tmp = ('forward', [current_position[0], current_position[1] + 1, 0], cost)
|
tmp = ('forward', [current_position[0], current_position[1] + 1, 0], cost)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][2] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][2] == 'grass':
|
# if neighbours_list[1][2] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[1][2] == 'sand':
|
# elif neighbours_list[1][2] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[1][2] == 'mine':
|
# elif neighbours_list[1][2] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('left', [current_position[0], current_position[1], 90], cost)
|
tmp = ('left', [current_position[0], current_position[1], 90], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][0] == 'grass':
|
# if neighbours_list[1][0] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[1][0] == 'sand':
|
# elif neighbours_list[1][0] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[1][0] == 'mine':
|
# elif neighbours_list[1][0] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('right', [current_position[0],current_position[1], 270], cost)
|
tmp = ('right', [current_position[0],current_position[1], 270], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
|
|
||||||
if current_position[2] == 270: # jesli patczy na wschod
|
if current_position[2] == 270: # jesli patczy na wschod
|
||||||
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[1][0] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[1][0] == 'grass':
|
if neighbours_list[1][0] == 'grass':
|
||||||
cost = 5
|
cost = 10
|
||||||
elif neighbours_list[1][0] == 'sand':
|
elif neighbours_list[1][0] == 'sand':
|
||||||
cost = 1
|
cost = 2
|
||||||
elif neighbours_list[1][0] == 'mine':
|
elif neighbours_list[1][0] == 'mine':
|
||||||
cost = 0
|
cost = 0
|
||||||
tmp = ('forward', [current_position[0] - 1,current_position[1], 270], cost)
|
tmp = ('forward', [current_position[0] - 1,current_position[1], 270], cost)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[2][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[2][1] == 'grass':
|
# if neighbours_list[2][1] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[2][1] == 'sand':
|
# elif neighbours_list[2][1] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[2][1] == 'mine':
|
# elif neighbours_list[2][1] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('left', [current_position[0], current_position[1], 0], cost)
|
tmp = ('left', [current_position[0], current_position[1], 0], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
if neighbours_list[0][1] not in ['wall', 'cliff_south', 'cliff_east', 'cliff_north', 'cliff_west']:
|
||||||
if neighbours_list[0][1] == 'grass':
|
# if neighbours_list[0][1] == 'grass':
|
||||||
cost = 5
|
# cost = 10
|
||||||
elif neighbours_list[0][1] == 'sand':
|
# elif neighbours_list[0][1] == 'sand':
|
||||||
cost = 1
|
# cost = 2
|
||||||
elif neighbours_list[0][1] == 'mine':
|
# elif neighbours_list[0][1] == 'mine':
|
||||||
cost = 0
|
# cost = 0
|
||||||
tmp = ('right', [current_position[0], current_position[1], 180], cost)
|
tmp = ('right', [current_position[0], current_position[1], 180], 1)
|
||||||
new_nodes.append(tmp)
|
new_nodes.append(tmp)
|
||||||
|
|
||||||
return new_nodes
|
return new_nodes
|
||||||
@ -208,29 +149,25 @@ class BFS:
|
|||||||
|
|
||||||
self.window.pause(True)
|
self.window.pause(True)
|
||||||
|
|
||||||
positiont_at_beginning = [self.agent.position_x, self.agent.position_y, self.agent.rotation_degrees] # x, y, gdzie_patczy
|
position_at_beginning = [self.agent.position_x, self.agent.position_y, self.agent.rotation_degrees] # x, y, gdzie_patczy
|
||||||
print(f'Position {positiont_at_beginning}')
|
|
||||||
final_action_list = [] # lista co ma robic zeby dojechac do miny
|
final_action_list = [] # lista co ma robic zeby dojechac do miny
|
||||||
root = node.Node(None, None, positiont_at_beginning, 0) # parent, action, position
|
root = node.Node(None, None, position_at_beginning, 0) # parent, action, position, cost
|
||||||
counter = 0
|
|
||||||
heapq.heappush(fringe, (counter, root))
|
heapq.heappush(fringe, (0, root)) # add first node to fringe
|
||||||
#visited_position = []
|
|
||||||
|
|
||||||
|
|
||||||
while len(fringe) != 0:
|
while len(fringe) != 0: # poki sa wezly do odwiedzenia(na fringe)
|
||||||
flag = True
|
|
||||||
|
|
||||||
|
|
||||||
if len(fringe) == 0:
|
if len(fringe) == 0:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
tmp_node = heapq.heappop(fringe) # node
|
# get node from fringe
|
||||||
|
tmp_node = heapq.heappop(fringe) # tuple(priority, node)
|
||||||
tmp_node_position = tmp_node[1].get_position() # x, y , gdzie patczy
|
tmp_node_position = tmp_node[1].get_position() # x, y , gdzie patczy
|
||||||
#print(f'Position before succ {tmp_node_position}')
|
|
||||||
|
|
||||||
#visited_position.append(tmp_node_position)
|
|
||||||
explored.append(tmp_node_position)
|
|
||||||
|
|
||||||
|
# jesli tmp node to goaltest
|
||||||
if tmp_node_position[:2] == goaltest:
|
if tmp_node_position[:2] == goaltest:
|
||||||
print('Find')
|
print('Find')
|
||||||
|
|
||||||
@ -238,28 +175,60 @@ class BFS:
|
|||||||
final_action_list.append(tmp_node[1].get_action())
|
final_action_list.append(tmp_node[1].get_action())
|
||||||
tmp_node = tmp_node[1].get_parent()
|
tmp_node = tmp_node[1].get_parent()
|
||||||
final_action_list.reverse()
|
final_action_list.reverse()
|
||||||
print(final_action_list)
|
#print(final_action_list)
|
||||||
self.window.pause(False)
|
self.window.pause(False)
|
||||||
return final_action_list
|
return final_action_list
|
||||||
|
|
||||||
# nie wiem na razie po co to
|
explored.append(tmp_node[1]) # add node to array of visited nodes
|
||||||
# chyba visited_position lepszy odpowiednik
|
|
||||||
# explored.append(tmp_node)
|
|
||||||
|
|
||||||
|
|
||||||
neighbours_list_of_our_node = self.successor(tmp_node_position) # lista możliwych akcij
|
neighbours_list_of_our_node = self.successor(tmp_node_position) # lista możliwych akcij
|
||||||
#print(neighbours_list_of_our_node)
|
|
||||||
|
|
||||||
for node_ in neighbours_list_of_our_node:
|
for node_ in neighbours_list_of_our_node:
|
||||||
# jesli pozucja wezla nie jest w fringe i nie jest w visited
|
|
||||||
if [node_[1][0], node_[1][1], node_[1][2]] not in explored and node_[1][0] >= 0 and node_[1][1] >=0:
|
|
||||||
counter += 1
|
|
||||||
x = node.Node(tmp_node, node_[0], node_[1], node_[2]) # action
|
|
||||||
heapq.heappush(fringe, (counter, x))
|
|
||||||
self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]], self.agent.current_map.tile_size, self.agent.current_map, self.agent)
|
|
||||||
|
|
||||||
p = manhattan(node_[1], goaltest) + node_[2]
|
notInFringe = True # false if node in fringe
|
||||||
# print(f'Mancgeten = {p}')
|
notInExplored = True # false if node in explored
|
||||||
|
|
||||||
|
# if node_[1] is None:
|
||||||
|
# continue
|
||||||
|
|
||||||
|
p = manhattan(node_[1], goaltest) + node_[2]
|
||||||
|
# wyznacza jaki jest priorytet nastempnika
|
||||||
|
# manchaten from node_ + cost of way from tmp_ode to node_
|
||||||
|
|
||||||
|
priority_in_fringe = 0
|
||||||
|
counter = 0
|
||||||
|
tmp_position_in_fringe = 0 # zero if node not in fringe
|
||||||
|
|
||||||
|
for fringeNode in fringe: # isc po wszystkich wezlach ktore juz sa w fringe
|
||||||
|
# jesli nasz wezel juz jest w fringe
|
||||||
|
if fringeNode[1].get_position()[0] == node_[1][0] and fringeNode[1].get_position()[1] == node_[1][1] and fringeNode[1].get_position()[2] == node_[1][2]:
|
||||||
|
notInFringe = False
|
||||||
|
priority_in_fringe = fringeNode[0]
|
||||||
|
# number of element in fringe
|
||||||
|
tmp_position_in_fringe = counter
|
||||||
|
counter = counter + 1
|
||||||
|
|
||||||
|
|
||||||
|
for exploredNode in explored: # isc po wszystkich wezlach z listy explored
|
||||||
|
# jesli nasz wezel juz jest w explored
|
||||||
|
if exploredNode.get_position()[0] == node_[1][0] and exploredNode.get_position()[1] == node_[1][1] and exploredNode.get_position()[2] == node_[1][2]:
|
||||||
|
notInExplored = False
|
||||||
|
|
||||||
|
|
||||||
|
# if node not in fringe and not in explored
|
||||||
|
if notInFringe and notInExplored:
|
||||||
|
x = node.Node(tmp_node, node_[0], node_[1], node_[2]) # parent, action, state_array, cost
|
||||||
|
heapq.heappush(fringe, (p, x))
|
||||||
|
# if node not in fringe
|
||||||
|
elif notInFringe is False and (priority_in_fringe > p):
|
||||||
|
x = node.Node(tmp_node, node_[0], node_[1], node_[2]) # parent, action, state_array, cost
|
||||||
|
tmp = list(fringe[tmp_position_in_fringe])
|
||||||
|
tmp[0] = p
|
||||||
|
tmp[1] = x
|
||||||
|
fringe[tmp_position_in_fringe] = tuple(tmp)
|
||||||
|
#self.window.draw_search([self.agent.position_x, self.agent.position_y], [node_[1][0], node_[1][1]],self.agent.current_map.tile_size, self.agent.current_map, self.agent)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -113,7 +113,7 @@ class Map:
|
|||||||
if ok and rng==0 and not (i<2 and j<3):
|
if ok and rng==0 and not (i<2 and j<3):
|
||||||
|
|
||||||
matrix[i].append(10) #kamień
|
matrix[i].append(10) #kamień
|
||||||
elif ok and rng>8 and not (i<2 and j<3):
|
elif ok and rng>4 and not (i<2 and j<3):
|
||||||
matrix[i].append(5) #trawa
|
matrix[i].append(5) #trawa
|
||||||
elif ok and rng<2 and not (i<2 and j<3):
|
elif ok and rng<2 and not (i<2 and j<3):
|
||||||
matrix[i].append(0) #piasek
|
matrix[i].append(0) #piasek
|
||||||
|
@ -20,3 +20,6 @@ class Node:
|
|||||||
def get_cost(self):
|
def get_cost(self):
|
||||||
return self.cost
|
return self.cost
|
||||||
|
|
||||||
|
def __lt__(self, other):
|
||||||
|
return self.cost < other.get_cost()
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user