From 243712510054f2d2f28bb6d905a45e7d1ac05bff Mon Sep 17 00:00:00 2001 From: mikoMaz Date: Mon, 8 May 2023 16:23:21 +0200 Subject: [PATCH] add heuristic; fix cost method --- astar_search.py | 53 ++++++++++++++++++++++++++++++++++++------------- main.py | 2 +- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/astar_search.py b/astar_search.py index fac3a294..018654c2 100644 --- a/astar_search.py +++ b/astar_search.py @@ -1,8 +1,9 @@ class Node: - def __init__(self, state, parent='', action=''): + def __init__(self, state, parent='', action='', distance=0): self.state = state self.parent = parent self.action = action + self.distance = distance class Search: def __init__(self, cell_size, cell_number): @@ -30,11 +31,27 @@ class Search: possible = [['left', x, y, 'DOWN'], ['right', x, y, 'UP']] if x != 0: possible.append(['move', x - self.cell_size, y, 'LEFT']) return possible + + def cost(self, node, stones, goal, flowers): + # cost = node.distance + cost = 0 + # cost += 10 if stones[node.state[0], node.state[1]] == 1 else 1 + cost += 1000000 if (node.state[0], node.state[1]) in stones else 1 + cost += 30 if (node.state[0], node.state[1]) in flowers else 1 + + if node.parent: + node = node.parent + cost += node.distance # should return only elem.action in prod + return cost + + def heuristic(self, node, goal): + return abs(node.state[0] - goal[0]) + abs(node.state[1] - goal[1]) + #bandaid to know about stones - def astarsearch(self, istate, goaltest, stones): + def astarsearch(self, istate, goaltest, stones, flowers): #to be expanded - def cost(x, y): + def cost_old(x, y): if (x, y) in stones: return 10 else: @@ -45,7 +62,12 @@ class Search: y = istate[1] angle = istate[2] - fringe = [(Node([x, y, angle]), cost(x, y))] # queue (moves/states to check) + # fringe = [(Node([x, y, angle]), cost_old(x, y))] # queue (moves/states to check) + fringe = [(Node([x, y, angle]))] # queue (moves/states to check) + fringe[0].distance = self.cost(fringe[0], stones, goaltest, flowers) + fringe.append((Node([x, y, angle]), self.cost(fringe[0], stones, goaltest, flowers))) + fringe.pop(0) + explored = [] while True: @@ -71,15 +93,18 @@ class Search: explored.append(elem.state) for (action, state_x, state_y, state_angle) in self.succ(elem.state): - x = Node([state_x, state_y, state_angle], elem, action) + x = Node([state_x, state_y, state_angle], elem, action) + x.parent = elem - priority = cost(state_x, state_y) - fringe_states = [node.state for (node, p) in fringe] + priority = self.cost(elem, stones, goaltest, flowers) + self.heuristic(elem, goaltest) + elem.distance = priority + # priority = cost_old(x, y) + self.heuristic(elem, goaltest) + fringe_states = [node.state for (node, p) in fringe] - if x.state not in fringe_states and x.state not in explored: - fringe.append((x, priority)) - elif x.state in fringe_states: - for i in range(len(fringe)): - if fringe[i][0].state == x.state: - if fringe[i][1] > priority: - fringe[i] = (x, priority) \ No newline at end of file + if x.state not in fringe_states and x.state not in explored: + fringe.append((x, priority)) + elif x.state in fringe_states: + for i in range(len(fringe)): + if fringe[i][0].state == x.state: + if fringe[i][1] > priority: + fringe[i] = (x, priority) diff --git a/main.py b/main.py index 5a3aba1a..2e34e0b1 100644 --- a/main.py +++ b/main.py @@ -102,7 +102,7 @@ class Game: angles = {0: 'UP', 90: 'RIGHT', 270: 'LEFT', 180: 'DOWN'} #bandaid to know about stones tractor_next_moves = astar_search_object.astarsearch( - [self.tractor.x, self.tractor.y, angles[self.tractor.angle]], [random_x, random_y], self.blocks.stones) + [self.tractor.x, self.tractor.y, angles[self.tractor.angle]], [random_x, random_y], self.blocks.stones, self.flower_body) else: self.tractor.move(tractor_next_moves.pop(0)[0], self.cell_size, self.cell_number) elif event.type == QUIT: