diff options
| -rw-r--r-- | pathfinding.py | 25 |
1 files changed, 20 insertions, 5 deletions
diff --git a/pathfinding.py b/pathfinding.py index 97ffc0f..0540698 100644 --- a/pathfinding.py +++ b/pathfinding.py @@ -1,3 +1,4 @@ +import heapq import math from agarnet.agarnet.vec import Vec @@ -106,10 +107,14 @@ class Node: self.parent = None self.H = 0 self.G = 0 + self.F = 0 self.graph = graph self.is_in_wormhole_plane = is_in_wormhole_plane self.near_wormholes = near_wormholes + def __lt__(self, other): + return False + def find_near_wormholes(self, radius): self.near_wormholes = list(filter(lambda blob : (self.point - blob.point).len() < radius, self.graph.blobs)) @@ -132,14 +137,16 @@ def distance(point,point2): def aStar(start, goal): openset = set() + openheap = [] closedset = set() current = start openset.add(current) + openheap.append((0,current)) while openset: - #Find the item in the open set with the lowest G + H score - current = min(openset, key=lambda o:o.G + o.H) + #Find the item in the open set with the lowest F = G + H score + current = heapq.heappop(openheap)[1] #If it is the item we want, retrace the path and return it if current == goal: @@ -149,8 +156,11 @@ def aStar(start, goal): current = current.parent path.append(current) return path[::-1] - - openset.remove(current) + + try: + openset.remove(current) + except KeyError: + pass closedset.add(current) for node in current.siblings(): @@ -158,19 +168,24 @@ def aStar(start, goal): continue if node in openset: - #Check if we beat the G score + #Check if we beat the G score new_g = current.G + current.move_cost(node) if node.G > new_g: #If so, update the node to have a new parent node.G = new_g + node.F = node.G + node.H node.parent = current + heapq.heappush(openheap, (node.F, node)) else: #If it isn't in the open set, calculate the G and H score for the node node.G = current.G + current.move_cost(node) node.H = distance(node, goal) + node.F = node.G + node.H node.parent = current + openset.add(node) + heapq.heappush(openheap, (node.F, node)) raise ValueError('No Path Found') |